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

[math] int32_sqrt returns unsigned int

Browse Source 
and takes unsigned int as arg
This commit is contained in:
Felix Ruess
2014-11-18 14:49:28 +01:00
parent 68e907947e
commit c03427b700
2 changed files with 8 additions and 8 deletions
Download Patch File Download Diff File Expand all files Collapse all files
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
+3 -3
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); }
@@ -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);
} }