zrzk
/
px4_autopilot
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity
Browse Source

Merge pull request #319 from PX4/pr-attitude_fw-cleanup 

attitude_fw delete unused and cleanup
master
Paul Riseborough 8 years ago committed by GitHub
parent
fac292fef9 25bd3ac5e6
commit
1531269f92
12 changed files with 96 additions and 251 deletions
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Unified View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 1
      CMakeLists.txt
  2. 2
      EKF/geo.cpp
  3. 21
      attitude_fw/ecl_controller.cpp
  4. 38
      attitude_fw/ecl_controller.h
  5. 50
      attitude_fw/ecl_pitch_controller.cpp
  6. 22
      attitude_fw/ecl_pitch_controller.h
  7. 37
      attitude_fw/ecl_roll_controller.cpp
  8. 14
      attitude_fw/ecl_roll_controller.h
  9. 30
      attitude_fw/ecl_wheel_controller.cpp
  10. 16
      attitude_fw/ecl_wheel_controller.h
  11. 75
      attitude_fw/ecl_yaw_controller.cpp
  12. 41
      attitude_fw/ecl_yaw_controller.h

1
CMakeLists.txt
Unescape View File

@ -61,4 +61,3 @@ px4_add_module(
DEPENDS DEPENDS
platforms__common platforms__common
) )
# vim: set noet ft=cmake fenc=utf-8 ff=unix :

2
EKF/geo.cpp
Unescape View File

@ -826,4 +826,4 @@ float _wrap_360(float bearing)
return bearing; return bearing;
} }
#endif //POSIX_SHARED #endif //POSIX_SHARED

21
attitude_fw/ecl_controller.cpp
Unescape View File

@ -48,25 +48,6 @@
#include "ecl_controller.h" #include "ecl_controller.h"
#include <stdio.h>
#include <mathlib/mathlib.h>
ECL_Controller::ECL_Controller(const char *name) :
_last_run(0),
_tc(0.1f),
_k_p(0.0f),
_k_i(0.0f),
_k_ff(0.0f),
_integrator_max(0.0f),
_max_rate(0.0f),
_last_output(0.0f),
_integrator(0.0f),
_rate_error(0.0f),
_rate_setpoint(0.0f),
_bodyrate_setpoint(0.0f)
{
}
void ECL_Controller::reset_integrator() void ECL_Controller::reset_integrator()
{ {
_integrator = 0.0f; _integrator = 0.0f;
@ -123,7 +104,7 @@ float ECL_Controller::constrain_airspeed(float airspeed, float minspeed, float m
{ {
float airspeed_result = airspeed; float airspeed_result = airspeed;
if (!PX4_ISFINITE(airspeed)) { if (!ISFINITE(airspeed)) {
/* airspeed is NaN, +- INF or not available, pick center of band */ /* airspeed is NaN, +- INF or not available, pick center of band */
airspeed_result = 0.5f * (minspeed + maxspeed); airspeed_result = 0.5f * (minspeed + maxspeed);

38
attitude_fw/ecl_controller.h
Unescape View File

@ -48,9 +48,8 @@
#pragma once #pragma once
#include <stdbool.h> #include "ecl/ecl.h"
#include <stdint.h> #include "mathlib/mathlib.h"
#include <systemlib/perf_counter.h>
struct ECL_ControlData { struct ECL_ControlData {
float roll; float roll;
@ -77,7 +76,7 @@ struct ECL_ControlData {
class __EXPORT ECL_Controller class __EXPORT ECL_Controller
{ {
public: public:
ECL_Controller(const char *name); ECL_Controller() = default;
~ECL_Controller() = default; ~ECL_Controller() = default;
virtual float control_attitude(const struct ECL_ControlData &ctl_data) = 0; virtual float control_attitude(const struct ECL_ControlData &ctl_data) = 0;
@ -91,7 +90,7 @@ public:
void set_k_ff(float k_ff); void set_k_ff(float k_ff);
void set_integrator_max(float max); void set_integrator_max(float max);
void set_max_rate(float max_rate); void set_max_rate(float max_rate);
void set_bodyrate_setpoint(float rate) {_bodyrate_setpoint = rate;} void set_bodyrate_setpoint(float rate) { _bodyrate_setpoint = rate; }
/* Getters */ /* Getters */
float get_rate_error(); float get_rate_error();
@ -101,17 +100,22 @@ public:
void reset_integrator(); void reset_integrator();
protected: protected:
uint64_t _last_run; uint64_t _last_run{0};
float _tc;
float _k_p; float _tc{0.5f};
float _k_i; float _k_p{0.0f};
float _k_ff; float _k_i{0.0f};
float _integrator_max; float _k_ff{0.0f};
float _max_rate;
float _last_output; float _integrator_max{0.0f};
float _integrator; float _integrator{0.0f};
float _rate_error;
float _rate_setpoint; float _rate_setpoint{0.0f};
float _bodyrate_setpoint; float _bodyrate_setpoint{0.0f};
float _max_rate{0.0f};
float _rate_error{0.0f};
float _last_output{0.0f};
float constrain_airspeed(float airspeed, float minspeed, float maxspeed); float constrain_airspeed(float airspeed, float minspeed, float maxspeed);
}; };

50
attitude_fw/ecl_pitch_controller.cpp
Unescape View File

@ -35,34 +35,20 @@
* @file ecl_pitch_controller.cpp * @file ecl_pitch_controller.cpp
* Implementation of a simple orthogonal pitch PID controller. * Implementation of a simple orthogonal pitch PID controller.
* *
* Authors and acknowledgements in header. * Authors and acknowledgments in header.
*/ */
#include "ecl_pitch_controller.h" #include "ecl_pitch_controller.h"
#include <math.h>
#include <stdint.h>
#include <float.h>
#include <geo/geo.h>
#include <ecl/ecl.h>
#include <mathlib/mathlib.h>
#include <systemlib/err.h>
ECL_PitchController::ECL_PitchController() :
ECL_Controller("pitch"),
_max_rate_neg(0.0f),
_roll_ff(0.0f)
{
}
float ECL_PitchController::control_attitude(const struct ECL_ControlData &ctl_data) float ECL_PitchController::control_attitude(const struct ECL_ControlData &ctl_data)
{ {
/* Do not calculate control signal with bad inputs */ /* Do not calculate control signal with bad inputs */
if (!(PX4_ISFINITE(ctl_data.pitch_setpoint) && if (!(ISFINITE(ctl_data.pitch_setpoint) &&
PX4_ISFINITE(ctl_data.roll) && ISFINITE(ctl_data.roll) &&
PX4_ISFINITE(ctl_data.pitch) && ISFINITE(ctl_data.pitch) &&
PX4_ISFINITE(ctl_data.airspeed))) { ISFINITE(ctl_data.airspeed))) {
warnx("not controlling pitch");
ECL_WARN("not controlling pitch");
return _rate_setpoint; return _rate_setpoint;
} }
@ -73,14 +59,13 @@ float ECL_PitchController::control_attitude(const struct ECL_ControlData &ctl_da
_rate_setpoint = pitch_error / _tc; _rate_setpoint = pitch_error / _tc;
/* limit the rate */ /* limit the rate */
if (_max_rate > 0.01f && _max_rate_neg > 0.01f) { if (_max_rate >= 0.0f && _max_rate_neg >= 0.0f) {
if (_rate_setpoint > 0.0f) { if (_rate_setpoint > 0.0f) {
_rate_setpoint = (_rate_setpoint > _max_rate) ? _max_rate : _rate_setpoint; _rate_setpoint = (_rate_setpoint > _max_rate) ? _max_rate : _rate_setpoint;
} else { } else {
_rate_setpoint = (_rate_setpoint < -_max_rate_neg) ? -_max_rate_neg : _rate_setpoint; _rate_setpoint = (_rate_setpoint < -_max_rate_neg) ? -_max_rate_neg : _rate_setpoint;
} }
} }
return _rate_setpoint; return _rate_setpoint;
@ -89,21 +74,22 @@ float ECL_PitchController::control_attitude(const struct ECL_ControlData &ctl_da
float ECL_PitchController::control_bodyrate(const struct ECL_ControlData &ctl_data) float ECL_PitchController::control_bodyrate(const struct ECL_ControlData &ctl_data)
{ {
/* Do not calculate control signal with bad inputs */ /* Do not calculate control signal with bad inputs */
if (!(PX4_ISFINITE(ctl_data.roll) && if (!(ISFINITE(ctl_data.roll) &&
PX4_ISFINITE(ctl_data.pitch) && ISFINITE(ctl_data.pitch) &&
PX4_ISFINITE(ctl_data.body_y_rate) && ISFINITE(ctl_data.body_y_rate) &&
PX4_ISFINITE(ctl_data.body_z_rate) && ISFINITE(ctl_data.body_z_rate) &&
PX4_ISFINITE(ctl_data.yaw_rate_setpoint) && ISFINITE(ctl_data.yaw_rate_setpoint) &&
PX4_ISFINITE(ctl_data.airspeed_min) && ISFINITE(ctl_data.airspeed_min) &&
PX4_ISFINITE(ctl_data.airspeed_max) && ISFINITE(ctl_data.airspeed_max) &&
PX4_ISFINITE(ctl_data.scaler))) { ISFINITE(ctl_data.scaler))) {
return math::constrain(_last_output, -1.0f, 1.0f); return math::constrain(_last_output, -1.0f, 1.0f);
} }
/* get the usual dt estimate */ /* get the usual dt estimate */
uint64_t dt_micros = ecl_elapsed_time(&_last_run); uint64_t dt_micros = ecl_elapsed_time(&_last_run);
_last_run = ecl_absolute_time(); _last_run = ecl_absolute_time();
float dt = (float)dt_micros * 1e-6f; float dt = dt_micros * 1e-6f;
/* lock integral for long intervals */ /* lock integral for long intervals */
bool lock_integrator = ctl_data.lock_integrator; bool lock_integrator = ctl_data.lock_integrator;

22
attitude_fw/ecl_pitch_controller.h
Unescape View File

@ -49,21 +49,17 @@
#ifndef ECL_PITCH_CONTROLLER_H #ifndef ECL_PITCH_CONTROLLER_H
#define ECL_PITCH_CONTROLLER_H #define ECL_PITCH_CONTROLLER_H
#include <stdbool.h>
#include <stdint.h>
#include "ecl_controller.h" #include "ecl_controller.h"
class __EXPORT ECL_PitchController : class __EXPORT ECL_PitchController final : public ECL_Controller
public ECL_Controller
{ {
public: public:
ECL_PitchController(); ECL_PitchController() = default;
~ECL_PitchController() = default; ~ECL_PitchController() = default;
float control_attitude(const struct ECL_ControlData &ctl_data); float control_attitude(const struct ECL_ControlData &ctl_data) override;
float control_euler_rate(const struct ECL_ControlData &ctl_data); float control_euler_rate(const struct ECL_ControlData &ctl_data) override;
float control_bodyrate(const struct ECL_ControlData &ctl_data); float control_bodyrate(const struct ECL_ControlData &ctl_data) override;
/* Additional Setters */ /* Additional Setters */
void set_max_rate_pos(float max_rate_pos) void set_max_rate_pos(float max_rate_pos)
@ -76,14 +72,8 @@ public:
_max_rate_neg = max_rate_neg; _max_rate_neg = max_rate_neg;
} }
void set_roll_ff(float roll_ff)
{
_roll_ff = roll_ff;
}
protected: protected:
float _max_rate_neg; float _max_rate_neg{0.0f};
float _roll_ff;
}; };
#endif // ECL_PITCH_CONTROLLER_H #endif // ECL_PITCH_CONTROLLER_H

37
attitude_fw/ecl_roll_controller.cpp
Unescape View File

@ -35,27 +35,15 @@
* @file ecl_roll_controller.cpp * @file ecl_roll_controller.cpp
* Implementation of a simple orthogonal roll PID controller. * Implementation of a simple orthogonal roll PID controller.
* *
* Authors and acknowledgements in header. * Authors and acknowledgments in header.
*/ */
#include <ecl/ecl.h>
#include "ecl_roll_controller.h" #include "ecl_roll_controller.h"
#include <stdint.h>
#include <float.h>
#include <geo/geo.h>
#include <ecl/ecl.h>
#include <mathlib/mathlib.h>
#include <systemlib/err.h>
ECL_RollController::ECL_RollController() :
ECL_Controller("roll")
{
}
float ECL_RollController::control_attitude(const struct ECL_ControlData &ctl_data) float ECL_RollController::control_attitude(const struct ECL_ControlData &ctl_data)
{ {
/* Do not calculate control signal with bad inputs */ /* Do not calculate control signal with bad inputs */
if (!(PX4_ISFINITE(ctl_data.roll_setpoint) && PX4_ISFINITE(ctl_data.roll))) { if (!(ISFINITE(ctl_data.roll_setpoint) && ISFINITE(ctl_data.roll))) {
return _rate_setpoint; return _rate_setpoint;
} }
@ -67,7 +55,7 @@ float ECL_RollController::control_attitude(const struct ECL_ControlData &ctl_dat
/* limit the rate */ //XXX: move to body angluar rates /* limit the rate */ //XXX: move to body angluar rates
if (_max_rate > 0.01f) { if (_max_rate >= 0.0f) {
_rate_setpoint = (_rate_setpoint > _max_rate) ? _max_rate : _rate_setpoint; _rate_setpoint = (_rate_setpoint > _max_rate) ? _max_rate : _rate_setpoint;
_rate_setpoint = (_rate_setpoint < -_max_rate) ? -_max_rate : _rate_setpoint; _rate_setpoint = (_rate_setpoint < -_max_rate) ? -_max_rate : _rate_setpoint;
} }
@ -78,20 +66,21 @@ float ECL_RollController::control_attitude(const struct ECL_ControlData &ctl_dat
float ECL_RollController::control_bodyrate(const struct ECL_ControlData &ctl_data) float ECL_RollController::control_bodyrate(const struct ECL_ControlData &ctl_data)
{ {
/* Do not calculate control signal with bad inputs */ /* Do not calculate control signal with bad inputs */
if (!(PX4_ISFINITE(ctl_data.pitch) && if (!(ISFINITE(ctl_data.pitch) &&
PX4_ISFINITE(ctl_data.body_x_rate) && ISFINITE(ctl_data.body_x_rate) &&
PX4_ISFINITE(ctl_data.body_z_rate) && ISFINITE(ctl_data.body_z_rate) &&
PX4_ISFINITE(ctl_data.yaw_rate_setpoint) && ISFINITE(ctl_data.yaw_rate_setpoint) &&
PX4_ISFINITE(ctl_data.airspeed_min) && ISFINITE(ctl_data.airspeed_min) &&
PX4_ISFINITE(ctl_data.airspeed_max) && ISFINITE(ctl_data.airspeed_max) &&
PX4_ISFINITE(ctl_data.scaler))) { ISFINITE(ctl_data.scaler))) {
return math::constrain(_last_output, -1.0f, 1.0f); return math::constrain(_last_output, -1.0f, 1.0f);
} }
/* get the usual dt estimate */ /* get the usual dt estimate */
uint64_t dt_micros = ecl_elapsed_time(&_last_run); uint64_t dt_micros = ecl_elapsed_time(&_last_run);
_last_run = ecl_absolute_time(); _last_run = ecl_absolute_time();
float dt = (float)dt_micros * 1e-6f; float dt = dt_micros * 1e-6f;
/* lock integral for long intervals */ /* lock integral for long intervals */
bool lock_integrator = ctl_data.lock_integrator; bool lock_integrator = ctl_data.lock_integrator;
@ -140,6 +129,4 @@ float ECL_RollController::control_euler_rate(const struct ECL_ControlData &ctl_d
_bodyrate_setpoint = ctl_data.roll_rate_setpoint - sinf(ctl_data.pitch) * ctl_data.yaw_rate_setpoint; _bodyrate_setpoint = ctl_data.roll_rate_setpoint - sinf(ctl_data.pitch) * ctl_data.yaw_rate_setpoint;
return control_bodyrate(ctl_data); return control_bodyrate(ctl_data);
} }

14
attitude_fw/ecl_roll_controller.h
Unescape View File

@ -49,21 +49,17 @@
#ifndef ECL_ROLL_CONTROLLER_H #ifndef ECL_ROLL_CONTROLLER_H
#define ECL_ROLL_CONTROLLER_H #define ECL_ROLL_CONTROLLER_H
#include <stdbool.h>
#include <stdint.h>
#include "ecl_controller.h" #include "ecl_controller.h"
class __EXPORT ECL_RollController : class __EXPORT ECL_RollController final : public ECL_Controller
public ECL_Controller
{ {
public: public:
ECL_RollController(); ECL_RollController() = default;
~ECL_RollController() = default; ~ECL_RollController() = default;
float control_attitude(const struct ECL_ControlData &ctl_data); float control_attitude(const struct ECL_ControlData &ctl_data) override;
float control_euler_rate(const struct ECL_ControlData &ctl_data); float control_euler_rate(const struct ECL_ControlData &ctl_data) override;
float control_bodyrate(const struct ECL_ControlData &ctl_data); float control_bodyrate(const struct ECL_ControlData &ctl_data) override;
}; };
#endif // ECL_ROLL_CONTROLLER_H #endif // ECL_ROLL_CONTROLLER_H

30
attitude_fw/ecl_wheel_controller.cpp
Unescape View File

@ -35,36 +35,26 @@
* @file ecl_wheel_controller.cpp * @file ecl_wheel_controller.cpp
* Implementation of a simple PID wheel controller for heading tracking. * Implementation of a simple PID wheel controller for heading tracking.
* *
* Authors and acknowledgements in header. * Authors and acknowledgments in header.
*/ */
#include "ecl_wheel_controller.h" #include "ecl_wheel_controller.h"
#include <stdint.h>
#include <float.h>
#include <geo/geo.h>
#include <ecl/ecl.h>
#include <mathlib/mathlib.h>
#include <systemlib/err.h>
#include <ecl/ecl.h>
ECL_WheelController::ECL_WheelController() : #include <geo/geo.h>
ECL_Controller("wheel")
{
}
float ECL_WheelController::control_bodyrate(const struct ECL_ControlData &ctl_data) float ECL_WheelController::control_bodyrate(const struct ECL_ControlData &ctl_data)
{ {
/* Do not calculate control signal with bad inputs */ /* Do not calculate control signal with bad inputs */
if (!(PX4_ISFINITE(ctl_data.body_z_rate) && if (!(ISFINITE(ctl_data.body_z_rate) &&
PX4_ISFINITE(ctl_data.groundspeed) && ISFINITE(ctl_data.groundspeed) &&
PX4_ISFINITE(ctl_data.groundspeed_scaler))) { ISFINITE(ctl_data.groundspeed_scaler))) {
return math::constrain(_last_output, -1.0f, 1.0f); return math::constrain(_last_output, -1.0f, 1.0f);
} }
/* get the usual dt estimate */ /* get the usual dt estimate */
uint64_t dt_micros = ecl_elapsed_time(&_last_run); uint64_t dt_micros = ecl_elapsed_time(&_last_run);
_last_run = ecl_absolute_time(); _last_run = ecl_absolute_time();
float dt = (float)dt_micros * 1e-6f; float dt = dt_micros * 1e-6f;
/* lock integral for long intervals */ /* lock integral for long intervals */
bool lock_integrator = ctl_data.lock_integrator; bool lock_integrator = ctl_data.lock_integrator;
@ -109,12 +99,11 @@ float ECL_WheelController::control_bodyrate(const struct ECL_ControlData &ctl_da
return math::constrain(_last_output, -1.0f, 1.0f); return math::constrain(_last_output, -1.0f, 1.0f);
} }
float ECL_WheelController::control_attitude(const struct ECL_ControlData &ctl_data) float ECL_WheelController::control_attitude(const struct ECL_ControlData &ctl_data)
{ {
/* Do not calculate control signal with bad inputs */ /* Do not calculate control signal with bad inputs */
if (!(PX4_ISFINITE(ctl_data.yaw_setpoint) && if (!(ISFINITE(ctl_data.yaw_setpoint) &&
PX4_ISFINITE(ctl_data.yaw))) { ISFINITE(ctl_data.yaw))) {
return _rate_setpoint; return _rate_setpoint;
} }
@ -125,14 +114,13 @@ float ECL_WheelController::control_attitude(const struct ECL_ControlData &ctl_da
_rate_setpoint = yaw_error / _tc; _rate_setpoint = yaw_error / _tc;
/* limit the rate */ /* limit the rate */
if (_max_rate > 0.01f) { if (_max_rate >= 0.0f) {
if (_rate_setpoint > 0.0f) { if (_rate_setpoint > 0.0f) {
_rate_setpoint = (_rate_setpoint > _max_rate) ? _max_rate : _rate_setpoint; _rate_setpoint = (_rate_setpoint > _max_rate) ? _max_rate : _rate_setpoint;
} else { } else {
_rate_setpoint = (_rate_setpoint < -_max_rate) ? -_max_rate : _rate_setpoint; _rate_setpoint = (_rate_setpoint < -_max_rate) ? -_max_rate : _rate_setpoint;
} }
} }
return _rate_setpoint; return _rate_setpoint;

16
attitude_fw/ecl_wheel_controller.h
Unescape View File

@ -49,23 +49,17 @@
#ifndef ECL_HEADING_CONTROLLER_H #ifndef ECL_HEADING_CONTROLLER_H
#define ECL_HEADING_CONTROLLER_H #define ECL_HEADING_CONTROLLER_H
#include <stdbool.h>
#include <stdint.h>
#include "ecl_controller.h" #include "ecl_controller.h"
class __EXPORT ECL_WheelController : class __EXPORT ECL_WheelController final : public ECL_Controller
public ECL_Controller
{ {
public: public:
ECL_WheelController(); ECL_WheelController() = default;
~ECL_WheelController() = default; ~ECL_WheelController() = default;
float control_attitude(const struct ECL_ControlData &ctl_data); float control_attitude(const struct ECL_ControlData &ctl_data) override;
float control_bodyrate(const struct ECL_ControlData &ctl_data) override;
float control_bodyrate(const struct ECL_ControlData &ctl_data); float control_euler_rate(const struct ECL_ControlData &ctl_data) override { return 0; }
float control_euler_rate(const struct ECL_ControlData &ctl_data) {return 0;}
}; };
#endif // ECL_HEADING_CONTROLLER_H #endif // ECL_HEADING_CONTROLLER_H

75
attitude_fw/ecl_yaw_controller.cpp
Unescape View File

@ -35,54 +35,18 @@
* @file ecl_yaw_controller.cpp * @file ecl_yaw_controller.cpp
* Implementation of a simple orthogonal coordinated turn yaw PID controller. * Implementation of a simple orthogonal coordinated turn yaw PID controller.
* *
* Authors and acknowledgements in header. * Authors and acknowledgments in header.
*/ */
#include "ecl_yaw_controller.h" #include "ecl_yaw_controller.h"
#include <stdint.h>
#include <float.h>
#include <geo/geo.h>
#include <ecl/ecl.h>
#include <mathlib/mathlib.h>
#include <systemlib/err.h>
#include <ecl/ecl.h>
ECL_YawController::ECL_YawController() :
ECL_Controller("yaw"),
_coordinated_min_speed(1.0f),
_max_rate(0.0f), /* disable by default */
_coordinated_method(0)
{
}
float ECL_YawController::control_attitude(const struct ECL_ControlData &ctl_data) float ECL_YawController::control_attitude(const struct ECL_ControlData &ctl_data)
{
switch (_coordinated_method) {
case COORD_METHOD_OPEN:
return control_attitude_impl_openloop(ctl_data);
case COORD_METHOD_CLOSEACC:
return control_attitude_impl_accclosedloop(ctl_data);
default:
static hrt_abstime last_print = 0;
if (ecl_elapsed_time(&last_print) > 5e6) {
warnx("invalid param setting FW_YCO_METHOD");
last_print = ecl_absolute_time();
}
}
return _rate_setpoint;
}
float ECL_YawController::control_attitude_impl_openloop(const struct ECL_ControlData &ctl_data)
{ {
/* Do not calculate control signal with bad inputs */ /* Do not calculate control signal with bad inputs */
if (!(PX4_ISFINITE(ctl_data.roll) && if (!(ISFINITE(ctl_data.roll) &&
PX4_ISFINITE(ctl_data.pitch) && ISFINITE(ctl_data.pitch) &&
PX4_ISFINITE(ctl_data.roll_rate_setpoint) && ISFINITE(ctl_data.roll_rate_setpoint) &&
PX4_ISFINITE(ctl_data.pitch_rate_setpoint))) { ISFINITE(ctl_data.pitch_rate_setpoint))) {
return _rate_setpoint; return _rate_setpoint;
} }
@ -120,12 +84,12 @@ float ECL_YawController::control_attitude_impl_openloop(const struct ECL_Control
/* limit the rate */ //XXX: move to body angluar rates /* limit the rate */ //XXX: move to body angluar rates
if (_max_rate > 0.01f) { if (_max_rate >= 0.0f) {
_rate_setpoint = (_rate_setpoint > _max_rate) ? _max_rate : _rate_setpoint; _rate_setpoint = (_rate_setpoint > _max_rate) ? _max_rate : _rate_setpoint;
_rate_setpoint = (_rate_setpoint < -_max_rate) ? -_max_rate : _rate_setpoint; _rate_setpoint = (_rate_setpoint < -_max_rate) ? -_max_rate : _rate_setpoint;
} }
if (!PX4_ISFINITE(_rate_setpoint)) { if (!ISFINITE(_rate_setpoint)) {
warnx("yaw rate sepoint not finite"); warnx("yaw rate sepoint not finite");
_rate_setpoint = 0.0f; _rate_setpoint = 0.0f;
} }
@ -136,17 +100,17 @@ float ECL_YawController::control_attitude_impl_openloop(const struct ECL_Control
float ECL_YawController::control_bodyrate(const struct ECL_ControlData &ctl_data) float ECL_YawController::control_bodyrate(const struct ECL_ControlData &ctl_data)
{ {
/* Do not calculate control signal with bad inputs */ /* Do not calculate control signal with bad inputs */
if (!(PX4_ISFINITE(ctl_data.roll) && PX4_ISFINITE(ctl_data.pitch) && PX4_ISFINITE(ctl_data.body_y_rate) && if (!(ISFINITE(ctl_data.roll) && ISFINITE(ctl_data.pitch) && ISFINITE(ctl_data.body_y_rate) &&
PX4_ISFINITE(ctl_data.body_z_rate) && PX4_ISFINITE(ctl_data.pitch_rate_setpoint) && ISFINITE(ctl_data.body_z_rate) && ISFINITE(ctl_data.pitch_rate_setpoint) &&
PX4_ISFINITE(ctl_data.airspeed_min) && PX4_ISFINITE(ctl_data.airspeed_max) && ISFINITE(ctl_data.airspeed_min) && ISFINITE(ctl_data.airspeed_max) &&
PX4_ISFINITE(ctl_data.scaler))) { ISFINITE(ctl_data.scaler))) {
return math::constrain(_last_output, -1.0f, 1.0f); return math::constrain(_last_output, -1.0f, 1.0f);
} }
/* get the usual dt estimate */ /* get the usual dt estimate */
uint64_t dt_micros = ecl_elapsed_time(&_last_run); uint64_t dt_micros = ecl_elapsed_time(&_last_run);
_last_run = ecl_absolute_time(); _last_run = ecl_absolute_time();
float dt = (float)dt_micros * 1e-6f; float dt = dt_micros * 1e-6f;
/* lock integral for long intervals */ /* lock integral for long intervals */
bool lock_integrator = ctl_data.lock_integrator; bool lock_integrator = ctl_data.lock_integrator;
@ -158,7 +122,7 @@ float ECL_YawController::control_bodyrate(const struct ECL_ControlData &ctl_data
/* input conditioning */ /* input conditioning */
float airspeed = ctl_data.airspeed; float airspeed = ctl_data.airspeed;
if (!PX4_ISFINITE(airspeed)) { if (!ISFINITE(airspeed)) {
/* airspeed is NaN, +- INF or not available, pick center of band */ /* airspeed is NaN, +- INF or not available, pick center of band */
airspeed = 0.5f * (ctl_data.airspeed_min + ctl_data.airspeed_max); airspeed = 0.5f * (ctl_data.airspeed_min + ctl_data.airspeed_max);
@ -166,12 +130,6 @@ float ECL_YawController::control_bodyrate(const struct ECL_ControlData &ctl_data
airspeed = ctl_data.airspeed_min; airspeed = ctl_data.airspeed_min;
} }
/* Close the acceleration loop if _coordinated_method wants this: change body_rate setpoint */
if (_coordinated_method == COORD_METHOD_CLOSEACC) {
// XXX lateral acceleration needs to go into integrator with a gain
//_bodyrate_setpoint -= (ctl_data.acc_body_y / (airspeed * cosf(ctl_data.pitch)));
}
/* Calculate body angular rate error */ /* Calculate body angular rate error */
_rate_error = _bodyrate_setpoint - ctl_data.body_z_rate; // body angular rate error _rate_error = _bodyrate_setpoint - ctl_data.body_z_rate; // body angular rate error
@ -202,16 +160,9 @@ float ECL_YawController::control_bodyrate(const struct ECL_ControlData &ctl_data
_last_output = (_bodyrate_setpoint * _k_ff + _rate_error * _k_p + integrator_constrained) * ctl_data.scaler * _last_output = (_bodyrate_setpoint * _k_ff + _rate_error * _k_p + integrator_constrained) * ctl_data.scaler *
ctl_data.scaler; //scaler is proportional to 1/airspeed ctl_data.scaler; //scaler is proportional to 1/airspeed
return math::constrain(_last_output, -1.0f, 1.0f); return math::constrain(_last_output, -1.0f, 1.0f);
} }
float ECL_YawController::control_attitude_impl_accclosedloop(const struct ECL_ControlData &ctl_data)
{
/* dont set a rate setpoint */
return 0.0f;
}
float ECL_YawController::control_euler_rate(const struct ECL_ControlData &ctl_data) float ECL_YawController::control_euler_rate(const struct ECL_ControlData &ctl_data)
{ {
/* Transform setpoint to body angular rates (jacobian) */ /* Transform setpoint to body angular rates (jacobian) */

41
attitude_fw/ecl_yaw_controller.h
Unescape View File

@ -48,48 +48,17 @@
#ifndef ECL_YAW_CONTROLLER_H #ifndef ECL_YAW_CONTROLLER_H
#define ECL_YAW_CONTROLLER_H #define ECL_YAW_CONTROLLER_H
#include <stdbool.h>
#include <stdint.h>
#include "ecl_controller.h" #include "ecl_controller.h"
class __EXPORT ECL_YawController : class __EXPORT ECL_YawController final : public ECL_Controller
public ECL_Controller
{ {
public: public:
ECL_YawController(); ECL_YawController() = default;
~ECL_YawController() = default; ~ECL_YawController() = default;
float control_attitude(const struct ECL_ControlData &ctl_data); float control_attitude(const struct ECL_ControlData &ctl_data) override;
float control_euler_rate(const struct ECL_ControlData &ctl_data); float control_euler_rate(const struct ECL_ControlData &ctl_data) override;
float control_bodyrate(const struct ECL_ControlData &ctl_data); float control_bodyrate(const struct ECL_ControlData &ctl_data) override;
/* Additional setters */
void set_coordinated_min_speed(float coordinated_min_speed)
{
_coordinated_min_speed = coordinated_min_speed;
}
void set_coordinated_method(int32_t coordinated_method)
{
_coordinated_method = coordinated_method;
}
enum {
COORD_METHOD_OPEN = 0,
COORD_METHOD_CLOSEACC = 1
};
protected:
float _coordinated_min_speed;
float _max_rate;
int32_t _coordinated_method;
float control_attitude_impl_openloop(const struct ECL_ControlData &ctl_data);
float control_attitude_impl_accclosedloop(const struct ECL_ControlData &ctl_data);
}; };
#endif // ECL_YAW_CONTROLLER_H #endif // ECL_YAW_CONTROLLER_H

Write Preview
Loading…
Cancel
Save