Mirrors_Framework/paparazzi
1
0
Fork 0
mirror of https://github.com/paparazzi/paparazzi.git synced 2026-06-04 22:17:01 +08:00
Code Issues Actions 30 Packages Projects Releases Wiki Activity

first attempt to fix the quaternion setpoint, see issue #160

Browse Source
This commit is contained in:
Felix Ruess
2012-05-04 17:40:14 +02:00
parent d7915568f2
commit 05fee0cd14
2 changed files with 47 additions and 5 deletions
Download Patch File Download Diff File Expand all files Collapse all files
sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_quat_int.c
+36 -5
View File
@@ -184,10 +184,41 @@ void stabilization_attitude_read_rc(bool_t in_flight) {
#if USE_SETPOINTS_WITH_TRANSITIONS
stabilization_attitude_read_rc_absolute(in_flight);
#else
stabilization_attitude_read_rc_setpoint_eulers(&stab_att_sp_euler, in_flight);
/* update quaternion setpoint from euler setpoint */
INT32_QUAT_OF_EULERS(stab_att_sp_quat, stab_att_sp_euler);
INT32_QUAT_WRAP_SHORTEST(stab_att_sp_quat);
#endif
//FIXME: remove me, do in quaternion directly
stabilization_attitude_read_rc_setpoint_eulers(&stab_att_sp_euler, in_flight);
struct FloatQuat q_rp_cmd;
stabilization_attitude_read_rc_roll_pitch_quat(&q_rp_cmd);
/* get current heading */
const struct FloatVect3 zaxis = {0., 0., 1.};
struct FloatQuat q_yaw;
FLOAT_QUAT_OF_AXIS_ANGLE(q_yaw, zaxis, ANGLE_FLOAT_OF_BFP(ahrs.ltp_to_body_euler.psi));
/* apply roll and pitch commands with respect to current heading */
struct FloatQuat q_sp;
FLOAT_QUAT_COMP(q_sp, q_yaw, q_rp_cmd);
FLOAT_QUAT_NORMALIZE(q_sp);
if (in_flight)
{
/* get current heading setpoint */
struct FloatQuat q_yaw_sp;
FLOAT_QUAT_OF_AXIS_ANGLE(q_yaw_sp, zaxis, ANGLE_FLOAT_OF_BFP(stab_att_sp_euler.psi));
/* rotation between current yaw and yaw setpoint */
struct FloatQuat q_yaw_diff;
FLOAT_QUAT_COMP_INV(q_yaw_diff, q_yaw_sp, q_yaw);
/* temporary copy with roll/pitch command and current heading */
struct FloatQuat q_rp_sp;
QUAT_COPY(q_rp_sp, q_sp);
/* compute final setpoint with yaw */
FLOAT_QUAT_COMP_NORM_SHORTEST(q_sp, q_rp_sp, q_yaw_diff);
}
QUAT_BFP_OF_REAL(stab_att_sp_quat, q_sp);
#endif
}
sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_rc_setpoint.h
+11
View File
@@ -85,5 +85,16 @@ static inline void stabilization_attitude_read_rc_setpoint_eulers(struct Int32Eu
}
static inline void stabilization_attitude_read_rc_roll_pitch_quat(struct FloatQuat* q) {
q->qx = radio_control.values[RADIO_ROLL] * STABILIZATION_ATTITUDE_SP_MAX_PHI / MAX_PPRZ / 2;
q->qy = radio_control.values[RADIO_PITCH] * STABILIZATION_ATTITUDE_SP_MAX_THETA / MAX_PPRZ / 2;
q->qz = 0.0;
/* normalize */
float norm = sqrtf(1.0 + SQUARE(q->qx)+ SQUARE(q->qy));
q->qi = 1.0 / norm;
q->qx /= norm;
q->qy /= norm;
}
#endif /* STABILISATION_ATTITUDE_RC_SETPOINT_H */