Mirrors_Framework/paparazzi
1
0
Fork 0
mirror of https://github.com/paparazzi/paparazzi.git synced 2026-06-07 00:53:41 +08:00
Code Issues Actions 25 Packages Projects Releases Wiki Activity

standardized the VECT[23]_(SMUL|SDIV) math macros with scalar always as last arg

Browse Source
This commit is contained in:
Felix Ruess
2009-08-15 22:21:21 +00:00
parent de667db0ef
commit 320c29d63c
7 changed files with 89 additions and 89 deletions
Download Patch File Download Diff File Expand all files Collapse all files
sw/airborne/booz/booz2_ins.c
+8 -8
View File
@@ -1,6 +1,6 @@
/*
* $Id$
*
*
* Copyright (C) 2008-2009 Antoine Drouin <poinix@gmail.com>
*
* This file is part of paparazzi.
@@ -18,7 +18,7 @@
* You should have received a copy of the GNU General Public License
* along with paparazzi; see the file COPYING. If not, write to
* the Free Software Foundation, 59 Temple Place - Suite 330,
* Boston, MA 02111-1307, USA.
* Boston, MA 02111-1307, USA.
*/
#include "booz2_ins.h"
@@ -51,7 +51,7 @@ struct NedCoor_i booz_ins_gps_speed_cm_s_ned;
int32_t booz_ins_qfe;
bool_t booz_ins_baro_initialised;
int32_t booz_ins_baro_alt;
bool_t booz_ins_vff_realign;
bool_t booz_ins_vff_realign;
#endif
/* output */
@@ -139,13 +139,13 @@ void booz_ins_update_gps(void) {
ned_of_ecef_vect_i(&booz_ins_gps_speed_cm_s_ned, &booz_ins_ltp_def, &booz_gps_state.ecef_vel);
#ifdef USE_HFF
b2ins_update_gps();
VECT2_SDIV(booz_ins_ltp_pos, (1<<(B2INS_POS_LTP_FRAC-INT32_POS_FRAC)), b2ins_pos_ltp);
VECT2_SDIV(booz_ins_ltp_speed, (1<<(B2INS_SPEED_LTP_FRAC-INT32_SPEED_FRAC)), b2ins_speed_ltp);
VECT2_SDIV(booz_ins_ltp_pos, b2ins_pos_ltp, (1<<(B2INS_POS_LTP_FRAC-INT32_POS_FRAC)));
VECT2_SDIV(booz_ins_ltp_speed, b2ins_speed_ltp, (1<<(B2INS_SPEED_LTP_FRAC-INT32_SPEED_FRAC)));
#else
INT32_VECT3_SCALE_2(b2ins_meas_gps_pos_ned, booz_ins_gps_pos_cm_ned,
INT32_POS_OF_CM_NUM, INT32_POS_OF_CM_DEN);
INT32_VECT3_SCALE_2(b2ins_meas_gps_pos_ned, booz_ins_gps_pos_cm_ned,
INT32_POS_OF_CM_NUM, INT32_POS_OF_CM_DEN);
INT32_VECT3_SCALE_2(b2ins_meas_gps_speed_ned, booz_ins_gps_speed_cm_s_ned,
INT32_SPEED_OF_CM_S_NUM, INT32_SPEED_OF_CM_S_DEN);
INT32_SPEED_OF_CM_S_NUM, INT32_SPEED_OF_CM_S_DEN);
VECT3_COPY(booz_ins_ltp_pos, b2ins_meas_gps_pos_ned);
VECT3_COPY(booz_ins_ltp_speed, b2ins_meas_gps_speed_ned);
#endif
sw/airborne/booz/booz2_navigation.c
+8 -8
View File
@@ -1,6 +1,6 @@
/*
* $Id$
*
*
* Copyright (C) 2008-2009 Antoine Drouin <poinix@gmail.com>
*
* This file is part of paparazzi.
@@ -18,7 +18,7 @@
* You should have received a copy of the GNU General Public License
* along with paparazzi; see the file COPYING. If not, write to
* the Free Software Foundation, 59 Temple Place - Suite 330,
* Boston, MA 02111-1307, USA.
* Boston, MA 02111-1307, USA.
*/
#define NAV_C
@@ -97,14 +97,14 @@ void booz2_nav_run(void) {
int32_t dist_to_waypoint;
INT32_VECT2_NORM(dist_to_waypoint, path_to_waypoint);
if (dist_to_waypoint < CLOSE_TO_WAYPOINT) {
VECT2_COPY( booz2_navigation_carrot, booz2_navigation_target);
}
else {
struct Int32Vect2 path_to_carrot;
VECT2_SMUL(path_to_carrot, CARROT_DIST, path_to_waypoint);
VECT2_SDIV(path_to_carrot, dist_to_waypoint, path_to_carrot);
VECT2_SMUL(path_to_carrot, path_to_waypoint, CARROT_DIST);
VECT2_SDIV(path_to_carrot, path_to_carrot, dist_to_waypoint);
VECT2_SUM(booz2_navigation_carrot, path_to_carrot, booz_ins_enu_pos);
}
@@ -125,8 +125,8 @@ void nav_route(uint8_t wp_start, uint8_t wp_end) {
int32_t nav_leg_progress = (pos_diff.x * wp_diff.x + pos_diff.y * wp_diff.y) / leg_length;
nav_leg_progress += Max((CARROT_DIST >> INT32_POS_FRAC), 0);
struct Int32Vect2 progress_pos;
VECT2_SMUL(progress_pos, nav_leg_progress, wp_diff);
VECT2_SDIV(progress_pos, leg_length, progress_pos);
VECT2_SMUL(progress_pos, wp_diff, nav_leg_progress);
VECT2_SDIV(progress_pos, progress_pos, leg_length);
INT32_VECT2_LSHIFT(progress_pos,progress_pos,INT32_POS_FRAC);
VECT2_SUM(booz2_navigation_target,waypoints[wp_start],progress_pos);
//printf("target %d %d | p %d %d | s %d %d | l %d %d %d\n",
@@ -201,7 +201,7 @@ void nav_init_stage( void ) {
horizontal_mode = HORIZONTAL_MODE_WAYPOINT;
}
void nav_init_block(void) {
void nav_init_block(void) {
if (nav_block >= NB_BLOCK)
nav_block=NB_BLOCK-1;
nav_stage = 0;
sw/airborne/booz/ins/booz2_hf_float.c
+18 -18
View File
@@ -1,6 +1,6 @@
/*
* $Id$
*
*
* Copyright (C) 2008-2009 Antoine Drouin <poinix@gmail.com>
*
* This file is part of paparazzi.
@@ -18,7 +18,7 @@
* You should have received a copy of the GNU General Public License
* along with paparazzi; see the file COPYING. If not, write to
* the Free Software Foundation, 59 Temple Place - Suite 330,
* Boston, MA 02111-1307, USA.
* Boston, MA 02111-1307, USA.
*/
#include "booz2_hf_float.h"
@@ -82,7 +82,7 @@ void b2ins_propagate(void) {
#define K_POS 3
/* make sure >=9 */
#define K_SPEED 9
#define K_SPEED 9
#define UPDATE_FROM_POS 1
#define UPDATE_FROM_SPEED 1
@@ -90,49 +90,49 @@ void b2ins_propagate(void) {
void b2ins_update_gps(void) {
/* FIXME : with Q_int32_XX_8 we overflow for 256m */
INT32_VECT3_SCALE_2(b2ins_meas_gps_pos_ned, booz_ins_gps_pos_cm_ned,
INT32_POS_OF_CM_NUM, INT32_POS_OF_CM_DEN);
INT32_VECT3_SCALE_2(b2ins_meas_gps_pos_ned, booz_ins_gps_pos_cm_ned,
INT32_POS_OF_CM_NUM, INT32_POS_OF_CM_DEN);
INT32_VECT3_SCALE_2(b2ins_meas_gps_speed_ned, booz_ins_gps_speed_cm_s_ned,
INT32_SPEED_OF_CM_S_NUM, INT32_SPEED_OF_CM_S_DEN);
INT32_SPEED_OF_CM_S_NUM, INT32_SPEED_OF_CM_S_DEN);
#ifdef UPDATE_FROM_POS
struct Int64Vect2 scaled_pos_meas;
VECT2_COPY(scaled_pos_meas, b2ins_meas_gps_pos_ned);
VECT2_SMUL(scaled_pos_meas, (1<<(B2INS_POS_LTP_FRAC-INT32_POS_FRAC)), scaled_pos_meas);
VECT2_SMUL(scaled_pos_meas, scaled_pos_meas, (1<<(B2INS_POS_LTP_FRAC-INT32_POS_FRAC)));
struct Int64Vect3 pos_residual;
VECT2_DIFF(pos_residual, scaled_pos_meas, b2ins_pos_ltp);
VECT2_DIFF(pos_residual, scaled_pos_meas, b2ins_pos_ltp);
struct Int32Vect2 pos_cor_1;
VECT2_SDIV(pos_cor_1, (1<<K_POS), pos_residual);
VECT2_SDIV(pos_cor_1, pos_residual, (1<<K_POS));
VECT2_ADD(b2ins_pos_ltp, pos_cor_1);
struct Int32Vect2 speed_cor_1;
VECT2_SDIV(speed_cor_1, (1<<(K_POS+9)), pos_residual);
VECT2_SDIV(speed_cor_1, pos_residual, (1<<(K_POS+9)));
VECT2_ADD(b2ins_speed_ltp, speed_cor_1);
#endif /* UPDATE_FROM_POS */
#ifdef UPDATE_FROM_SPEED
struct Int32Vect2 scaled_speed_meas;
VECT2_SMUL(scaled_speed_meas, (1<<(B2INS_SPEED_LTP_FRAC-INT32_SPEED_FRAC)), b2ins_meas_gps_speed_ned);
VECT2_SMUL(scaled_speed_meas, b2ins_meas_gps_speed_ned, (1<<(B2INS_SPEED_LTP_FRAC-INT32_SPEED_FRAC)));
struct Int32Vect2 speed_residual;
VECT2_DIFF(speed_residual, scaled_speed_meas, b2ins_speed_ltp);
struct Int32Vect2 pos_cor_s;
VECT2_SDIV(pos_cor_s, (1<<(K_SPEED-9)), speed_residual);
VECT2_SDIV(pos_cor_s, speed_residual, (1<<(K_SPEED-9)));
VECT2_ADD(b2ins_pos_ltp, pos_cor_s);
struct Int32Vect2 speed_cor_s;
VECT2_SDIV(speed_cor_s, (1<<K_SPEED), speed_residual);
VECT2_SDIV(speed_cor_s, speed_residual, (1<<K_SPEED));
VECT2_ADD(b2ins_speed_ltp, speed_cor_s);
struct Int32Vect3 speed_residual3;
VECT2_SDIV(speed_residual3, (1<<9), speed_residual);
VECT2_SDIV(speed_residual3, speed_residual, (1<<9));
speed_residual3.z = 0;
struct Int32Vect3 bias_cor_s;
INT32_QUAT_VMULT( bias_cor_s, booz_ahrs.ltp_to_imu_quat, speed_residual3);
// VECT3_ADD(b2ins_accel_bias, bias_cor_s);
// VECT3_ADD(b2ins_accel_bias, bias_cor_s);
#endif /* UPDATE_FROM_SPEED */
#endif /* UPDATE_FROM_SPEED */
}
sw/airborne/math/pprz_algebra.h
+14 -14
View File
@@ -67,14 +67,14 @@
(_c).y = (_a).y - (_b).y; \
}
/* _vo = _s * _vi */
#define VECT2_SMUL(_vo, _s, _vi) { \
(_vo).x = (_s) * (_vi).x; \
(_vo).y = (_s) * (_vi).y; \
/* _vo = _vi * _s */
#define VECT2_SMUL(_vo, _vi, _s) { \
(_vo).x = (_vi).x * (_s); \
(_vo).y = (_vi).y * (_s); \
}
/* _vo = _vi / _s */
#define VECT2_SDIV(_vo, _s, _vi) { \
#define VECT2_SDIV(_vo, _vi, _s) { \
(_vo).x = (_vi).x / (_s); \
(_vo).y = (_vi).y / (_s); \
}
@@ -133,11 +133,11 @@
(_c).z = (_a).z - (_b).z; \
}
/* _vo = _s * _vi */
#define VECT3_SMUL(_vo, _s, _vi) { \
(_vo).x = (_s) * (_vi).x; \
(_vo).y = (_s) * (_vi).y; \
(_vo).z = (_s) * (_vi).z; \
/* _vo = _vi * _s */
#define VECT3_SMUL(_vo, _vi, _s) { \
(_vo).x = (_vi).x * (_s); \
(_vo).y = (_vi).y * (_s); \
(_vo).z = (_vi).z * (_s); \
}
/* _vo = _vi / _s */
@@ -159,21 +159,21 @@
(_vo).x = (_va).x / (_vb).x; \
(_vo).y = (_va).y / (_vb).y; \
(_vo).z = (_va).z / (_vb).z; \
}
}
/* */
#define VECT3_EW_MUL(_vo, _va, _vb) { \
(_vo).x = (_va).x * (_vb).x; \
(_vo).y = (_va).y * (_vb).y; \
(_vo).z = (_va).z * (_vb).z; \
}
}
/* */
#define VECT3_BOUND_CUBE(_v, _min, _max) { \
if ((_v).x > (_max)) (_v).x = (_max); else if ((_v).x < (_min)) (_v).x = (_min); \
if ((_v).y > (_max)) (_v).y = (_max); else if ((_v).y < (_min)) (_v).y = (_min); \
if ((_v).z > (_max)) (_v).z = (_max); else if ((_v).z < (_min)) (_v).z = (_min); \
}
}
/* */
#define VECT3_BOUND_BOX(_v, _v_min, _v_max) { \
@@ -420,7 +420,7 @@
(_qo).qy += (_qi).qy; \
(_qo).qz += (_qi).qz; \
}
#define QUAT_INVERT(_qo, _qi) { \
(_qo).qi = (_qi).qi; \
(_qo).qx = -(_qi).qx; \
sw/airborne/math/pprz_algebra_float.h
+3 -3
View File
@@ -84,8 +84,8 @@ struct FloatRates {
/* a -= b */
#define FLOAT_VECT3_SUB(_a, _b) VECT3_SUB(_a, _b)
/* _vo = _s * _vi */
#define FLOAT_VECT3_SMUL(_vo, _s, _vi) VECT3_SMUL(_vo, _s, _vi)
/* _vo = _vi * _s */
#define FLOAT_VECT3_SMUL(_vo, _vi, _s) VECT3_SMUL(_vo, _vi, _s)
#define FLOAT_VECT3_NORM(_v) (sqrtf((_v).x*(_v).x + (_v).y*(_v).y + (_v).z*(_v).z))
@@ -97,7 +97,7 @@ struct FloatRates {
#define FLOAT_VECT3_NORMALIZE(_v) { \
const float n = FLOAT_VECT3_NORM(_v); \
FLOAT_VECT3_SMUL(_v, 1./n, _v); \
FLOAT_VECT3_SMUL(_v, _v, 1./n); \
}
#define FLOAT_RATES_ZERO(_r) { \
sw/simulator/nps/nps_random.c
+25 -25
View File
@@ -48,19 +48,19 @@ void double_vect3_update_random_walk(struct DoubleVect3* rw, struct DoubleVect3*
struct DoubleVect3 drw;
double_vect3_get_gaussian_noise(&drw, std_dev);
struct DoubleVect3 tmp;
VECT3_SMUL(tmp, (-1./thau), *rw);
VECT3_SMUL(tmp, *rw, (-1./thau));
VECT3_ADD(drw, tmp);
VECT3_SMUL(drw, dt, drw);
VECT3_SMUL(drw, drw, dt);
VECT3_ADD(*rw, drw);
}
/*
http://www.taygeta.com/random/gaussian.html
/*
http://www.taygeta.com/random/gaussian.html
*/
double get_gaussian_noise(void) {
double x1;
static int nb_call = 0;
static double x2, w;
@@ -72,7 +72,7 @@ double get_gaussian_noise(void) {
x2 = 2.0 * dr250() - 1.0;
w = x1 * x1 + x2 * x2;
} while ( w >= 1.0 );
w = sqrt( (-2.0 * log( w ) ) / w );
return x1 * w;
}
@@ -133,18 +133,18 @@ static void r250_init(int sd) {
int j, k;
unsigned int mask, msb;
set_seed( sd );
r250_index = 0;
for (j = 0; j < 250; j++) /* fill r250 buffer with BITS-1 bit values */
r250_buffer[j] = randlcg();
for (j = 0; j < 250; j++) /* set some MSBs to 1 */
if ( randlcg() > HALF_RANGE )
r250_buffer[j] |= MSB;
msb = MSB; /* turn on diagonal bit */
mask = ALL_BITS; /* turn off the leftmost bits */
for (j = 0; j < BITS; j++) {
k = STEP * j + 3; /* select a word to operate on */
r250_buffer[k] &= mask; /* turn off bits left of the diagonal */
@@ -152,7 +152,7 @@ static void r250_init(int sd) {
mask >>= 1;
msb >>= 1;
}
}
#if 0
@@ -160,22 +160,22 @@ static void r250_init(int sd) {
static unsigned int r250(void) {
register int j;
register unsigned int new_rand;
if ( r250_index >= 147 )
j = r250_index - 147; /* wrap pointer around */
else
j = r250_index + 103;
new_rand = r250_buffer[ r250_index ] ^ r250_buffer[ j ];
r250_buffer[ r250_index ] = new_rand;
if ( r250_index >= 249 ) /* increment pointer for next time */
r250_index = 0;
else
r250_index++;
return new_rand;
}
#endif
@@ -183,22 +183,22 @@ static unsigned int r250(void) {
static double dr250(void) {
register int j;
register unsigned int new_rand;
if ( r250_index >= 147 )
j = r250_index - 147; /* wrap pointer around */
else
j = r250_index + 103;
new_rand = r250_buffer[ r250_index ] ^ r250_buffer[ j ];
r250_buffer[ r250_index ] = new_rand;
if ( r250_index >= 249 ) /* increment pointer for next time */
r250_index = 0;
else
r250_index++;
return (double)new_rand / ALL_BITS;
}
/*
@@ -229,16 +229,16 @@ unsigned long int randlcg() {
{
long int high_part = seed_val / quotient;
long int low_part = seed_val % quotient;
long int test = 16807L * low_part - my_remainder * high_part;
if ( test > 0 )
seed_val = test;
else
seed_val = test + LONG_MAX;
}
return seed_val;
}
sw/simulator/old_booz/booz_sensors_model_gps.c
+13 -13
View File
@@ -69,14 +69,14 @@ void booz_sensors_model_gps_run( double time) {
if (time < bsm.gps_next_update)
return;
/*
* simulate speed sensor
/*
* simulate speed sensor
*/
static VEC *cur_speed_reading = VNULL;
cur_speed_reading = v_resize(cur_speed_reading, AXIS_NB);
v_copy(bfm.speed_ltp, cur_speed_reading);;
/* add a gaussian noise */
cur_speed_reading = v_add_gaussian_noise(cur_speed_reading, bsm.gps_speed_noise_std_dev,
cur_speed_reading = v_add_gaussian_noise(cur_speed_reading, bsm.gps_speed_noise_std_dev,
cur_speed_reading);
UpdateSensorLatency(time, cur_speed_reading, bsm.gps_speed_history, BSM_GPS_SPEED_LATENCY, bsm.gps_speed);
@@ -87,10 +87,10 @@ void booz_sensors_model_gps_run( double time) {
bsm.gps_speed->ve[AXIS_Y] * bsm.gps_speed->ve[AXIS_Y]) * 100.;
bsm.gps_speed_gspeed = rint(bsm.gps_speed_gspeed);
bsm.gps_speed_climb = rint(-bsm.gps_speed->ve[AXIS_Z] * 100);
/*
* simulate position sensor
/*
* simulate position sensor
*/
/* compute position error */
@@ -100,16 +100,16 @@ void booz_sensors_model_gps_run( double time) {
/* add a gaussian noise */
pos_error = v_add_gaussian_noise(pos_error, bsm.gps_pos_noise_std_dev, pos_error);
/* update random walk bias */
bsm.gps_pos_bias_random_walk_value =
v_update_random_walk(bsm.gps_pos_bias_random_walk_value,
bsm.gps_pos_bias_random_walk_std_dev, BSM_GPS_DT,
bsm.gps_pos_bias_random_walk_value =
v_update_random_walk(bsm.gps_pos_bias_random_walk_value,
bsm.gps_pos_bias_random_walk_std_dev, BSM_GPS_DT,
bsm.gps_pos_bias_random_walk_value);
/* add it */
pos_error = v_add(pos_error, bsm.gps_pos_bias_random_walk_value, pos_error);
pos_error = v_add(pos_error, bsm.gps_pos_bias_random_walk_value, pos_error);
/* sum true pos and error reading */
static VEC *cur_pos_reading = VNULL;
cur_pos_reading = v_resize(cur_pos_reading, AXIS_NB);
v_add(bfm.pos_ltp, pos_error, cur_pos_reading);
v_add(bfm.pos_ltp, pos_error, cur_pos_reading);
/* store that for later and retrieve a previously stored data */
UpdateSensorLatency(time, cur_pos_reading, bsm.gps_pos_history, BSM_GPS_POS_LATENCY, bsm.gps_pos);
@@ -139,10 +139,10 @@ void booz_sensors_model_gps_run( double time) {
/* ECEF Conversion */
struct NedCoor_d pos_ned = {bsm.gps_pos->ve[AXIS_X], bsm.gps_pos->ve[AXIS_Y], bsm.gps_pos->ve[AXIS_Z]};
ecef_of_ned_point_d(&bsm.gps_pos_ecef, &bsm.gps_ltp_def, &pos_ned);
VECT3_SMUL(bsm.gps_pos_ecef, (double)1e2, bsm.gps_pos_ecef);
VECT3_SMUL(bsm.gps_pos_ecef, bsm.gps_pos_ecef, (double)1e2);
struct NedCoor_d speed_ned = {bsm.gps_speed->ve[AXIS_X], bsm.gps_speed->ve[AXIS_Y], bsm.gps_speed->ve[AXIS_Z]};
ecef_of_ned_vect_d(&bsm.gps_speed_ecef, &bsm.gps_ltp_def , &speed_ned);
VECT3_SMUL(bsm.gps_speed_ecef, (double)1e2, bsm.gps_speed_ecef);
VECT3_SMUL(bsm.gps_speed_ecef, bsm.gps_speed_ecef, (double)1e2);
bsm.gps_next_update += BSM_GPS_DT;
bsm.gps_available = TRUE;