Mirrors_Framework/paparazzi
1
0
Fork 0
mirror of https://github.com/paparazzi/paparazzi.git synced 2026-05-24 22:05:58 +08:00
Code Issues Actions 25 Packages Projects Releases Wiki Activity

[math] add double versions for vect3/quat norm

Browse Source
This commit is contained in:
Felix Ruess
2014-09-12 00:17:30 +02:00
parent 336defae49
commit b8349ce252
4 changed files with 46 additions and 14 deletions
Download Patch File Download Diff File Expand all files Collapse all files
sw/airborne/math/pprz_algebra_double.h
+33
View File
@@ -93,6 +93,39 @@ struct DoubleRates {
(_vout).z = rint((_vin).z); \
}
static inline double double_vect3_norm(struct DoubleVect3* v)
{
return sqrt(VECT3_NORM2(*v));
}
/** normalize 3D vector in place */
static inline void double_vect3_normalize(struct DoubleVect3* v)
{
const double n = double_vect3_norm(v);
if (n > 0) {
v->x /= n;
v->y /= n;
v->z /= n;
}
}
static inline double double_quat_norm(struct DoubleQuat* q)
{
return sqrt(SQUARE(q->qi) + SQUARE(q->qx) + SQUARE(q->qy) + SQUARE(q->qz));
}
static inline void double_quat_normalize(struct DoubleQuat* q)
{
double qnorm = double_quat_norm(q);
if (qnorm > FLT_MIN) {
q->qi = q->qi / qnorm;
q->qx = q->qx / qnorm;
q->qy = q->qy / qnorm;
q->qz = q->qz / qnorm;
}
}
extern void double_rmat_of_eulers_321(struct DoubleRMat* rm, struct DoubleEulers* e);
extern void double_quat_of_eulers(struct DoubleQuat* q, struct DoubleEulers* e);
extern void double_eulers_of_quat(struct DoubleEulers* e, struct DoubleQuat* q);
sw/airborne/modules/geo_mag/geo_mag.c
+8 -8
View File
@@ -34,15 +34,15 @@
#include "subsystems/ahrs.h"
bool_t geo_mag_calc_flag;
struct GeoMagVect geo_mag_vect;
struct GeoMag geo_mag;
void geo_mag_init(void) {
geo_mag_calc_flag = FALSE;
geo_mag_vect.ready = FALSE;
geo_mag.ready = FALSE;
}
void geo_mag_periodic(void) {
if (!geo_mag_vect.ready && gps.fix == GPS_FIX_3D && kill_throttle)
if (!geo_mag.ready && gps.fix == GPS_FIX_3D && kill_throttle)
geo_mag_calc_flag = TRUE;
}
@@ -66,19 +66,19 @@ void geo_mag_event(void) {
nmax = extrapsh(sdate, GEO_EPOCH, NMAX_1, NMAX_2, gha);
// Calculates absolute magnet fields
mag_calc(1, latitude, longitude, alt, nmax, gha,
&geo_mag_vect.x, &geo_mag_vect.y, &geo_mag_vect.z,
&geo_mag.vect.x, &geo_mag.vect.y, &geo_mag.vect.z,
IEXT, EXT_COEFF1, EXT_COEFF2, EXT_COEFF3);
FLOAT_VECT3_NORMALIZE(geo_mag_vect);
double_vect3_normalize(&geo_mag.vect);
// copy to ahrs
#ifdef AHRS_FLOAT
VECT3_COPY(ahrs_impl.mag_h, geo_mag_vect);
VECT3_COPY(ahrs_impl.mag_h, geo_mag.vect);
#else
// convert to MAG_BFP and copy to ahrs
VECT3_ASSIGN(ahrs_impl.mag_h, MAG_BFP_OF_REAL(geo_mag_vect.x), MAG_BFP_OF_REAL(geo_mag_vect.y), MAG_BFP_OF_REAL(geo_mag_vect.z));
VECT3_ASSIGN(ahrs_impl.mag_h, MAG_BFP_OF_REAL(geo_mag.vect.x), MAG_BFP_OF_REAL(geo_mag.vect.y), MAG_BFP_OF_REAL(geo_mag.vect.z));
#endif
geo_mag_vect.ready = TRUE;
geo_mag.ready = TRUE;
}
geo_mag_calc_flag = FALSE;
}
sw/airborne/modules/geo_mag/geo_mag.h
+4 -5
View File
@@ -29,11 +29,10 @@
#define GEO_MAG_H
#include "std.h"
#include "math/pprz_algebra_double.h"
struct GeoMagVect {
double x;
double y;
double z;
struct GeoMag {
struct DoubleVect3 vect;
bool_t ready;
};
@@ -41,6 +40,6 @@ extern void geo_mag_init(void);
extern void geo_mag_periodic(void);
extern void geo_mag_event(void);
extern struct GeoMagVect geo_mag_vect;
extern struct GeoMag geo_mag;
#endif
sw/simulator/nps/nps_fdm_jsbsim.cpp
+1 -1
View File
@@ -506,7 +506,7 @@ PRINT_CONFIG_MSG("Using WMM2010 model to calculate magnetic field at simulated l
mag_calc(1, latitude, longitude, alt, nmax, gha,
&fdm.ltp_h.x, &fdm.ltp_h.y, &fdm.ltp_h.z,
IEXT, EXT_COEFF1, EXT_COEFF2, EXT_COEFF3);
FLOAT_VECT3_NORMALIZE(fdm.ltp_h);
double_vect3_normalize(&fdm.ltp_h);
printf("normalized magnetic field: %.4f %.4f %.4f\n", fdm.ltp_h.x, fdm.ltp_h.y, fdm.ltp_h.z);
#endif