Mirrors_Framework/paparazzi
1
0
Fork 0
mirror of https://github.com/paparazzi/paparazzi.git synced 2026-05-25 23:46:04 +08:00
Code Issues Actions 25 Packages Projects Releases Wiki Activity

[fix] rates and attitude use the same memory, so they could be used at the same time. (#3113)

Browse Source 
[fix] stabilization FF
This commit is contained in:
Christophe De Wagter
2023-09-27 14:30:40 +02:00
committed by GitHub
parent 46b07c1e9a
commit 63fd5837a8
4 changed files with 22 additions and 16 deletions
Download Patch File Download Diff File Expand all files Collapse all files
sw/airborne/firmwares/rotorcraft/stabilization.c
+8 -8
View File
@@ -277,7 +277,7 @@ struct FloatEulers stab_sp_to_eulers_f(struct StabilizationSetpoint *sp)
}
} else {
// error, rates setpoint
struct FloatEulers eulers = {0};
struct FloatEulers eulers = {0, 0, 0};
return eulers;
}
}
@@ -286,10 +286,10 @@ struct Int32Rates stab_sp_to_rates_i(struct StabilizationSetpoint *sp)
{
if ((sp->type == STAB_SP_RATES) || (sp->type == STAB_SP_QUAT_FF_RATE)) {
if (sp->format == STAB_SP_INT) {
return sp->sp.rates_i;
return sp->r_sp.rates_i;
} else {
struct Int32Rates rates;
RATES_BFP_OF_REAL(rates, sp->sp.rates_f);
RATES_BFP_OF_REAL(rates, sp->r_sp.rates_f);
return rates;
}
} else {
@@ -303,10 +303,10 @@ struct FloatRates stab_sp_to_rates_f(struct StabilizationSetpoint *sp)
{
if ((sp->type == STAB_SP_RATES) || (sp->type == STAB_SP_QUAT_FF_RATE)) {
if (sp->format == STAB_SP_FLOAT) {
return sp->sp.rates_f;
return sp->r_sp.rates_f;
} else {
struct FloatRates rates;
RATES_FLOAT_OF_BFP(rates, sp->sp.rates_i);
RATES_FLOAT_OF_BFP(rates, sp->r_sp.rates_i);
return rates;
}
} else {
@@ -342,7 +342,7 @@ struct StabilizationSetpoint stab_sp_from_quat_ff_rates_f(struct FloatQuat *quat
.type = STAB_SP_QUAT_FF_RATE,
.format = STAB_SP_FLOAT,
.sp.quat_f = *quat,
.sp.rates_f = *rates
.r_sp.rates_f = *rates
};
return sp;
}
@@ -394,7 +394,7 @@ struct StabilizationSetpoint stab_sp_from_rates_i(struct Int32Rates *rates)
struct StabilizationSetpoint sp = {
.type = STAB_SP_RATES,
.format = STAB_SP_INT,
.sp.rates_i = *rates
.r_sp.rates_i = *rates
};
return sp;
}
@@ -404,7 +404,7 @@ struct StabilizationSetpoint stab_sp_from_rates_f(struct FloatRates *rates)
struct StabilizationSetpoint sp = {
.type = STAB_SP_RATES,
.format = STAB_SP_FLOAT,
.sp.rates_f = *rates
.r_sp.rates_f = *rates
};
return sp;
}
sw/airborne/firmwares/rotorcraft/stabilization.h
+3 -1
View File
@@ -58,9 +58,11 @@ struct StabilizationSetpoint {
struct FloatEulers eulers_f;
struct { struct Int32Vect2 vect; int32_t heading; } ltp_i;
struct { struct FloatVect2 vect; float heading; } ltp_f;
} sp;
union {
struct Int32Rates rates_i;
struct FloatRates rates_f;
} sp;
} r_sp;
};
// helper convert functions
sw/airborne/firmwares/rotorcraft/stabilization/stabilization_indi.c
+5 -5
View File
@@ -189,7 +189,7 @@ int32_t num_thrusters;
struct Int32Eulers stab_att_sp_euler;
struct Int32Quat stab_att_sp_quat;
struct Int32Rates stab_att_ff_rates;
struct FloatRates 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);
@@ -414,7 +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);
FLOAT_RATES_ZERO(stab_att_ff_rates);
}
/**
@@ -424,7 +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);
FLOAT_RATES_ZERO(stab_att_ff_rates);
}
/**
@@ -448,7 +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);
FLOAT_RATES_ZERO(stab_att_ff_rates);
}
/**
@@ -460,7 +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);
stab_att_ff_rates = stab_sp_to_rates_f(sp);
}
/**
sw/airborne/firmwares/rotorcraft/stabilization/stabilization_indi_simple.c
+6 -2
View File
@@ -92,7 +92,7 @@
struct Int32Eulers stab_att_sp_euler;
struct Int32Quat stab_att_sp_quat;
struct Int32Rates stab_att_ff_rates;
struct FloatRates stab_att_ff_rates;
static struct FirstOrderLowPass rates_filt_fo[3];
@@ -249,6 +249,7 @@ void stabilization_indi_set_failsafe_setpoint(void)
stab_att_sp_quat.qx = 0;
stab_att_sp_quat.qy = 0;
PPRZ_ITRIG_SIN(stab_att_sp_quat.qz, heading2);
FLOAT_RATES_ZERO(stab_att_ff_rates);
}
/**
@@ -262,6 +263,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);
FLOAT_RATES_ZERO(stab_att_ff_rates);
}
/**
@@ -271,6 +273,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);
FLOAT_RATES_ZERO(stab_att_ff_rates);
}
/**
@@ -294,6 +297,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);
FLOAT_RATES_ZERO(stab_att_ff_rates);
}
/**
@@ -305,7 +309,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);
stab_att_ff_rates = stab_sp_to_rates_f(sp);
}
/**