Browse Source

fix initialization of attitude controllers

sbg
Thomas Gubler 11 years ago
parent
commit
3b1086b879
  1. 11
      src/lib/ecl/attitude_fw/ecl_pitch_controller.cpp
  2. 6
      src/lib/ecl/attitude_fw/ecl_roll_controller.cpp
  3. 11
      src/lib/ecl/attitude_fw/ecl_yaw_controller.cpp
  4. 2
      src/lib/ecl/attitude_fw/ecl_yaw_controller.h

11
src/lib/ecl/attitude_fw/ecl_pitch_controller.cpp

@ -49,10 +49,19 @@ @@ -49,10 +49,19 @@
ECL_PitchController::ECL_PitchController() :
_last_run(0),
_tc(0.1f),
_k_p(0.0f),
_k_i(0.0f),
_k_d(0.0f),
_integrator_max(0.0f),
_max_rate_pos(0.0f),
_max_rate_neg(0.0f),
_roll_ff(0.0f),
_last_output(0.0f),
_integrator(0.0f),
_rate_error(0.0f),
_rate_setpoint(0.0f)
_rate_setpoint(0.0f),
_bodyrate_setpoint(0.0f)
{
}

6
src/lib/ecl/attitude_fw/ecl_roll_controller.cpp

@ -50,13 +50,17 @@ @@ -50,13 +50,17 @@
ECL_RollController::ECL_RollController() :
_last_run(0),
_tc(0.1f),
_k_p(0.0f),
_k_i(0.0f),
_k_d(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)
{
}
float ECL_RollController::control_attitude(float roll_setpoint, float roll)

11
src/lib/ecl/attitude_fw/ecl_yaw_controller.cpp

@ -48,16 +48,19 @@ @@ -48,16 +48,19 @@
ECL_YawController::ECL_YawController() :
_last_run(0),
_tc(0.1f),
_k_p(0.0f),
_k_i(0.0f),
_k_d(0.0f),
_integrator_max(0.0f),
_max_rate(0.0f),
_roll_ff(0.0f),
_last_output(0.0f),
_integrator(0.0f),
_rate_error(0.0f),
_rate_setpoint(0.0f),
_bodyrate_setpoint(0.0f),
_coordinated(1.0f)
_coordinated(0.0f)
{
}
float ECL_YawController::control_attitude(float roll, float pitch,

2
src/lib/ecl/attitude_fw/ecl_yaw_controller.h

@ -104,8 +104,6 @@ public: @@ -104,8 +104,6 @@ public:
private:
uint64_t _last_run;
float _tc;
float _k_p;
float _k_i;
float _k_d;

Loading…
Cancel
Save