diff --git a/sw/airborne/firmwares/rotorcraft/guidance/guidance_indi_hybrid.c b/sw/airborne/firmwares/rotorcraft/guidance/guidance_indi_hybrid.c index f60284f7ba..a1339be7a9 100644 --- a/sw/airborne/firmwares/rotorcraft/guidance/guidance_indi_hybrid.c +++ b/sw/airborne/firmwares/rotorcraft/guidance/guidance_indi_hybrid.c @@ -351,6 +351,16 @@ struct StabilizationSetpoint guidance_indi_run(struct FloatVect3 *accel_sp, floa omega -= accely_filt.o[0]*FWD_SIDESLIP_GAIN; #endif + // We can pre-compute the required rates to achieve this turn rate: + // NOTE: there *should* not be any problems possible with Euler singularities here + struct FloatEulers *euler_zyx = stateGetNedToBodyEulers_f(); + + struct FloatRates ff_rates; + + ff_rates.p = -sinf(euler_zyx->theta) * omega; + ff_rates.q = cosf(euler_zyx->theta) * sinf(euler_zyx->phi) * omega; + ff_rates.r = cosf(euler_zyx->theta) * cosf(euler_zyx->phi) * omega; + // For a hybrid it is important to reduce the sideslip, which is done by changing the heading. // For experiments, it is possible to fix the heading to a different value. if (take_heading_control) { @@ -399,7 +409,7 @@ struct StabilizationSetpoint guidance_indi_run(struct FloatVect3 *accel_sp, floa float_quat_of_eulers_zxy(&sp_quat, &guidance_euler_cmd); float_quat_normalize(&sp_quat); - return stab_sp_from_quat_f(&sp_quat); + return stab_sp_from_quat_ff_rates_f(&sp_quat, &ff_rates); } // compute accel setpoint from speed setpoint (use global variables ! FIXME) diff --git a/sw/airborne/firmwares/rotorcraft/stabilization.c b/sw/airborne/firmwares/rotorcraft/stabilization.c index 41b38d7e20..5c7db82520 100644 --- a/sw/airborne/firmwares/rotorcraft/stabilization.c +++ b/sw/airborne/firmwares/rotorcraft/stabilization.c @@ -122,7 +122,7 @@ void stabilization_filter_commands(void) struct Int32Quat stab_sp_to_quat_i(struct StabilizationSetpoint *sp) { - if (sp->type == STAB_SP_QUAT) { + if ((sp->type == STAB_SP_QUAT) || (sp->type == STAB_SP_QUAT_FF_RATE)) { if (sp->format == STAB_SP_INT) { return sp->sp.quat_i; } else { @@ -164,7 +164,7 @@ struct Int32Quat stab_sp_to_quat_i(struct StabilizationSetpoint *sp) struct FloatQuat stab_sp_to_quat_f(struct StabilizationSetpoint *sp) { - if (sp->type == STAB_SP_QUAT) { + if ((sp->type == STAB_SP_QUAT) || (sp->type == STAB_SP_QUAT_FF_RATE)) { if (sp->format == STAB_SP_FLOAT) { return sp->sp.quat_f; } else { @@ -214,7 +214,7 @@ struct Int32Eulers stab_sp_to_eulers_i(struct StabilizationSetpoint *sp) EULERS_BFP_OF_REAL(eulers, sp->sp.eulers_f); return eulers; } - } else if (sp->type == STAB_SP_QUAT) { + } else if ((sp->type == STAB_SP_QUAT) || (sp->type == STAB_SP_QUAT_FF_RATE)) { if (sp->format == STAB_SP_INT) { struct Int32Eulers eulers; int32_eulers_of_quat(&eulers, &sp->sp.quat_i); @@ -253,7 +253,7 @@ struct FloatEulers stab_sp_to_eulers_f(struct StabilizationSetpoint *sp) EULERS_FLOAT_OF_BFP(eulers, sp->sp.eulers_i); return eulers; } - } else if (sp->type == STAB_SP_QUAT) { + } else if ((sp->type == STAB_SP_QUAT) || (sp->type == STAB_SP_QUAT_FF_RATE)) { if (sp->format == STAB_SP_FLOAT) { struct FloatEulers eulers; float_eulers_of_quat(&eulers, &sp->sp.quat_f); @@ -284,7 +284,7 @@ struct FloatEulers stab_sp_to_eulers_f(struct StabilizationSetpoint *sp) struct Int32Rates stab_sp_to_rates_i(struct StabilizationSetpoint *sp) { - if (sp->type == STAB_SP_RATES) { + if ((sp->type == STAB_SP_RATES) || (sp->type == STAB_SP_QUAT_FF_RATE)) { if (sp->format == STAB_SP_INT) { return sp->sp.rates_i; } else { @@ -294,14 +294,14 @@ struct Int32Rates stab_sp_to_rates_i(struct StabilizationSetpoint *sp) } } else { // error, attitude setpoint - struct Int32Rates rates = {0}; + struct Int32Rates rates = {0, 0, 0}; return rates; } } struct FloatRates stab_sp_to_rates_f(struct StabilizationSetpoint *sp) { - if (sp->type == STAB_SP_RATES) { + if ((sp->type == STAB_SP_RATES) || (sp->type == STAB_SP_QUAT_FF_RATE)) { if (sp->format == STAB_SP_FLOAT) { return sp->sp.rates_f; } else { @@ -311,7 +311,7 @@ struct FloatRates stab_sp_to_rates_f(struct StabilizationSetpoint *sp) } } else { // error, attitude setpoint - struct FloatRates rates = {0}; + struct FloatRates rates = {0, 0, 0}; return rates; } } @@ -336,6 +336,17 @@ struct StabilizationSetpoint stab_sp_from_quat_f(struct FloatQuat *quat) return sp; } +struct StabilizationSetpoint stab_sp_from_quat_ff_rates_f(struct FloatQuat *quat, struct FloatRates *rates) +{ + struct StabilizationSetpoint sp = { + .type = STAB_SP_QUAT_FF_RATE, + .format = STAB_SP_FLOAT, + .sp.quat_f = *quat, + .sp.rates_f = *rates + }; + return sp; +} + struct StabilizationSetpoint stab_sp_from_eulers_i(struct Int32Eulers *eulers) { struct StabilizationSetpoint sp = { diff --git a/sw/airborne/firmwares/rotorcraft/stabilization.h b/sw/airborne/firmwares/rotorcraft/stabilization.h index 5718118a2b..4988a5290e 100644 --- a/sw/airborne/firmwares/rotorcraft/stabilization.h +++ b/sw/airborne/firmwares/rotorcraft/stabilization.h @@ -44,7 +44,8 @@ struct StabilizationSetpoint { STAB_SP_QUAT, ///< LTP to Body orientation in unit quaternion STAB_SP_EULERS, ///< LTP to Body orientation in euler angles STAB_SP_LTP, ///< banking and heading in LTP (NED) frame - STAB_SP_RATES ///< body rates + STAB_SP_RATES, ///< body rates + STAB_SP_QUAT_FF_RATE ///< LTP to Body orientation in unit quaternion with precomputed feedforward rates } type; enum { STAB_SP_INT, @@ -73,6 +74,7 @@ extern struct FloatRates stab_sp_to_rates_f(struct StabilizationSetpoint *sp); // helper make functions extern struct StabilizationSetpoint stab_sp_from_quat_i(struct Int32Quat *quat); extern struct StabilizationSetpoint stab_sp_from_quat_f(struct FloatQuat *quat); +extern struct StabilizationSetpoint stab_sp_from_quat_ff_rates_f(struct FloatQuat *quat, struct FloatRates *rates); extern struct StabilizationSetpoint stab_sp_from_eulers_i(struct Int32Eulers *eulers); extern struct StabilizationSetpoint stab_sp_from_eulers_f(struct FloatEulers *eulers); extern struct StabilizationSetpoint stab_sp_from_ltp_i(struct Int32Vect2 *vect, int32_t heading); diff --git a/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_indi.c b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_indi.c index 5a73927947..1af24ead02 100644 --- a/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_indi.c +++ b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_indi.c @@ -189,6 +189,7 @@ int32_t num_thrusters; struct Int32Eulers stab_att_sp_euler; struct Int32Quat stab_att_sp_quat; +struct Int32Rates stab_att_ff_rates; abi_event rpm_ev; static void rpm_cb(uint8_t sender_id, struct rpm_act_t *rpm_msg, uint8_t num_act); @@ -413,6 +414,7 @@ void stabilization_indi_set_rpy_setpoint_i(struct Int32Eulers *rpy) stab_att_sp_euler = *rpy; int32_quat_of_eulers(&stab_att_sp_quat, &stab_att_sp_euler); + INT_RATES_ZERO(stab_att_ff_rates); } /** @@ -422,6 +424,7 @@ void stabilization_indi_set_quat_setpoint_i(struct Int32Quat *quat) { stab_att_sp_quat = *quat; int32_eulers_of_quat(&stab_att_sp_euler, quat); + INT_RATES_ZERO(stab_att_ff_rates); } /** @@ -445,6 +448,7 @@ void stabilization_indi_set_earth_cmd_i(struct Int32Vect2 *cmd, int32_t heading) stab_att_sp_euler.theta = -(c_psi * cmd->x + s_psi * cmd->y) >> INT32_TRIG_FRAC; quat_from_earth_cmd_i(&stab_att_sp_quat, cmd, heading); + INT_RATES_ZERO(stab_att_ff_rates); } /** @@ -456,6 +460,7 @@ void stabilization_indi_set_stab_sp(struct StabilizationSetpoint *sp) { stab_att_sp_euler = stab_sp_to_eulers_i(sp); stab_att_sp_quat = stab_sp_to_quat_i(sp); + stab_att_ff_rates = stab_sp_to_rates_i(sp); } /** @@ -686,6 +691,9 @@ void stabilization_indi_attitude_run(struct Int32Quat quat_sp, bool in_flight) rate_sp.q = indi_gains.att.q * att_fb.y / indi_gains.rate.q; rate_sp.r = indi_gains.att.r * att_fb.z / indi_gains.rate.r; + // Add feed-forward rates to the attitude feedback part + RATES_ADD(rate_sp, stab_att_ff_rates); + // Store for telemetry angular_rate_ref.p = rate_sp.p; angular_rate_ref.q = rate_sp.q; diff --git a/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_indi_simple.c b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_indi_simple.c index 663da3d4e0..dcd5163f5d 100644 --- a/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_indi_simple.c +++ b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_indi_simple.c @@ -92,6 +92,7 @@ struct Int32Eulers stab_att_sp_euler; struct Int32Quat stab_att_sp_quat; +struct Int32Rates stab_att_ff_rates; static struct FirstOrderLowPass rates_filt_fo[3]; @@ -304,6 +305,7 @@ void stabilization_indi_set_stab_sp(struct StabilizationSetpoint *sp) { stab_att_sp_euler = stab_sp_to_eulers_i(sp); stab_att_sp_quat = stab_sp_to_quat_i(sp); + stab_att_ff_rates = stab_sp_to_rates_i(sp); } /** @@ -485,6 +487,9 @@ void stabilization_indi_attitude_run(struct Int32Quat quat_sp, bool in_flight __ rate_sp.q = indi.gains.att.q * att_fb.y / indi.gains.rate.q; rate_sp.r = indi.gains.att.r * att_fb.z / indi.gains.rate.r; + // Add feed-forward rates to the attitude feedback part + RATES_ADD(rate_sp, stab_att_ff_rates); + /* compute the INDI command */ stabilization_indi_rate_run(rate_sp, in_flight); }