void stabilization_attitude_set_failsafe_setpoint(void) { /* set failsafe to zero roll/pitch and current heading */ float heading2 = stabilization_attitude_get_heading_f() / 2; stab_att_sp_quat.qi = cosf(heading2); stab_att_sp_quat.qx = 0.0; stab_att_sp_quat.qy = 0.0; stab_att_sp_quat.qz = sinf(heading2); }
void stabilization_attitude_enter(void) { /* reset psi setpoint to current psi angle */ stab_att_sp_euler.psi = stabilization_attitude_get_heading_f(); stabilization_attitude_ref_enter(); FLOAT_EULERS_ZERO(stabilization_att_sum_err); }
void stabilization_attitude_enter(void) { /* reset psi setpoint to current psi angle */ stab_att_sp_euler.psi = stabilization_attitude_get_heading_f(); attitude_ref_quat_float_enter(&att_ref_quat_f, stab_att_sp_euler.psi); float_quat_identity(&stabilization_att_sum_err_quat); }
void stabilization_attitude_enter(void) { float heading = stabilization_attitude_get_heading_f(); /* reset psi setpoint to current psi angle */ stab_att_sp_euler.psi = heading; stabilization_attitude_ref_enter(); FLOAT_QUAT_ZERO( stabilization_att_sum_err_quat ); FLOAT_EULERS_ZERO( stabilization_att_sum_err_eulers ); }
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; } }
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(); }