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 28 Packages Projects Releases Wiki Activity

Merge pull request #494 'guidance_h_quat'

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 and in cases of where heading_sp != current heading.
This commit is contained in:
Felix Ruess
2013-09-01 17:06:40 +02:00
parent 9489857f01 25609e0c5a
commit 8c8049722f
9 changed files with 152 additions and 21 deletions
Download Patch File Download Diff File Expand all files Collapse all files
sw/airborne/firmwares/rotorcraft/guidance/guidance_h.c
+8 -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);
@@ -267,6 +267,7 @@ void guidance_h_run(bool_t in_flight) {
/* set psi command */
guidance_h_command_body.psi = nav_heading;
INT32_ANGLE_NORMALIZE(guidance_h_command_body.psi);
/* compute roll and pitch commands and set final attitude setpoint */
guidance_h_traj_run(in_flight);
}
@@ -362,8 +363,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/guidance/guidance_v.c
+2
View File
@@ -217,6 +217,7 @@ void guidance_v_run(bool_t in_flight) {
if (fms.enabled && fms.input.v_mode == GUIDANCE_V_MODE_HOVER)
guidance_v_z_sp = fms.input.v_sp.height;
#endif
guidance_v_zd_sp = 0;
gv_update_ref_from_z_sp(guidance_v_z_sp);
run_hover_loop(in_flight);
#if NO_RC_THRUST_LIMIT
@@ -231,6 +232,7 @@ void guidance_v_run(bool_t in_flight) {
{
if (vertical_mode == VERTICAL_MODE_ALT) {
guidance_v_z_sp = -nav_flight_altitude;
guidance_v_zd_sp = 0;
gv_update_ref_from_z_sp(guidance_v_z_sp);
run_hover_loop(in_flight);
}
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
+55 -3
View File
@@ -142,9 +142,61 @@ 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);
/// @todo optimize yaw angle calculation
/*
* Instead of using the psi setpoint angle to rotate around the body z-axis,
* calculate the real angle needed to align the projection of the body x-axis
* onto the horizontal plane with the psi setpoint.
*
* angle between two vectors a and b:
* angle = atan2(norm(cross(a,b)), dot(a,b))
*/
const struct FloatVect3 xaxis = {1.0, 0.0, 0.0};
const struct FloatVect3 zaxis = {0.0, 0.0, 1.0};
struct FloatVect3 a;
FLOAT_QUAT_VMULT(a, q_rp, xaxis);
// desired heading vect in earth x-y plane
struct FloatVect3 psi_vect;
psi_vect.x = cosf(stab_att_sp_euler.psi);
psi_vect.y = sinf(stab_att_sp_euler.psi);
psi_vect.z = 0.0;
struct FloatVect3 normal;
FLOAT_QUAT_VMULT(normal, q_rp, zaxis);
// projection of desired heading onto body x-y plane
// b = v - dot(v,n)*n
float dot = FLOAT_VECT3_DOT_PRODUCT(psi_vect, normal);
struct FloatVect3 dotn;
FLOAT_VECT3_SMUL(dotn, normal, dot);
struct FloatVect3 b;
FLOAT_VECT3_DIFF(b, psi_vect, dotn);
dot = FLOAT_VECT3_DOT_PRODUCT(a, b);
struct FloatVect3 cross;
VECT3_CROSS_PRODUCT(cross, a, b);
// norm of the cross product
float nc = FLOAT_VECT3_NORM(cross);
float yaw2 = atan2(nc, dot) / 2.0;
/* quaternion with yaw command */
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
+64 -4
View File
@@ -92,11 +92,71 @@ 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);
const float psi_sp = ANGLE_FLOAT_OF_BFP(sp_cmd->psi);
/// @todo optimize yaw angle calculation
/*
* Instead of using the psi setpoint angle to rotate around the body z-axis,
* calculate the real angle needed to align the projection of the body x-axis
* onto the horizontal plane with the psi setpoint.
*
* angle between two vectors a and b:
* angle = atan2(norm(cross(a,b)), dot(a,b))
*/
const struct FloatVect3 xaxis = {1.0, 0.0, 0.0};
const struct FloatVect3 zaxis = {0.0, 0.0, 1.0};
struct FloatVect3 a;
FLOAT_QUAT_VMULT(a, q_rp, xaxis);
// desired heading vect in earth x-y plane
struct FloatVect3 psi_vect;
psi_vect.x = cosf(psi_sp);
psi_vect.y = sinf(psi_sp);
psi_vect.z = 0.0;
struct FloatVect3 normal;
FLOAT_QUAT_VMULT(normal, q_rp, zaxis);
// projection of desired heading onto body x-y plane
// b = v - dot(v,n)*n
float dot = FLOAT_VECT3_DOT_PRODUCT(psi_vect, normal);
struct FloatVect3 dotn;
FLOAT_VECT3_SMUL(dotn, normal, dot);
struct FloatVect3 b;
FLOAT_VECT3_DIFF(b, psi_vect, dotn);
dot = FLOAT_VECT3_DOT_PRODUCT(a, b);
struct FloatVect3 cross;
VECT3_CROSS_PRODUCT(cross, a, b);
// norm of the cross product
float nc = FLOAT_VECT3_NORM(cross);
float yaw2 = atan2(nc, dot) / 2.0;
/* quaternion with yaw command */
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))
sw/airborne/math/pprz_algebra_float.h
+16
View File
@@ -647,6 +647,22 @@ static inline float float_rmat_reorthogonalize(struct FloatRMat* rm) {
(_q).qz = san * (_uv).z; \
}
#define FLOAT_QUAT_OF_ORIENTATION_VECT(_q, _ov) { \
const float ov_norm = sqrtf((_ov).x*(_ov).x + (_ov).y*(_ov).y + (_ov).z*(_ov).z); \
if (ov_norm < 1e-8) { \
(_q).qi = 1; \
(_q).qx = 0; \
(_q).qy = 0; \
(_q).qz = 0; \
} else { \
const float s2_normalized = sinf(ov_norm/2.0) / ov_norm; \
(_q).qi = cosf(ov_norm/2.0); \
(_q).qx = (_ov).x * s2_normalized; \
(_q).qy = (_ov).y * s2_normalized; \
(_q).qz = (_ov).z * s2_normalized; \
} \
}
#define FLOAT_QUAT_OF_RMAT(_q, _r) { \
const float tr = RMAT_TRACE((_r)); \
if (tr > 0) { \