/** Read attitude setpoint from RC as euler angles.
* @param[in] in_flight true if in flight
* @param[out] sp attitude setpoint as euler angles
*/
void stabilization_attitude_read_rc_setpoint_eulers(struct Int32Eulers *sp, bool_t in_flight) {
sp->phi = ((int32_t) radio_control.values[RADIO_ROLL] * SP_MAX_PHI / MAX_PPRZ);
sp->theta = ((int32_t) radio_control.values[RADIO_PITCH] * SP_MAX_THETA / MAX_PPRZ);
if (in_flight) {
if (YAW_DEADBAND_EXCEEDED()) {
sp->psi += ((int32_t) radio_control.values[RADIO_YAW] * SP_MAX_R / MAX_PPRZ / RC_UPDATE_FREQ);
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
int32_t delta_psi = sp->psi - stateGetNedToBodyEulers_i()->psi;
int32_t delta_limit = ANGLE_BFP_OF_REAL(STABILIZATION_ATTITUDE_SP_PSI_DELTA_LIMIT);
INT32_ANGLE_NORMALIZE(delta_psi);
if (delta_psi > delta_limit){
sp->psi = stateGetNedToBodyEulers_i()->psi + delta_limit;
}
else if (delta_psi < -delta_limit){
sp->psi = stateGetNedToBodyEulers_i()->psi - delta_limit;
}
INT32_ANGLE_NORMALIZE(sp->psi);
#endif
//Care Free mode
if (guidance_h_mode == GUIDANCE_H_MODE_CARE_FREE) {
//care_free_heading has been set to current psi when entering care free mode.
int32_t cos_psi;
int32_t sin_psi;
int32_t temp_theta;
int32_t care_free_delta_psi_i;
care_free_delta_psi_i = sp->psi - ANGLE_BFP_OF_REAL(care_free_heading);
INT32_ANGLE_NORMALIZE(care_free_delta_psi_i);
PPRZ_ITRIG_SIN(sin_psi, care_free_delta_psi_i);
PPRZ_ITRIG_COS(cos_psi, care_free_delta_psi_i);
temp_theta = INT_MULT_RSHIFT(cos_psi, sp->theta, INT32_ANGLE_FRAC) - INT_MULT_RSHIFT(sin_psi, sp->phi, INT32_ANGLE_FRAC);
sp->phi = INT_MULT_RSHIFT(cos_psi, sp->phi, INT32_ANGLE_FRAC) - INT_MULT_RSHIFT(sin_psi, sp->theta, INT32_ANGLE_FRAC);
sp->theta = temp_theta;
}
}
else { /* if not flying, use current yaw as setpoint */
sp->psi = stateGetNedToBodyEulers_i()->psi;
}
}
void stabilization_attitude_read_rc_setpoint_eulers_f(struct FloatEulers *sp, bool_t in_flight) {
sp->phi = (radio_control.values[RADIO_ROLL] * SP_MAX_PHI / MAX_PPRZ);
sp->theta = (radio_control.values[RADIO_PITCH] * SP_MAX_THETA / MAX_PPRZ);
if (in_flight) {
if (YAW_DEADBAND_EXCEEDED()) {
sp->psi += (radio_control.values[RADIO_YAW] * SP_MAX_R / MAX_PPRZ / RC_UPDATE_FREQ);
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
float delta_psi = sp->psi - stateGetNedToBodyEulers_f()->psi;
FLOAT_ANGLE_NORMALIZE(delta_psi);
if (delta_psi > STABILIZATION_ATTITUDE_SP_PSI_DELTA_LIMIT){
sp->psi = stateGetNedToBodyEulers_f()->psi + STABILIZATION_ATTITUDE_SP_PSI_DELTA_LIMIT;
}
else if (delta_psi < -STABILIZATION_ATTITUDE_SP_PSI_DELTA_LIMIT){
sp->psi = stateGetNedToBodyEulers_f()->psi - STABILIZATION_ATTITUDE_SP_PSI_DELTA_LIMIT;
}
FLOAT_ANGLE_NORMALIZE(sp->psi);
#endif
//Care Free mode
if (guidance_h_mode == GUIDANCE_H_MODE_CARE_FREE) {
//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 = sin(care_free_delta_psi_f);
cos_psi = cos(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_t in_flight, bool_t in_carefree, bool_t coordinated_turn) {
sp->phi = (radio_control.values[RADIO_ROLL] * STABILIZATION_ATTITUDE_SP_MAX_PHI / MAX_PPRZ);
sp->theta = (radio_control.values[RADIO_PITCH] * STABILIZATION_ATTITUDE_SP_MAX_THETA / MAX_PPRZ);
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 += (radio_control.values[RADIO_YAW] * STABILIZATION_ATTITUDE_SP_MAX_R / MAX_PPRZ / 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;
}
}
/** Read attitude setpoint from RC as euler angles
* @param[in] coordinated_turn true if in horizontal mode forward
* @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
*/
void stabilization_attitude_read_rc_setpoint_eulers(struct Int32Eulers *sp, bool_t in_flight, bool_t in_carefree, bool_t coordinated_turn) {
const int32_t max_rc_phi = (int32_t) ANGLE_BFP_OF_REAL(STABILIZATION_ATTITUDE_SP_MAX_PHI);
const int32_t max_rc_theta = (int32_t) ANGLE_BFP_OF_REAL(STABILIZATION_ATTITUDE_SP_MAX_THETA);
const int32_t max_rc_r = (int32_t) ANGLE_BFP_OF_REAL(STABILIZATION_ATTITUDE_SP_MAX_R);
sp->phi = (int32_t) ((radio_control.values[RADIO_ROLL] * max_rc_phi) / MAX_PPRZ);
sp->theta = (int32_t) ((radio_control.values[RADIO_PITCH] * max_rc_theta) / MAX_PPRZ);
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 += (int32_t) ((radio_control.values[RADIO_YAW] * max_rc_r) / MAX_PPRZ / RC_UPDATE_FREQ);
INT32_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
int32_t omega;
const int32_t max_phi = ANGLE_BFP_OF_REAL(RadOfDeg(85.0));
if(abs(sp->phi) < max_phi)
omega = ANGLE_BFP_OF_REAL(1.3*tanf(ANGLE_FLOAT_OF_BFP(sp->phi)));
else //max 60 degrees roll, then take constant omega
omega = ANGLE_BFP_OF_REAL(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
const int32_t delta_limit = ANGLE_BFP_OF_REAL(STABILIZATION_ATTITUDE_SP_PSI_DELTA_LIMIT);
int32_t heading = stabilization_attitude_get_heading_i();
int32_t delta_psi = sp->psi - heading;
INT32_ANGLE_NORMALIZE(delta_psi);
if (delta_psi > delta_limit){
sp->psi = heading + delta_limit;
}
else if (delta_psi < -delta_limit){
sp->psi = heading - delta_limit;
}
INT32_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.
int32_t cos_psi;
int32_t sin_psi;
int32_t temp_theta;
int32_t care_free_delta_psi_i;
care_free_delta_psi_i = sp->psi - ANGLE_BFP_OF_REAL(care_free_heading);
INT32_ANGLE_NORMALIZE(care_free_delta_psi_i);
PPRZ_ITRIG_SIN(sin_psi, care_free_delta_psi_i);
PPRZ_ITRIG_COS(cos_psi, care_free_delta_psi_i);
temp_theta = INT_MULT_RSHIFT(cos_psi, sp->theta, INT32_ANGLE_FRAC) - INT_MULT_RSHIFT(sin_psi, sp->phi, INT32_ANGLE_FRAC);
sp->phi = INT_MULT_RSHIFT(cos_psi, sp->phi, INT32_ANGLE_FRAC) - INT_MULT_RSHIFT(sin_psi, sp->theta, INT32_ANGLE_FRAC);
sp->theta = temp_theta;
}
}
else { /* if not flying, use current yaw as setpoint */
sp->psi = stateGetNedToBodyEulers_i()->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();
}
/** Read attitude setpoint from RC as euler angles
* @param[in] coordinated_turn true if in horizontal mode forward
* @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
*/
void 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 */
static float last_ts = 0.f;
sp->phi = get_rc_roll();
sp->theta = get_rc_pitch();
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() * dt;
INT32_ANGLE_NORMALIZE(sp->psi);
}
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)));
} else { //max 60 degrees roll
omega = ANGLE_BFP_OF_REAL(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
const int32_t delta_limit = ANGLE_BFP_OF_REAL(STABILIZATION_ATTITUDE_SP_PSI_DELTA_LIMIT);
int32_t heading = stabilization_attitude_get_heading_i();
int32_t delta_psi = sp->psi - heading;
INT32_ANGLE_NORMALIZE(delta_psi);
if (delta_psi > delta_limit) {
sp->psi = heading + delta_limit;
} else if (delta_psi < -delta_limit) {
sp->psi = heading - delta_limit;
}
INT32_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.
int32_t cos_psi;
int32_t sin_psi;
int32_t temp_theta;
int32_t care_free_delta_psi_i;
care_free_delta_psi_i = sp->psi - ANGLE_BFP_OF_REAL(care_free_heading);
INT32_ANGLE_NORMALIZE(care_free_delta_psi_i);
PPRZ_ITRIG_SIN(sin_psi, care_free_delta_psi_i);
PPRZ_ITRIG_COS(cos_psi, care_free_delta_psi_i);
temp_theta = INT_MULT_RSHIFT(cos_psi, sp->theta, INT32_ANGLE_FRAC) - INT_MULT_RSHIFT(sin_psi, sp->phi,
INT32_ANGLE_FRAC);
sp->phi = INT_MULT_RSHIFT(cos_psi, sp->phi, INT32_ANGLE_FRAC) - INT_MULT_RSHIFT(sin_psi, sp->theta, INT32_ANGLE_FRAC);
sp->theta = temp_theta;
}
} else { /* if not flying, use current yaw as setpoint */
sp->psi = stateGetNedToBodyEulers_i()->psi;
}
/* update timestamp for dt calculation */
last_ts = get_sys_time_float();
}
