Skip to content

Commit

Permalink
Restructure INDI to limit estimated disturbances
Browse files Browse the repository at this point in the history
  • Loading branch information
@EwoudSmeur
EwoudSmeur committed Mar 26, 2024
1 parent 5b13467 commit 3d0700a
Show file tree
Hide file tree
Showing 5 changed files with 139 additions and 96 deletions.
1 change: 1 addition & 0 deletions conf/modules/stabilization_indi.xml
View file
Original file line number Diff line number Diff line change
Expand Up @@ -61,6 +61,7 @@
<dl_setting var="indi_gains.att.r" min="0" step="1" max="2500" shortname="kp_r" param="STABILIZATION_INDI_REF_ERR_R"/>
<dl_setting var="indi_gains.rate.r" min="0.1" step="0.1" max="100" shortname="kd_r" param="STABILIZATION_INDI_REF_RATE_P"/>
<dl_setting var="indi_use_adaptive" min="0" step="1" max="1" shortname="use_adaptive" values="FALSE|TRUE" param="STABILIZATION_INDI_USE_ADAPTIVE" type="uint8"/>
<dl_setting var="stablization_indi_yaw_dist_limit" min="0" step=".01" max="10.0" shortname="lim_yaw_spec_moment" param="STABILIZATION_INDI_YAW_DISTURBANCE_LIMIT"/>
</dl_settings>
</dl_settings>
</settings>
Expand Down
60 changes: 38 additions & 22 deletions sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_rc_setpoint.c
View file
Original file line number Diff line number Diff line change
Expand Up @@ -169,8 +169,10 @@ float stabilization_attitude_get_heading_f(void)
* @param[in] in_carefree true if in carefree mode
* @param[in] in_flight true if in flight
* @param[out] sp attitude setpoint as euler angles
*
* @return yaw_rate yaw rate in rad/s with RATE FRAC
*/
void stabilization_attitude_read_rc_setpoint_eulers(struct Int32Eulers *sp, bool in_flight, bool in_carefree,
int32_t stabilization_attitude_read_rc_setpoint_eulers(struct Int32Eulers *sp, bool in_flight, bool in_carefree,
bool coordinated_turn)
{
/* last time this function was called, used to calculate yaw setpoint update */
Expand All @@ -179,6 +181,8 @@ void stabilization_attitude_read_rc_setpoint_eulers(struct Int32Eulers *sp, bool
sp->phi = get_rc_roll();
sp->theta = get_rc_pitch();

int32_t omega = 0;

if (in_flight) {
/* calculate dt for yaw integration */
float dt = get_sys_time_float() - last_ts;
Expand All @@ -187,22 +191,20 @@ void stabilization_attitude_read_rc_setpoint_eulers(struct Int32Eulers *sp, bool

/* do not advance yaw setpoint if within a small deadband around stick center or if throttle is zero */
if (YAW_DEADBAND_EXCEEDED() && !THROTTLE_STICK_DOWN()) {
sp->psi += get_rc_yaw() * dt;
INT32_ANGLE_NORMALIZE(sp->psi);
omega += get_rc_yaw();
}
if (coordinated_turn) {
//Coordinated turn
//feedforward estimate angular rotation omega = g*tan(phi)/v
int32_t omega;
const int32_t max_phi = ANGLE_BFP_OF_REAL(RadOfDeg(60.0));
if (abs(sp->phi) < max_phi) {
omega = ANGLE_BFP_OF_REAL(9.81 / COORDINATED_TURN_AIRSPEED * tanf(ANGLE_FLOAT_OF_BFP(sp->phi)));
omega += ANGLE_BFP_OF_REAL(9.81 / COORDINATED_TURN_AIRSPEED * tanf(ANGLE_FLOAT_OF_BFP(sp->phi)));
} else { //max 60 degrees roll
omega = ANGLE_BFP_OF_REAL(9.81 / COORDINATED_TURN_AIRSPEED * 1.72305 * ((sp->phi > 0) - (sp->phi < 0)));
omega += ANGLE_BFP_OF_REAL(9.81 / COORDINATED_TURN_AIRSPEED * 1.72305 * ((sp->phi > 0) - (sp->phi < 0)));
}

sp->psi += omega * dt;
}
sp->psi += omega * dt;
INT32_ANGLE_NORMALIZE(sp->psi);
#ifdef STABILIZATION_ATTITUDE_SP_PSI_DELTA_LIMIT
// Make sure the yaw setpoint does not differ too much from the real yaw
// to prevent a sudden switch at 180 deg
Expand Down Expand Up @@ -246,10 +248,12 @@ void stabilization_attitude_read_rc_setpoint_eulers(struct Int32Eulers *sp, bool

/* update timestamp for dt calculation */
last_ts = get_sys_time_float();

return omega;
}


void stabilization_attitude_read_rc_setpoint_eulers_f(struct FloatEulers *sp, bool in_flight, bool in_carefree,
float stabilization_attitude_read_rc_setpoint_eulers_f(struct FloatEulers *sp, bool in_flight, bool in_carefree,
bool coordinated_turn)
{
/* last time this function was called, used to calculate yaw setpoint update */
Expand All @@ -258,6 +262,8 @@ void stabilization_attitude_read_rc_setpoint_eulers_f(struct FloatEulers *sp, bo
sp->phi = get_rc_roll_f();
sp->theta = get_rc_pitch_f();

float omega = 0.f;

if (in_flight) {
/* calculate dt for yaw integration */
float dt = get_sys_time_float() - last_ts;
Expand All @@ -266,22 +272,20 @@ void stabilization_attitude_read_rc_setpoint_eulers_f(struct FloatEulers *sp, bo

/* do not advance yaw setpoint if within a small deadband around stick center or if throttle is zero */
if (YAW_DEADBAND_EXCEEDED() && !THROTTLE_STICK_DOWN()) {
sp->psi += get_rc_yaw_f() * dt;
FLOAT_ANGLE_NORMALIZE(sp->psi);
omega += get_rc_yaw_f();
}
if (coordinated_turn) {
//Coordinated turn
//feedforward estimate angular rotation omega = g*tan(phi)/v
float omega;
const float max_phi = RadOfDeg(60.0);
if (fabsf(sp->phi) < max_phi) {
omega = 9.81 / COORDINATED_TURN_AIRSPEED * tanf(sp->phi);
omega += 9.81 / COORDINATED_TURN_AIRSPEED * tanf(sp->phi);
} else { //max 60 degrees roll
omega = 9.81 / COORDINATED_TURN_AIRSPEED * 1.72305 * ((sp->phi > 0) - (sp->phi < 0));
omega += 9.81 / COORDINATED_TURN_AIRSPEED * 1.72305 * ((sp->phi > 0) - (sp->phi < 0));
}

sp->psi += omega * dt;
}
sp->psi += omega * dt;
FLOAT_ANGLE_NORMALIZE(sp->psi);
#ifdef STABILIZATION_ATTITUDE_SP_PSI_DELTA_LIMIT
// Make sure the yaw setpoint does not differ too much from the real yaw
// to prevent a sudden switch at 180 deg
Expand Down Expand Up @@ -321,6 +325,8 @@ void stabilization_attitude_read_rc_setpoint_eulers_f(struct FloatEulers *sp, bo

/* update timestamp for dt calculation */
last_ts = get_sys_time_float();

return omega;
}


Expand Down Expand Up @@ -371,16 +377,20 @@ void stabilization_attitude_read_rc_roll_pitch_earth_quat_f(struct FloatQuat *q)
* @param[in] in_flight true if in flight
* @param[out] q_sp attitude setpoint as quaternion
*/
void stabilization_attitude_read_rc_setpoint_quat_f(struct FloatQuat *q_sp, bool in_flight, bool in_carefree,
float stabilization_attitude_read_rc_setpoint_quat_f(struct FloatQuat *q_sp, bool in_flight, bool in_carefree,
bool coordinated_turn)
{

float yaw_rate;

// FIXME: remove me, do in quaternion directly
// is currently still needed, since the yaw setpoint integration is done in eulers
#if defined STABILIZATION_ATTITUDE_TYPE_INT
stabilization_attitude_read_rc_setpoint_eulers(&stab_att_sp_euler, in_flight, in_carefree, coordinated_turn);
int32_t yaw_rate_i;
yaw_rate_i = stabilization_attitude_read_rc_setpoint_eulers(&stab_att_sp_euler, in_flight, in_carefree, coordinated_turn);
yaw_rate = ANGLE_FLOAT_OF_BFP(yaw_rate_i);
#else
stabilization_attitude_read_rc_setpoint_eulers_f(&stab_att_sp_euler, in_flight, in_carefree, coordinated_turn);
yaw_rate = stabilization_attitude_read_rc_setpoint_eulers_f(&stab_att_sp_euler, in_flight, in_carefree, coordinated_turn);
#endif

struct FloatQuat q_rp_cmd;
Expand Down Expand Up @@ -421,18 +431,23 @@ void stabilization_attitude_read_rc_setpoint_quat_f(struct FloatQuat *q_sp, bool
} else {
QUAT_COPY(*q_sp, q_rp_sp);
}

return yaw_rate;
}

//Function that reads the rc setpoint in an earth bound frame
void stabilization_attitude_read_rc_setpoint_quat_earth_bound_f(struct FloatQuat *q_sp, bool in_flight,
float stabilization_attitude_read_rc_setpoint_quat_earth_bound_f(struct FloatQuat *q_sp, bool in_flight,
bool in_carefree, bool coordinated_turn)
{
float yaw_rate;
// FIXME: remove me, do in quaternion directly
// is currently still needed, since the yaw setpoint integration is done in eulers
#if defined STABILIZATION_ATTITUDE_TYPE_INT
stabilization_attitude_read_rc_setpoint_eulers(&stab_att_sp_euler, in_flight, in_carefree, coordinated_turn);
int32_t yaw_rate_i;
yaw_rate_i = stabilization_attitude_read_rc_setpoint_eulers(&stab_att_sp_euler, in_flight, in_carefree, coordinated_turn);
yaw_rate = ANGLE_FLOAT_OF_BFP(yaw_rate_i);
#else
stabilization_attitude_read_rc_setpoint_eulers_f(&stab_att_sp_euler, in_flight, in_carefree, coordinated_turn);
yaw_rate = stabilization_attitude_read_rc_setpoint_eulers_f(&stab_att_sp_euler, in_flight, in_carefree, coordinated_turn);
#endif

const struct FloatVect3 zaxis = {0., 0., 1.};
Expand Down Expand Up @@ -462,4 +477,5 @@ void stabilization_attitude_read_rc_setpoint_quat_earth_bound_f(struct FloatQuat

QUAT_COPY(*q_sp, q_rp_sp);
}
return yaw_rate;
}
8 changes: 4 additions & 4 deletions sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_rc_setpoint.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -35,15 +35,15 @@ extern int32_t transition_theta_offset; // Pitch offset added for hybrid vehicle
extern void stabilization_attitude_reset_care_free_heading(void);
extern int32_t stabilization_attitude_get_heading_i(void);
extern float stabilization_attitude_get_heading_f(void);
extern void stabilization_attitude_read_rc_setpoint_eulers(struct Int32Eulers *sp, bool in_flight, bool in_carefree,
extern int32_t stabilization_attitude_read_rc_setpoint_eulers(struct Int32Eulers *sp, bool in_flight, bool in_carefree,
bool coordinated_turn);
extern void stabilization_attitude_read_rc_setpoint_eulers_f(struct FloatEulers *sp, bool in_flight,
extern float stabilization_attitude_read_rc_setpoint_eulers_f(struct FloatEulers *sp, bool in_flight,
bool in_carefree, bool coordinated_turn);
extern void stabilization_attitude_read_rc_roll_pitch_quat_f(struct FloatQuat *q);
extern void stabilization_attitude_read_rc_roll_pitch_earth_quat_f(struct FloatQuat *q);
extern void stabilization_attitude_read_rc_setpoint_quat_f(struct FloatQuat *q_sp, bool in_flight, bool in_carefree,
extern float stabilization_attitude_read_rc_setpoint_quat_f(struct FloatQuat *q_sp, bool in_flight, bool in_carefree,
bool coordinated_turn);
extern void stabilization_attitude_read_rc_setpoint_quat_earth_bound_f(struct FloatQuat *q_sp, bool in_flight,
extern float stabilization_attitude_read_rc_setpoint_quat_earth_bound_f(struct FloatQuat *q_sp, bool in_flight,
bool in_carefree, bool coordinated_turn);

#endif /* STABILIZATION_ATTITUDE_RC_SETPOINT_H */

0 comments on commit 3d0700a

Please sign in to comment.