Merge pull request #494 'guidance_h_quat'

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.

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;
+        struct Int32Eulers sp_cmd_i;
-        sp_euler_i.phi = nav_roll;
+        sp_cmd_i.phi = nav_roll;
-        sp_euler_i.theta = nav_pitch;
+        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;
+        sp_cmd_i.psi = stateGetNedToBodyEulers_i()->psi;
-        stabilization_attitude_set_from_eulers_i(&sp_euler_i);
+        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 */
+  /* Set attitude setpoint from pseudo-euler commands */
-  stabilization_attitude_set_from_eulers_i(&guidance_h_command_body);
+  stabilization_attitude_set_cmd_i(&guidance_h_command_body);
 }
 
 static void guidance_h_hover_enter(void) {

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);
       }

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);
 
 

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) {
+void stabilization_attitude_set_cmd_i(struct Int32Eulers *sp_cmd) {
-  EULERS_FLOAT_OF_BFP(stab_att_sp_euler, *sp_euler);
+  EULERS_FLOAT_OF_BFP(stab_att_sp_euler, *sp_cmd);
 }
 
 

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) {
+void stabilization_attitude_set_cmd_i(struct Int32Eulers *sp_cmd) {
-  memcpy(&stab_att_sp_euler, sp_euler, sizeof(struct Int32Eulers));
+  memcpy(&stab_att_sp_euler, sp_cmd, sizeof(struct Int32Eulers));
 }
 
 

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) {
+void stabilization_attitude_set_cmd_i(struct Int32Eulers *sp_cmd) {
-  memcpy(&stab_att_sp_euler, sp_euler, sizeof(struct Int32Eulers));
+  memcpy(&stab_att_sp_euler, sp_cmd, sizeof(struct Int32Eulers));
 }
 

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) {
+void stabilization_attitude_set_cmd_i(struct Int32Eulers *sp_cmd) {
-  EULERS_FLOAT_OF_BFP(stab_att_sp_euler, *sp_euler);
+  EULERS_FLOAT_OF_BFP(stab_att_sp_euler, *sp_cmd);
-  FLOAT_QUAT_OF_EULERS(stab_att_sp_quat, stab_att_sp_euler);
+
+  /* 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

@@ -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));
+  memcpy(&stab_att_sp_euler, sp_cmd, sizeof(struct Int32Eulers));
-  INT32_QUAT_OF_EULERS(stab_att_sp_quat, *sp_euler);
+
-  INT32_QUAT_WRAP_SHORTEST(stab_att_sp_quat);
+  /// @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

@@ -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) {                                                       \