diff options
Diffstat (limited to 'src/lib/ecl')
-rw-r--r-- | src/lib/ecl/attitude_fw/ecl_yaw_controller.cpp | 24 | ||||
-rw-r--r-- | src/lib/ecl/attitude_fw/ecl_yaw_controller.h | 30 |
2 files changed, 22 insertions, 32 deletions
diff --git a/src/lib/ecl/attitude_fw/ecl_yaw_controller.cpp b/src/lib/ecl/attitude_fw/ecl_yaw_controller.cpp index b786acf24..9601fa544 100644 --- a/src/lib/ecl/attitude_fw/ecl_yaw_controller.cpp +++ b/src/lib/ecl/attitude_fw/ecl_yaw_controller.cpp @@ -47,12 +47,10 @@ ECL_YawController::ECL_YawController() : _last_run(0), - _last_error(0.0f), _last_output(0.0f), - _last_rate_hp_out(0.0f), - _last_rate_hp_in(0.0f), - _k_d_last(0.0f), - _integrator(0.0f) + _rate_setpoint(0.0f), + _max_deflection_rad(math::radians(45.0f)) + { } @@ -66,7 +64,21 @@ float ECL_YawController::control(float roll, float yaw_rate, float accel_y, floa float dt = (dt_micros > 500000) ? 0.0f : dt_micros / 1000000; - return 0.0f; +// float psi_dot = 0.0f; +// float denumerator = (speed_body[0] * cosf(att_sp->roll_body) * cosf(att_sp->pitch_body) + speed_body[2] * sinf(att_sp->pitch_body)); +// if(denumerator != 0.0f) { +// psi_dot = (speed_body[2] * phi_dot + 9.81f * sinf(att_sp->roll_body) * cosf(att_sp->pitch_body) + speed_body[0] * theta_dot * sinf(att_sp->roll_body)) +// / (speed_body[0] * cosf(att_sp->roll_body) * cosf(att_sp->pitch_body) + speed_body[2] * sinf(att_sp->pitch_body)); +// } + + /* Calculate desired yaw rate from coordinated turn constraint / (no side forces) */ + _last_output = 0.0f; + float denumerator = (speed_body_u * cosf(roll) * cosf(pitch) + speed_body_w * sinf(pitch)); + if(denumerator != 0.0f) { //XXX: floating point comparison + _last_output = (speed_body_w * roll_rate_desired + 9.81f * sinf(roll) * cosf(pitch) + speed_body_u * pitch_rate_desired * sinf(roll)) / denumerator; + } + + return math::constrain(_last_output, -_max_deflection_rad, _max_deflection_rad); } void ECL_YawController::reset_integrator() diff --git a/src/lib/ecl/attitude_fw/ecl_yaw_controller.h b/src/lib/ecl/attitude_fw/ecl_yaw_controller.h index 66b227918..fe0abb956 100644 --- a/src/lib/ecl/attitude_fw/ecl_yaw_controller.h +++ b/src/lib/ecl/attitude_fw/ecl_yaw_controller.h @@ -50,39 +50,17 @@ public: float control(float roll, float yaw_rate, float accel_y, float scaler = 1.0f, bool lock_integrator = false, float airspeed_min = 0, float airspeed_max = 0, float aspeed = (0.0f / 0.0f)); - void reset_integrator(); - void set_k_side(float k_a) { - _k_side = k_a; - } - void set_k_i(float k_i) { - _k_i = k_i; - } - void set_k_d(float k_d) { - _k_d = k_d; - } - void set_k_roll_ff(float k_roll_ff) { - _k_roll_ff = k_roll_ff; - } - void set_integrator_max(float max) { - _integrator_max = max; + float get_desired_rate() { + return _rate_setpoint; } private: uint64_t _last_run; - float _k_side; - float _k_i; - float _k_d; - float _k_roll_ff; - float _integrator_max; - - float _last_error; + float _rate_setpoint; float _last_output; - float _last_rate_hp_out; - float _last_rate_hp_in; - float _k_d_last; - float _integrator; + float _max_deflection_rad; }; |