void stabilization_attitude_read_rc_setpoint_eulers_f(struct FloatEulers *sp, bool_t in_flight, bool_t in_carefree,
bool_t coordinated_turn)
{
sp->phi = get_rc_roll_f();
sp->theta = get_rc_pitch_f();
if (in_flight) {
/* 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() / RC_UPDATE_FREQ;
FLOAT_ANGLE_NORMALIZE(sp->psi);
}
if (coordinated_turn) {
//Coordinated turn
//feedforward estimate angular rotation omega = g*tan(phi)/v
//Take v = 9.81/1.3 m/s
float omega;
const float max_phi = RadOfDeg(85.0);
if (abs(sp->phi) < max_phi) {
omega = 1.3 * tanf(sp->phi);
} else { //max 60 degrees roll, then take constant omega
omega = 1.3 * 1.72305 * ((sp->phi > 0) - (sp->phi < 0));
}
sp->psi += omega / RC_UPDATE_FREQ;
}
#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
float heading = stabilization_attitude_get_heading_f();
float delta_psi = sp->psi - heading;
FLOAT_ANGLE_NORMALIZE(delta_psi);
if (delta_psi > STABILIZATION_ATTITUDE_SP_PSI_DELTA_LIMIT) {
sp->psi = heading + STABILIZATION_ATTITUDE_SP_PSI_DELTA_LIMIT;
} else if (delta_psi < -STABILIZATION_ATTITUDE_SP_PSI_DELTA_LIMIT) {
sp->psi = heading - STABILIZATION_ATTITUDE_SP_PSI_DELTA_LIMIT;
}
FLOAT_ANGLE_NORMALIZE(sp->psi);
#endif
//Care Free mode
if (in_carefree) {
//care_free_heading has been set to current psi when entering care free mode.
float cos_psi;
float sin_psi;
float temp_theta;
float care_free_delta_psi_f = sp->psi - care_free_heading;
FLOAT_ANGLE_NORMALIZE(care_free_delta_psi_f);
sin_psi = sinf(care_free_delta_psi_f);
cos_psi = cosf(care_free_delta_psi_f);
temp_theta = cos_psi * sp->theta - sin_psi * sp->phi;
sp->phi = cos_psi * sp->phi - sin_psi * sp->theta;
sp->theta = temp_theta;
}
} else { /* if not flying, use current yaw as setpoint */
sp->psi = stateGetNedToBodyEulers_f()->psi;
}
}
void 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 */
static float last_ts = 0.f;
sp->phi = get_rc_roll_f();
sp->theta = get_rc_pitch_f();
if (in_flight) {
/* calculate dt for yaw integration */
float dt = get_sys_time_float() - last_ts;
/* make sure nothing drastically weird happens, bound dt to 0.5sec */
Bound(dt, 0, 0.5);
/* 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);
}
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);
} else { //max 60 degrees roll
omega = 9.81 / COORDINATED_TURN_AIRSPEED * 1.72305 * ((sp->phi > 0) - (sp->phi < 0));
}
sp->psi += omega * dt;
}
#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
float heading = stabilization_attitude_get_heading_f();
float delta_psi = sp->psi - heading;
FLOAT_ANGLE_NORMALIZE(delta_psi);
if (delta_psi > STABILIZATION_ATTITUDE_SP_PSI_DELTA_LIMIT) {
sp->psi = heading + STABILIZATION_ATTITUDE_SP_PSI_DELTA_LIMIT;
} else if (delta_psi < -STABILIZATION_ATTITUDE_SP_PSI_DELTA_LIMIT) {
sp->psi = heading - STABILIZATION_ATTITUDE_SP_PSI_DELTA_LIMIT;
}
FLOAT_ANGLE_NORMALIZE(sp->psi);
#endif
//Care Free mode
if (in_carefree) {
//care_free_heading has been set to current psi when entering care free mode.
float cos_psi;
float sin_psi;
float temp_theta;
float care_free_delta_psi_f = sp->psi - care_free_heading;
FLOAT_ANGLE_NORMALIZE(care_free_delta_psi_f);
sin_psi = sinf(care_free_delta_psi_f);
cos_psi = cosf(care_free_delta_psi_f);
temp_theta = cos_psi * sp->theta - sin_psi * sp->phi;
sp->phi = cos_psi * sp->phi - sin_psi * sp->theta;
sp->theta = temp_theta;
}
} else { /* if not flying, use current yaw as setpoint */
sp->psi = stateGetNedToBodyEulers_f()->psi;
}
/* update timestamp for dt calculation */
last_ts = get_sys_time_float();
}