Mirrors_Framework/paparazzi
1
0
Fork 0
mirror of https://github.com/paparazzi/paparazzi.git synced 2026-05-27 08:55:51 +08:00
Code Issues Actions 28 Packages Projects Releases Wiki Activity

Merge pull request #956 from paparazzi/algebra_int_unsigned

Browse Source 
[math] some cleanup and fixes for fixedpoint functions

Tested by @fvantienen and @flixr
This commit is contained in:
Gautier Hattenberger
2014-11-19 12:57:13 +01:00
parent 9aee8b3777 a99d6662d6
commit c319d770a4
6 changed files with 23 additions and 23 deletions
Download Patch File Download Diff File Expand all files Collapse all files
sw/airborne/firmwares/rotorcraft/navigation.c
+3 -3
View File
@@ -77,7 +77,7 @@ struct EnuCoor_i nav_circle_center;
int32_t nav_circle_radius, nav_circle_qdr, nav_circle_radians; int32_t nav_circle_radius, nav_circle_qdr, nav_circle_radians;
int32_t nav_leg_progress; int32_t nav_leg_progress;
int32_t nav_leg_length; uint32_t nav_leg_length;
int32_t nav_roll, nav_pitch; int32_t nav_roll, nav_pitch;
int32_t nav_heading; 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); VECT2_COPY(wp_diff_prec, wp_diff);
INT32_VECT2_RSHIFT(wp_diff,wp_diff,INT32_POS_FRAC); INT32_VECT2_RSHIFT(wp_diff,wp_diff,INT32_POS_FRAC);
INT32_VECT2_RSHIFT(pos_diff,pos_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); uint32_t leg_length2 = Max((wp_diff.x * wp_diff.x + wp_diff.y * wp_diff.y),1);
INT32_SQRT(nav_leg_length,leg_length2); 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; 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); int32_t progress = Max((CARROT_DIST >> INT32_POS_FRAC), 0);
nav_leg_progress += progress; nav_leg_progress += progress;
sw/airborne/firmwares/rotorcraft/navigation.h
+1 -1
View File
@@ -60,7 +60,7 @@ extern int32_t nav_heading; ///< with #INT32_ANGLE_FRAC
extern float nav_radius; extern float nav_radius;
extern int32_t nav_leg_progress; extern int32_t nav_leg_progress;
extern int32_t nav_leg_length; extern uint32_t nav_leg_length;
extern uint8_t vertical_mode; extern uint8_t vertical_mode;
extern uint32_t nav_throttle; ///< direct throttle from 0:MAX_PPRZ, used in VERTICAL_MODE_MANUAL extern uint32_t nav_throttle; ///< direct throttle from 0:MAX_PPRZ, used in VERTICAL_MODE_MANUAL
sw/airborne/math/pprz_algebra_int.c
+5 -5
View File
@@ -27,7 +27,7 @@
#include "pprz_algebra_int.h" #include "pprz_algebra_int.h"
#define INT32_SQRT_MAX_ITER 40 #define INT32_SQRT_MAX_ITER 40
int32_t int32_sqrt(int32_t in) uint32_t int32_sqrt(uint32_t in)
{ {
if (in == 0) { if (in == 0) {
return 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); const int32_t tr = RMAT_TRACE(*r);
if (tr > 0) { if (tr > 0) {
const int32_t two_qi_two = TRIG_BFP_OF_REAL(1.) + tr; 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); two_qi = two_qi << (INT32_QUAT_FRAC - INT32_TRIG_FRAC);
q->qi = two_qi / 2; q->qi = two_qi / 2;
q->qx = ((RMAT_ELMT(*r, 1, 2) - RMAT_ELMT(*r, 2, 1)) << 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)) { 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) 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.); - 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); two_qx = two_qx << (INT32_QUAT_FRAC - INT32_TRIG_FRAC);
q->qi = ((RMAT_ELMT(*r, 1, 2) - RMAT_ELMT(*r, 2, 1)) << q->qi = ((RMAT_ELMT(*r, 1, 2) - RMAT_ELMT(*r, 2, 1)) <<
(INT32_QUAT_FRAC - INT32_TRIG_FRAC + INT32_QUAT_FRAC - 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)) { } 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) 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.); - 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); two_qy = two_qy << (INT32_QUAT_FRAC - INT32_TRIG_FRAC);
q->qi = ((RMAT_ELMT(*r, 2, 0) - RMAT_ELMT(*r, 0, 2)) << q->qi = ((RMAT_ELMT(*r, 2, 0) - RMAT_ELMT(*r, 0, 2)) <<
(INT32_QUAT_FRAC - INT32_TRIG_FRAC + INT32_QUAT_FRAC - 1)) (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 { } else {
const int32_t two_qz_two = RMAT_ELMT(*r, 2, 2) - RMAT_ELMT(*r, 0, 0) 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.); - 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); two_qz = two_qz << (INT32_QUAT_FRAC - INT32_TRIG_FRAC);
q->qi = ((RMAT_ELMT(*r, 0, 1) - RMAT_ELMT(*r, 1, 0)) << q->qi = ((RMAT_ELMT(*r, 0, 1) - RMAT_ELMT(*r, 1, 0)) <<
(INT32_QUAT_FRAC - INT32_TRIG_FRAC + INT32_QUAT_FRAC - 1)) (INT32_QUAT_FRAC - INT32_TRIG_FRAC + INT32_QUAT_FRAC - 1))
sw/airborne/math/pprz_algebra_int.h
+6 -6
View File
@@ -221,7 +221,7 @@ struct Int64Vect3 {
#define INT_MULT_RSHIFT(_a, _b, _r) (((_a)*(_b))>>(_r)) #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); } #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 */ /** normalize 2D vector inplace */
static inline void int32_vect2_normalize(struct Int32Vect2* v, uint8_t frac) 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); const uint32_t n = int32_vect2_norm(v);
if (n > 0) { if (n > 0) {
v->x = v->x * f / n; const int32_t f = BFP_OF_REAL((1.), frac);
v->y = v->y * f / n; 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. /** 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); return int32_sqrt(n2);
} }
sw/airborne/state.c
+6 -6
View File
@@ -762,8 +762,8 @@ void stateCalcHorizontalSpeedNorm_i(void) {
state.h_speed_norm_i = SPEED_BFP_OF_REAL(state.h_speed_norm_f); 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)) { else if (bit_is_set(state.speed_status, SPEED_NED_I)) {
int32_t n2 = (state.ned_speed_i.x*state.ned_speed_i.x + 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; state.ned_speed_i.y*state.ned_speed_i.y) >> INT32_SPEED_FRAC;
INT32_SQRT(state.h_speed_norm_i, n2); INT32_SQRT(state.h_speed_norm_i, n2);
} }
else if (bit_is_set(state.speed_status, SPEED_NED_F)) { 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); 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)) { else if (bit_is_set(state.speed_status, SPEED_ENU_I)) {
int32_t n2 = (state.enu_speed_i.x*state.enu_speed_i.x + 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; state.enu_speed_i.y*state.enu_speed_i.y) >> INT32_SPEED_FRAC;
INT32_SQRT(state.h_speed_norm_i, n2); INT32_SQRT(state.h_speed_norm_i, n2);
} }
else if (bit_is_set(state.speed_status, SPEED_ENU_F)) { 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 */ /* 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); ned_of_ecef_vect_i(&state.ned_speed_i, &state.ned_origin_i, &state.ecef_speed_i);
SetBit(state.speed_status, SPEED_NED_I); SetBit(state.speed_status, SPEED_NED_I);
int32_t n2 = (state.ned_speed_i.x*state.ned_speed_i.x + 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; state.ned_speed_i.y*state.ned_speed_i.y) >> INT32_SPEED_FRAC;
INT32_SQRT(state.h_speed_norm_i, n2); INT32_SQRT(state.h_speed_norm_i, n2);
} }
else if (bit_is_set(state.speed_status, SPEED_ECEF_F)) { else if (bit_is_set(state.speed_status, SPEED_ECEF_F)) {
sw/airborne/state.h
+2 -2
View File
@@ -277,7 +277,7 @@ struct State {
* Norm of horizontal ground speed. * Norm of horizontal ground speed.
* Unit: m/s in BFP with #INT32_SPEED_FRAC * 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. * Direction of horizontal ground speed.
@@ -816,7 +816,7 @@ static inline struct EcefCoor_i* stateGetSpeedEcef_i(void) {
} }
/// Get norm of horizontal ground speed (int). /// 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)) if (!bit_is_set(state.speed_status, SPEED_HNORM_I))
stateCalcHorizontalSpeedNorm_i(); stateCalcHorizontalSpeedNorm_i();
return &state.h_speed_norm_i; return &state.h_speed_norm_i;