diff --git a/sw/airborne/booz/booz2_ins.c b/sw/airborne/booz/booz2_ins.c index 7496d923d7..e1755d7459 100644 --- a/sw/airborne/booz/booz2_ins.c +++ b/sw/airborne/booz/booz2_ins.c @@ -1,6 +1,6 @@ /* * $Id$ - * + * * Copyright (C) 2008-2009 Antoine Drouin * * 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 diff --git a/sw/airborne/booz/booz2_navigation.c b/sw/airborne/booz/booz2_navigation.c index b42fd22f8e..033c01e933 100644 --- a/sw/airborne/booz/booz2_navigation.c +++ b/sw/airborne/booz/booz2_navigation.c @@ -1,6 +1,6 @@ /* * $Id$ - * + * * Copyright (C) 2008-2009 Antoine Drouin * * 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; diff --git a/sw/airborne/booz/ins/booz2_hf_float.c b/sw/airborne/booz/ins/booz2_hf_float.c index 9ed678bbd4..d108561947 100644 --- a/sw/airborne/booz/ins/booz2_hf_float.c +++ b/sw/airborne/booz/ins/booz2_hf_float.c @@ -1,6 +1,6 @@ /* * $Id$ - * + * * Copyright (C) 2008-2009 Antoine Drouin * * 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< (_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; \ diff --git a/sw/airborne/math/pprz_algebra_float.h b/sw/airborne/math/pprz_algebra_float.h index 2963fa73e9..5325574f28 100644 --- a/sw/airborne/math/pprz_algebra_float.h +++ b/sw/airborne/math/pprz_algebra_float.h @@ -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) { \ diff --git a/sw/simulator/nps/nps_random.c b/sw/simulator/nps/nps_random.c index 1a8668106f..05315d483e 100644 --- a/sw/simulator/nps/nps_random.c +++ b/sw/simulator/nps/nps_random.c @@ -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; } diff --git a/sw/simulator/old_booz/booz_sensors_model_gps.c b/sw/simulator/old_booz/booz_sensors_model_gps.c index 3c41930151..cbd26079de 100644 --- a/sw/simulator/old_booz/booz_sensors_model_gps.c +++ b/sw/simulator/old_booz/booz_sensors_model_gps.c @@ -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;