Mirrors_Framework/paparazzi
1
0
Fork 0
mirror of https://github.com/paparazzi/paparazzi.git synced 2026-06-07 00:53:41 +08:00
Code Issues Actions 25 Packages Projects Releases Wiki Activity

Merge branch 'dev' into 4.0_beta

Browse Source 
* preliminary fix for rotorcraft guidance when fixed-point quaternion stabilization is used
This commit is contained in:
Felix Ruess
2012-03-26 01:35:44 +02:00
parent c840489326 f8fd4f8799
commit acdd03e3e0
6 changed files with 55 additions and 30 deletions
Download Patch File Download Diff File Expand all files Collapse all files
sw/airborne/firmwares/rotorcraft/guidance/guidance_h.c
+36 -9
View File
@@ -217,10 +217,16 @@ void guidance_h_run(bool_t in_flight) {
if (horizontal_mode == HORIZONTAL_MODE_ATTITUDE) {
#ifndef STABILISATION_ATTITUDE_TYPE_FLOAT
#ifdef STABILISATION_ATTITUDE_TYPE_QUAT
/* FIXME: stab_att_sp_euler should always have REF_ANGLE_FRAC
* and not be different for quat and euler versions */
stab_att_sp_euler.phi = nav_roll;
stab_att_sp_euler.theta = nav_pitch;
INT32_QUAT_OF_EULERS(stab_att_sp_quat, stab_att_sp_euler);
INT32_QUAT_WRAP_SHORTEST(stab_att_sp_quat);
#else
stab_att_sp_euler.phi = nav_roll << (REF_ANGLE_FRAC - INT32_ANGLE_FRAC);
stab_att_sp_euler.theta = nav_pitch << (REF_ANGLE_FRAC - INT32_ANGLE_FRAC);
#ifdef STABILISATION_ATTITUDE_TYPE_QUAT
INT32_QUAT_OF_EULERS(stab_att_sp_quat, stab_att_sp_euler);
#endif
#endif
}
@@ -303,14 +309,25 @@ __attribute__ ((always_inline)) static inline void guidance_h_hover_run(void) {
guidance_h_command_body.phi += guidance_h_rc_sp.phi;
guidance_h_command_body.theta += guidance_h_rc_sp.theta;
guidance_h_command_body.psi = guidance_h_psi_sp + guidance_h_rc_sp.psi;
#ifndef STABILISATION_ATTITUDE_TYPE_FLOAT
ANGLE_REF_NORMALIZE(guidance_h_command_body.psi);
#endif /* STABILISATION_ATTITUDE_TYPE_FLOAT */
EULERS_COPY(stab_att_sp_euler, guidance_h_command_body);
#ifdef STABILISATION_ATTITUDE_TYPE_INT
ANGLE_REF_NORMALIZE(guidance_h_command_body.psi);
#ifdef STABILISATION_ATTITUDE_TYPE_QUAT
/* guidance_h_command_body with REF_ANGLE_FRAC
* stab_att_sp_euler with INT32_ANGLE_FRAC
* FIXME: stab_att_sp_euler should always have REF_ANGLE_FRAC
*/
INT32_EULERS_LSHIFT(stab_att_sp_euler, guidance_h_command_body, (REF_ANGLE_FRAC - INT32_ANGLE_FRAC));
INT32_QUAT_OF_EULERS(stab_att_sp_quat, stab_att_sp_euler);
#endif
INT32_QUAT_WRAP_SHORTEST(stab_att_sp_quat);
#else
EULERS_COPY(stab_att_sp_euler, guidance_h_command_body);
#endif /* STABILISATION_ATTITUDE_TYPE_QUAT */
#else
EULERS_COPY(stab_att_sp_euler, guidance_h_command_body);
#endif /* STABILISATION_ATTITUDE_TYPE_INT */
}
@@ -401,13 +418,23 @@ __attribute__ ((always_inline)) static inline void guidance_h_nav_run(bool_t in
guidance_h_command_body.phi += guidance_h_rc_sp.phi;
guidance_h_command_body.theta += guidance_h_rc_sp.theta;
guidance_h_command_body.psi = guidance_h_psi_sp + guidance_h_rc_sp.psi;
#ifdef STABILISATION_ATTITUDE_TYPE_INT
ANGLE_REF_NORMALIZE(guidance_h_command_body.psi);
#endif
/* Set attitude setpoint in eulers and as quaternion */
EULERS_COPY(stab_att_sp_euler, guidance_h_command_body);
#ifdef STABILISATION_ATTITUDE_TYPE_QUAT
/* guidance_h_command_body with REF_ANGLE_FRAC
* stab_att_sp_euler with INT32_ANGLE_FRAC
* FIXME: stab_att_sp_euler should always have REF_ANGLE_FRAC
*/
INT32_EULERS_LSHIFT(stab_att_sp_euler, guidance_h_command_body, (REF_ANGLE_FRAC - INT32_ANGLE_FRAC));
INT32_QUAT_OF_EULERS(stab_att_sp_quat, stab_att_sp_euler);
#endif
INT32_QUAT_WRAP_SHORTEST(stab_att_sp_quat);
#else
EULERS_COPY(stab_att_sp_euler, guidance_h_command_body);
#endif /* STABILISATION_ATTITUDE_TYPE_QUAT */
}
sw/airborne/firmwares/rotorcraft/guidance/guidance_h.h
+3 -3
View File
@@ -48,7 +48,7 @@ extern uint8_t guidance_h_mode;
*/
extern struct Int32Vect2 guidance_h_pos_sp;
extern int32_t guidance_h_psi_sp;
extern int32_t guidance_h_psi_sp; ///< with #REF_ANGLE_FRAC
extern struct Int32Vect2 guidance_h_pos_ref;
extern struct Int32Vect2 guidance_h_speed_ref;
extern struct Int32Vect2 guidance_h_accel_ref;
@@ -58,9 +58,9 @@ extern struct Int32Vect2 guidance_h_speed_err;
extern struct Int32Vect2 guidance_h_pos_err_sum;
extern struct Int32Vect2 guidance_h_nav_err;
extern struct Int32Eulers guidance_h_rc_sp;
extern struct Int32Eulers guidance_h_rc_sp; ///< with #REF_ANGLE_FRAC
extern struct Int32Vect2 guidance_h_command_earth;
extern struct Int32Eulers guidance_h_command_body;
extern struct Int32Eulers guidance_h_command_body; ///< with #REF_ANGLE_FRAC
extern int32_t guidance_h_pgain;
extern int32_t guidance_h_dgain;
sw/airborne/firmwares/rotorcraft/navigation.h
+2 -2
View File
@@ -54,8 +54,8 @@ extern int32_t nav_circle_radius, nav_circle_qdr, nav_circle_radians;
#define HORIZONTAL_MODE_ROUTE 1
#define HORIZONTAL_MODE_CIRCLE 2
#define HORIZONTAL_MODE_ATTITUDE 3
extern int32_t nav_roll, nav_pitch;
extern int32_t nav_heading, nav_course;
extern int32_t nav_roll, nav_pitch; ///< with #INT32_ANGLE_FRAC
extern int32_t nav_heading, nav_course; ///< with #INT32_ANGLE_FRAC
extern float nav_radius;
extern uint8_t vertical_mode;
sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_ref_quat_int.c
+2 -1
View File
@@ -109,7 +109,8 @@ void stabilization_attitude_ref_enter()
stabilization_attitude_sp_enter();
memcpy(&stab_att_ref_quat, &stab_att_sp_quat, sizeof(struct Int32Quat));
#else
update_quat_from_eulers(&stab_att_ref_quat, &stab_att_ref_euler);
INT32_QUAT_OF_EULERS(stab_att_ref_quat, stab_att_ref_euler);
INT32_QUAT_WRAP_SHORTEST(stab_att_ref_quat);
#endif
/* set reference rate and acceleration to zero */
sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_ref_quat_int.h
+2 -13
View File
@@ -49,18 +49,6 @@
#define STABILIZATION_ATTITUDE_RESET_PSI_REF(_sp) {}
static inline void update_quat_from_eulers(struct Int32Quat *quat, struct Int32Eulers *eulers) {
struct Int32RMat rmat;
#ifdef STICKS_RMAT312
INT32_RMAT_OF_EULERS_312(rmat, *eulers);
#else
INT32_RMAT_OF_EULERS_321(rmat, *eulers);
#endif
INT32_QUAT_OF_RMAT(*quat, rmat);
INT32_QUAT_WRAP_SHORTEST(*quat);
}
static inline void stabilization_attitude_read_rc_setpoint(bool_t in_flight) {
@@ -78,7 +66,8 @@ static inline void stabilization_attitude_read_rc_setpoint(bool_t in_flight) {
}
/* update quaternion setpoint from euler setpoint */
update_quat_from_eulers(&stab_att_sp_quat, &stab_att_sp_euler);
INT32_QUAT_OF_EULERS(stab_att_sp_quat, stab_att_sp_euler);
INT32_QUAT_WRAP_SHORTEST(stab_att_sp_quat);
}
sw/airborne/math/pprz_algebra_int.h
+10 -2
View File
@@ -837,9 +837,17 @@ struct Int64Vect3 {
\
}
#define INT32_EULERS_LSHIFT(_o, _i, _r) { \
(_o).phi = ((_i).phi << (_r)); \
(_o).theta = ((_i).theta << (_r)); \
(_o).psi = ((_i).psi << (_r)); \
}
#define INT32_EULERS_RSHIFT(_o, _i, _r) { \
(_o).phi = ((_i).phi >> (_r)); \
(_o).theta = ((_i).theta >> (_r)); \
(_o).psi = ((_i).psi >> (_r)); \
}
/*