diff --git a/sw/airborne/firmwares/rotorcraft/guidance/guidance_h.c b/sw/airborne/firmwares/rotorcraft/guidance/guidance_h.c index 39995aad2f..1b5cdbf0b2 100644 --- a/sw/airborne/firmwares/rotorcraft/guidance/guidance_h.c +++ b/sw/airborne/firmwares/rotorcraft/guidance/guidance_h.c @@ -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) { diff --git a/sw/airborne/firmwares/rotorcraft/guidance/guidance_v.c b/sw/airborne/firmwares/rotorcraft/guidance/guidance_v.c index 432f655f1b..81c72e79bf 100644 --- a/sw/airborne/firmwares/rotorcraft/guidance/guidance_v.c +++ b/sw/airborne/firmwares/rotorcraft/guidance/guidance_v.c @@ -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); } diff --git a/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude.h b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude.h index 7a0572f69e..94b47466e4 100644 --- a/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude.h +++ b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude.h @@ -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); diff --git a/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_euler_float.c b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_euler_float.c index b2f9f24178..3a5df19417 100644 --- a/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_euler_float.c +++ b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_euler_float.c @@ -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); } diff --git a/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_euler_int.c b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_euler_int.c index f435e97a1f..a86f9c4c80 100644 --- a/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_euler_int.c +++ b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_euler_int.c @@ -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)); } diff --git a/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_passthrough.c b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_passthrough.c index 69f7004aca..5ddcb0e7c3 100644 --- a/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_passthrough.c +++ b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_passthrough.c @@ -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)); } diff --git a/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_quat_float.c b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_quat_float.c index e546692a87..d6c83774b6 100644 --- a/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_quat_float.c +++ b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_quat_float.c @@ -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); } diff --git a/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_quat_int.c b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_quat_int.c index 1bff46cd25..b0b5b3ae6e 100644 --- a/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_quat_int.c +++ b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_quat_int.c @@ -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)) diff --git a/sw/airborne/math/pprz_algebra_float.h b/sw/airborne/math/pprz_algebra_float.h index 413a1c06fc..799087f1f6 100644 --- a/sw/airborne/math/pprz_algebra_float.h +++ b/sw/airborne/math/pprz_algebra_float.h @@ -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) { \