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

[rotorcraft][guidance][stabilization] quat setpoint fixes

Browse Source 
Don't pretend that the commands from guidance_h are actually real euler angles.
Compose a roll/pitch quaternion from simultaneous rotation of roll/pitch,
then rotate around resulting body z-axis to align the heading.

This should "fix" the setpoint passed to the attitude stabilization if in HOVER or NAV
at large angles.
Only tested quickly in simulation...
This commit is contained in:
Felix Ruess
2013-08-07 13:46:08 +02:00
parent 3951bd7f27
commit 104acd2767
7 changed files with 59 additions and 21 deletions
Download Patch File Download Diff File Expand all files Collapse all files
sw/airborne/firmwares/rotorcraft/guidance/guidance_h.c
+7 -7
View File
@@ -253,12 +253,12 @@ void guidance_h_run(bool_t in_flight) {
guidance_h_nav_enter();
if (horizontal_mode == HORIZONTAL_MODE_ATTITUDE) {
struct Int32Eulers sp_euler_i;
sp_euler_i.phi = nav_roll;
sp_euler_i.theta = nav_pitch;
struct Int32Eulers sp_cmd_i;
sp_cmd_i.phi = nav_roll;
sp_cmd_i.theta = nav_pitch;
/* FIXME: heading can't be set via attitude block yet, use current heading for now */
sp_euler_i.psi = stateGetNedToBodyEulers_i()->psi;
stabilization_attitude_set_from_eulers_i(&sp_euler_i);
sp_cmd_i.psi = stateGetNedToBodyEulers_i()->psi;
stabilization_attitude_set_cmd_i(&sp_cmd_i);
}
else {
INT32_VECT2_NED_OF_ENU(guidance_h_pos_sp, navigation_carrot);
@@ -362,8 +362,8 @@ static void guidance_h_traj_run(bool_t in_flight) {
guidance_h_command_body.phi += guidance_h_rc_sp.phi;
guidance_h_command_body.theta += guidance_h_rc_sp.theta;
/* Set attitude setpoint from pseudo-eulers */
stabilization_attitude_set_from_eulers_i(&guidance_h_command_body);
/* Set attitude setpoint from pseudo-euler commands */
stabilization_attitude_set_cmd_i(&guidance_h_command_body);
}
static void guidance_h_hover_enter(void) {
sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude.h
+1 -1
View File
@@ -34,7 +34,7 @@ extern void stabilization_attitude_init(void);
extern void stabilization_attitude_read_rc(bool_t in_flight);
extern void stabilization_attitude_enter(void);
extern void stabilization_attitude_set_failsafe_setpoint(void);
extern void stabilization_attitude_set_from_eulers_i(struct Int32Eulers *sp_euler);
extern void stabilization_attitude_set_cmd_i(struct Int32Eulers *sp_cmd);
extern void stabilization_attitude_run(bool_t in_flight);
sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_euler_float.c
+2 -2
View File
@@ -86,8 +86,8 @@ void stabilization_attitude_set_failsafe_setpoint(void) {
stab_att_sp_euler.psi = stateGetNedToBodyEulers_f()->psi;
}
void stabilization_attitude_set_from_eulers_i(struct Int32Eulers *sp_euler) {
EULERS_FLOAT_OF_BFP(stab_att_sp_euler, *sp_euler);
void stabilization_attitude_set_cmd_i(struct Int32Eulers *sp_cmd) {
EULERS_FLOAT_OF_BFP(stab_att_sp_euler, *sp_cmd);
}
sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_euler_int.c
+2 -2
View File
@@ -102,8 +102,8 @@ void stabilization_attitude_set_failsafe_setpoint(void) {
stab_att_sp_euler.psi = stateGetNedToBodyEulers_i()->psi;
}
void stabilization_attitude_set_from_eulers_i(struct Int32Eulers *sp_euler) {
memcpy(&stab_att_sp_euler, sp_euler, sizeof(struct Int32Eulers));
void stabilization_attitude_set_cmd_i(struct Int32Eulers *sp_cmd) {
memcpy(&stab_att_sp_euler, sp_cmd, sizeof(struct Int32Eulers));
}
sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_passthrough.c
+2 -2
View File
@@ -78,7 +78,7 @@ void stabilization_attitude_set_failsafe_setpoint(void) {
stab_att_sp_euler.psi = stateGetNedToBodyEulers_i()->psi;
}
void stabilization_attitude_set_from_eulers_i(struct Int32Eulers *sp_euler) {
memcpy(&stab_att_sp_euler, sp_euler, sizeof(struct Int32Eulers));
void stabilization_attitude_set_cmd_i(struct Int32Eulers *sp_cmd) {
memcpy(&stab_att_sp_euler, sp_cmd, sizeof(struct Int32Eulers));
}
sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_quat_float.c
+19 -3
View File
@@ -142,9 +142,25 @@ void stabilization_attitude_set_failsafe_setpoint(void) {
stab_att_sp_quat.qz = sinf(heading2);
}
void stabilization_attitude_set_from_eulers_i(struct Int32Eulers *sp_euler) {
EULERS_FLOAT_OF_BFP(stab_att_sp_euler, *sp_euler);
FLOAT_QUAT_OF_EULERS(stab_att_sp_quat, stab_att_sp_euler);
void stabilization_attitude_set_cmd_i(struct Int32Eulers *sp_cmd) {
EULERS_FLOAT_OF_BFP(stab_att_sp_euler, *sp_cmd);
/* orientation vector describing simultaneous rotation of roll/pitch */
struct FloatVect3 ov;
ov.x = stab_att_sp_euler.phi;
ov.y = stab_att_sp_euler.theta;
ov.z = 0.0;
/* quaternion from that orientation vector */
struct FloatQuat q_rp;
FLOAT_QUAT_OF_ORIENTATION_VECT(q_rp, ov);
/* quaternion with only heading setpoint */
const float yaw2 = stab_att_sp_euler.psi / 2.0;
struct FloatQuat q_yaw;
QUAT_ASSIGN(q_yaw, cosf(yaw2), 0.0, 0.0, sinf(yaw2));
/* final setpoint: apply roll/pitch, then yaw around resulting body z-axis */
FLOAT_QUAT_COMP(stab_att_sp_quat, q_yaw, q_rp);
FLOAT_QUAT_WRAP_SHORTEST(stab_att_sp_quat);
}
sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_quat_int.c
+26 -4
View File
@@ -92,11 +92,33 @@ void stabilization_attitude_set_failsafe_setpoint(void) {
PPRZ_ITRIG_SIN(stab_att_sp_quat.qz, heading2);
}
void stabilization_attitude_set_from_eulers_i(struct Int32Eulers *sp_euler) {
void stabilization_attitude_set_cmd_i(struct Int32Eulers *sp_cmd) {
// copy euler setpoint for debugging
memcpy(&stab_att_sp_euler, sp_euler, sizeof(struct Int32Eulers));
INT32_QUAT_OF_EULERS(stab_att_sp_quat, *sp_euler);
INT32_QUAT_WRAP_SHORTEST(stab_att_sp_quat);
memcpy(&stab_att_sp_euler, sp_cmd, sizeof(struct Int32Eulers));
/// @todo calc sp_quat in fixed-point
/* orientation vector describing simultaneous rotation of roll/pitch */
struct FloatVect3 ov;
ov.x = ANGLE_FLOAT_OF_BFP(sp_cmd->phi);
ov.y = ANGLE_FLOAT_OF_BFP(sp_cmd->theta);
ov.z = 0.0;
/* quaternion from that orientation vector */
struct FloatQuat q_rp;
FLOAT_QUAT_OF_ORIENTATION_VECT(q_rp, ov);
/* quaternion with only heading setpoint */
const float yaw2 = ANGLE_FLOAT_OF_BFP(sp_cmd->psi) / 2.0;
struct FloatQuat q_yaw;
QUAT_ASSIGN(q_yaw, cosf(yaw2), 0.0, 0.0, sinf(yaw2));
/* final setpoint: apply roll/pitch, then yaw around resulting body z-axis */
struct FloatQuat q_sp;
FLOAT_QUAT_COMP(q_sp, q_yaw, q_rp);
FLOAT_QUAT_WRAP_SHORTEST(q_sp);
/* convert to fixed point */
QUAT_BFP_OF_REAL(stab_att_sp_quat, q_sp);
}
#define OFFSET_AND_ROUND(_a, _b) (((_a)+(1<<((_b)-1)))>>(_b))