Mirrors_Framework/paparazzi
1
0
Fork 0
mirror of https://github.com/paparazzi/paparazzi.git synced 2026-05-09 22:49:53 +08:00
Code Issues Actions 28 Packages Projects Releases Wiki Activity

Feed forward guidance (#3068)

Browse Source
This commit is contained in:
Christophe De Wagter
2023-09-27 08:53:49 +02:00
committed by GitHub
parent ffcbfb6c2f
commit 46b07c1e9a
5 changed files with 46 additions and 10 deletions
Download Patch File Download Diff File Expand all files Collapse all files
sw/airborne/firmwares/rotorcraft/guidance/guidance_indi_hybrid.c
+11 -1
View File
@@ -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)
sw/airborne/firmwares/rotorcraft/stabilization.c
+19 -8
View File
@@ -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 = {
sw/airborne/firmwares/rotorcraft/stabilization.h
+3 -1
View File
@@ -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);
sw/airborne/firmwares/rotorcraft/stabilization/stabilization_indi.c
+8
View File
@@ -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;
sw/airborne/firmwares/rotorcraft/stabilization/stabilization_indi_simple.c
+5
View File
@@ -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);
}