Merge pull request paparazzi#956 from paparazzi/algebra_int_unsigned

[math] some cleanup and fixes for fixedpoint functions

Tested by @fvantienen and @flixr

sw/airborne/firmwares/rotorcraft/navigation.c

@@ -77,7 +77,7 @@ struct EnuCoor_i nav_circle_center;
 int32_t nav_circle_radius, nav_circle_qdr, nav_circle_radians;
 
 int32_t nav_leg_progress;
-int32_t nav_leg_length;
+uint32_t nav_leg_length;
 
 int32_t nav_roll, nav_pitch;
 int32_t nav_heading;
@@ -269,8 +269,8 @@ void nav_route(struct EnuCoor_i * wp_start, struct EnuCoor_i * wp_end) {
   VECT2_COPY(wp_diff_prec, wp_diff);
   INT32_VECT2_RSHIFT(wp_diff,wp_diff,INT32_POS_FRAC);
   INT32_VECT2_RSHIFT(pos_diff,pos_diff,INT32_POS_FRAC);
-  int32_t leg_length2 = Max((wp_diff.x * wp_diff.x + wp_diff.y * wp_diff.y),1);
-  INT32_SQRT(nav_leg_length,leg_length2);
+  uint32_t leg_length2 = Max((wp_diff.x * wp_diff.x + wp_diff.y * wp_diff.y),1);
+  nav_leg_length = int32_sqrt(leg_length2);
   nav_leg_progress = (pos_diff.x * wp_diff.x + pos_diff.y * wp_diff.y) / nav_leg_length;
   int32_t progress = Max((CARROT_DIST >> INT32_POS_FRAC), 0);
   nav_leg_progress += progress;

sw/airborne/firmwares/rotorcraft/navigation.h

@@ -60,7 +60,7 @@ extern int32_t nav_heading; ///< with #INT32_ANGLE_FRAC
 extern float nav_radius;
 
 extern int32_t nav_leg_progress;
-extern int32_t nav_leg_length;
+extern uint32_t nav_leg_length;
 
 extern uint8_t vertical_mode;
 extern uint32_t nav_throttle;  ///< direct throttle from 0:MAX_PPRZ, used in VERTICAL_MODE_MANUAL

sw/airborne/math/pprz_algebra_int.c

@@ -27,7 +27,7 @@
 #include "pprz_algebra_int.h"
 
 #define INT32_SQRT_MAX_ITER 40
-int32_t int32_sqrt(int32_t in)
+uint32_t int32_sqrt(uint32_t in)
 {
   if (in == 0) {
     return 0;
@@ -407,7 +407,7 @@ void int32_quat_of_rmat(struct Int32Quat* q, struct Int32RMat* r)
   const int32_t tr = RMAT_TRACE(*r);
   if (tr > 0) {
     const int32_t two_qi_two = TRIG_BFP_OF_REAL(1.) + tr;
-    int32_t two_qi = int32_sqrt(two_qi_two << INT32_TRIG_FRAC);
+    uint32_t two_qi = int32_sqrt(two_qi_two << INT32_TRIG_FRAC);
     two_qi = two_qi << (INT32_QUAT_FRAC - INT32_TRIG_FRAC);
     q->qi = two_qi / 2;
     q->qx = ((RMAT_ELMT(*r, 1, 2) - RMAT_ELMT(*r, 2, 1)) <<
@@ -424,7 +424,7 @@ void int32_quat_of_rmat(struct Int32Quat* q, struct Int32RMat* r)
         RMAT_ELMT(*r, 0, 0) > RMAT_ELMT(*r, 2, 2)) {
       const int32_t two_qx_two = RMAT_ELMT(*r, 0, 0) - RMAT_ELMT(*r, 1, 1)
                                  - RMAT_ELMT(*r, 2, 2) + TRIG_BFP_OF_REAL(1.);
-      int32_t two_qx = int32_sqrt(two_qx_two << INT32_TRIG_FRAC);
+      uint32_t two_qx = int32_sqrt(two_qx_two << INT32_TRIG_FRAC);
       two_qx = two_qx << (INT32_QUAT_FRAC - INT32_TRIG_FRAC);
       q->qi = ((RMAT_ELMT(*r, 1, 2) - RMAT_ELMT(*r, 2, 1)) <<
                (INT32_QUAT_FRAC - INT32_TRIG_FRAC + INT32_QUAT_FRAC - 1))
@@ -439,7 +439,7 @@ void int32_quat_of_rmat(struct Int32Quat* q, struct Int32RMat* r)
     } else if (RMAT_ELMT(*r, 1, 1) > RMAT_ELMT(*r, 2, 2)) {
       const int32_t two_qy_two = RMAT_ELMT(*r, 1, 1) - RMAT_ELMT(*r, 0, 0)
                                  - RMAT_ELMT(*r, 2, 2) + TRIG_BFP_OF_REAL(1.);
-      int32_t two_qy = int32_sqrt(two_qy_two << INT32_TRIG_FRAC);
+      uint32_t two_qy = int32_sqrt(two_qy_two << INT32_TRIG_FRAC);
       two_qy = two_qy << (INT32_QUAT_FRAC - INT32_TRIG_FRAC);
       q->qi = ((RMAT_ELMT(*r, 2, 0) - RMAT_ELMT(*r, 0, 2)) <<
                (INT32_QUAT_FRAC - INT32_TRIG_FRAC + INT32_QUAT_FRAC - 1))
@@ -454,7 +454,7 @@ void int32_quat_of_rmat(struct Int32Quat* q, struct Int32RMat* r)
     } else {
       const int32_t two_qz_two = RMAT_ELMT(*r, 2, 2) - RMAT_ELMT(*r, 0, 0)
                                  - RMAT_ELMT(*r, 1, 1) + TRIG_BFP_OF_REAL(1.);
-      int32_t two_qz = int32_sqrt(two_qz_two << INT32_TRIG_FRAC);
+      uint32_t two_qz = int32_sqrt(two_qz_two << INT32_TRIG_FRAC);
       two_qz = two_qz << (INT32_QUAT_FRAC - INT32_TRIG_FRAC);
       q->qi = ((RMAT_ELMT(*r, 0, 1) - RMAT_ELMT(*r, 1, 0)) <<
                (INT32_QUAT_FRAC - INT32_TRIG_FRAC + INT32_QUAT_FRAC - 1))

sw/airborne/math/pprz_algebra_int.h

@@ -221,7 +221,7 @@ struct Int64Vect3 {
 #define INT_MULT_RSHIFT(_a, _b, _r) (((_a)*(_b))>>(_r))
 
 
-extern int32_t int32_sqrt(int32_t in);
+extern uint32_t int32_sqrt(uint32_t in);
 #define INT32_SQRT(_out,_in) { _out = int32_sqrt(_in); }
 
 
@@ -249,11 +249,11 @@ static inline uint32_t int32_vect2_norm(struct Int32Vect2* v)
 /** normalize 2D vector inplace */
 static inline void int32_vect2_normalize(struct Int32Vect2* v, uint8_t frac)
 {
-  const uint32_t f = BFP_OF_REAL((1.), frac);
   const uint32_t n = int32_vect2_norm(v);
   if (n > 0) {
-    v->x = v->x * f / n;
-    v->y = v->y * f / n;
+    const int32_t f = BFP_OF_REAL((1.), frac);
+    v->x = v->x * f / (int32_t)n;
+    v->y = v->y * f / (int32_t)n;
   }
 }
 
@@ -433,9 +433,9 @@ static inline void int32_quat_identity(struct Int32Quat* q)
 
 /** Norm of a quaternion.
  */
-static inline int32_t int32_quat_norm(struct Int32Quat* q)
+static inline uint32_t int32_quat_norm(struct Int32Quat* q)
 {
-  int32_t n2 = q->qi * q->qi + q->qx * q->qx + q->qy * q->qy + q->qz * q->qz;
+  uint32_t n2 = q->qi * q->qi + q->qx * q->qx + q->qy * q->qy + q->qz * q->qz;
   return int32_sqrt(n2);
 }
 

sw/airborne/state.c

@@ -762,8 +762,8 @@ void stateCalcHorizontalSpeedNorm_i(void) {
     state.h_speed_norm_i = SPEED_BFP_OF_REAL(state.h_speed_norm_f);
   }
   else if (bit_is_set(state.speed_status, SPEED_NED_I)) {
-    int32_t n2 = (state.ned_speed_i.x*state.ned_speed_i.x +
-                  state.ned_speed_i.y*state.ned_speed_i.y) >> INT32_SPEED_FRAC;
+    uint32_t n2 = (state.ned_speed_i.x*state.ned_speed_i.x +
+                   state.ned_speed_i.y*state.ned_speed_i.y) >> INT32_SPEED_FRAC;
     INT32_SQRT(state.h_speed_norm_i, n2);
   }
   else if (bit_is_set(state.speed_status, SPEED_NED_F)) {
@@ -772,8 +772,8 @@ void stateCalcHorizontalSpeedNorm_i(void) {
     state.h_speed_norm_i = SPEED_BFP_OF_REAL(state.h_speed_norm_f);
   }
   else if (bit_is_set(state.speed_status, SPEED_ENU_I)) {
-    int32_t n2 = (state.enu_speed_i.x*state.enu_speed_i.x +
-                  state.enu_speed_i.y*state.enu_speed_i.y) >> INT32_SPEED_FRAC;
+    uint32_t n2 = (state.enu_speed_i.x*state.enu_speed_i.x +
+                   state.enu_speed_i.y*state.enu_speed_i.y) >> INT32_SPEED_FRAC;
     INT32_SQRT(state.h_speed_norm_i, n2);
   }
   else if (bit_is_set(state.speed_status, SPEED_ENU_F)) {
@@ -785,8 +785,8 @@ void stateCalcHorizontalSpeedNorm_i(void) {
     /* transform ecef speed to ned, set status bit, then compute norm */
     ned_of_ecef_vect_i(&state.ned_speed_i, &state.ned_origin_i, &state.ecef_speed_i);
     SetBit(state.speed_status, SPEED_NED_I);
-    int32_t n2 = (state.ned_speed_i.x*state.ned_speed_i.x +
-                  state.ned_speed_i.y*state.ned_speed_i.y) >> INT32_SPEED_FRAC;
+    uint32_t n2 = (state.ned_speed_i.x*state.ned_speed_i.x +
+                   state.ned_speed_i.y*state.ned_speed_i.y) >> INT32_SPEED_FRAC;
     INT32_SQRT(state.h_speed_norm_i, n2);
   }
   else if (bit_is_set(state.speed_status, SPEED_ECEF_F)) {

sw/airborne/state.h

@@ -277,7 +277,7 @@ struct State {
    * Norm of horizontal ground speed.
    * Unit: m/s in BFP with #INT32_SPEED_FRAC
    */
-  int32_t h_speed_norm_i;
+  uint32_t h_speed_norm_i;
 
   /**
    * Direction of horizontal ground speed.
@@ -816,7 +816,7 @@ static inline struct EcefCoor_i* stateGetSpeedEcef_i(void) {
 }
 
 /// Get norm of horizontal ground speed (int).
-static inline int32_t* stateGetHorizontalSpeedNorm_i(void) {
+static inline uint32_t* stateGetHorizontalSpeedNorm_i(void) {
   if (!bit_is_set(state.speed_status, SPEED_HNORM_I))
     stateCalcHorizontalSpeedNorm_i();
   return &state.h_speed_norm_i;