diff --git a/sw/airborne/firmwares/rotorcraft/navigation.c b/sw/airborne/firmwares/rotorcraft/navigation.c index b99afe568a..2dee31333d 100644 --- a/sw/airborne/firmwares/rotorcraft/navigation.c +++ b/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; diff --git a/sw/airborne/firmwares/rotorcraft/navigation.h b/sw/airborne/firmwares/rotorcraft/navigation.h index 4aa00f5906..89f74dffa0 100644 --- a/sw/airborne/firmwares/rotorcraft/navigation.h +++ b/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 diff --git a/sw/airborne/math/pprz_algebra_int.c b/sw/airborne/math/pprz_algebra_int.c index 1289a1cc71..abe8830def 100644 --- a/sw/airborne/math/pprz_algebra_int.c +++ b/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)) diff --git a/sw/airborne/math/pprz_algebra_int.h b/sw/airborne/math/pprz_algebra_int.h index 837ac2b73e..f24c55c1bc 100644 --- a/sw/airborne/math/pprz_algebra_int.h +++ b/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); } diff --git a/sw/airborne/state.c b/sw/airborne/state.c index 9a32002cb6..cb80491c98 100644 --- a/sw/airborne/state.c +++ b/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)) { diff --git a/sw/airborne/state.h b/sw/airborne/state.h index faea93c479..f6c1273f42 100644 --- a/sw/airborne/state.h +++ b/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;