From 09b631c80d7d8627940bafa0101d7054e3b60fa7 Mon Sep 17 00:00:00 2001 From: Antoine Drouin Date: Tue, 6 Nov 2007 21:08:19 +0000 Subject: [PATCH] *** empty log message *** --- sw/airborne/booz_control.c | 3 +- sw/airborne/booz_nav.c | 32 +++++++++++- sw/airborne/booz_nav.h | 26 ++++++++-- sw/airborne/booz_nav_hover.c | 7 +-- sw/simulator/booz_sensors_model.c | 65 +++++++++++------------- sw/simulator/booz_sensors_model_params.h | 56 ++++++++++++++++++++ sw/simulator/main_booz_sim.c | 4 +- 7 files changed, 145 insertions(+), 48 deletions(-) create mode 100644 sw/simulator/booz_sensors_model_params.h diff --git a/sw/airborne/booz_control.c b/sw/airborne/booz_control.c index 8991382052..fbf96d883a 100644 --- a/sw/airborne/booz_control.c +++ b/sw/airborne/booz_control.c @@ -137,7 +137,8 @@ void booz_control_attitude_run(void) { #ifndef DISABLE_MAGNETOMETER - const float att_err_psi = booz_estimator_psi - booz_control_attitude_psi_sp; + float att_err_psi = booz_estimator_psi - booz_control_attitude_psi_sp; + NormRadAngle(att_err_psi); const float cmd_r = booz_control_attitude_psi_pgain * att_err_psi + booz_control_attitude_psi_dgain * booz_estimator_r; #else diff --git a/sw/airborne/booz_nav.c b/sw/airborne/booz_nav.c index 047b9423f6..fbfbc3b169 100644 --- a/sw/airborne/booz_nav.c +++ b/sw/airborne/booz_nav.c @@ -7,6 +7,32 @@ #include "radio_control.h" #ifndef DISABLE_NAV + + +#define Block(x) case x: nav_block=x; +#define InitBlock() { nav_stage = 0; block_time = 0; InitStage(); } +#define NextBlock() { nav_block++; InitBlock(); } +#define GotoBlock(b) { nav_block=b; InitBlock(); } + +#define Stage(s) case s: nav_stage=s; +#define NextStage() { nav_stage++; InitStage() } +#define NextStageFrom(wp) { last_wp = wp; NextStage() } +#define GotoStage(s) { nav_stage = s; InitStage() } + +#define NavGotoWaypoint(_wp) {} +#define NavVerticalAutoThrottleMode(_foo) {} +#define NavVerticalAltitudeMode(_foo, _bar) {} + +#define NAV_C +#include "flight_plan.h" + +const uint8_t nb_waypoint; +struct point waypoints[NB_WAYPOINT] = WAYPOINTS; + +uint8_t nav_stage, nav_block; +uint16_t stage_time, block_time; + + #include "booz_nav_hover.h" #define BoozNavRunOnceEvery(_prescaler, _code) { \ @@ -21,17 +47,19 @@ void booz_nav_init(void) { + nav_block = 0; + nav_stage = 0; booz_nav_hover_init(); } void booz_nav_run(void) { #ifndef DISABLE_NAV - BoozNavRunOnceEvery( 10, \ + BoozNavRunOnceEvery( 62, \ { \ booz_nav_hover_run(); \ BoozControlAttitudeSetSetPoints(booz_nav_hover_phi_command, booz_nav_hover_theta_command, \ - booz_nav_hover_psi_sp, booz_nav_hover_power_command); \ + booz_nav_hover_psi_sp, booz_nav_hover_power_command); \ }); booz_control_attitude_run(); diff --git a/sw/airborne/booz_nav.h b/sw/airborne/booz_nav.h index 1c81d99c45..872ad075ee 100644 --- a/sw/airborne/booz_nav.h +++ b/sw/airborne/booz_nav.h @@ -1,15 +1,35 @@ #ifndef BOOZ_NAV_H #define BOOZ_NAV_H +#include "std.h" #include "booz_nav_hover.h" -#ifndef DISABLE_NAV - -#endif extern void booz_nav_init(void); extern void booz_nav_run(void); extern void booz_nav_read_setpoints_from_rc(void); +struct point { + float x; + float y; + float a; +}; + +#define WaypointX(_wp) (waypoints[_wp].x) +#define WaypointY(_wp) (waypoints[_wp].y) +#define WaypointAlt(_wp) (waypoints[_wp].a) + +extern const uint8_t nb_waypoint; +extern struct point waypoints[]; + +extern uint16_t stage_time, block_time; +extern uint8_t nav_stage, nav_block; + +extern void nav_init_stage( void ); +#define InitStage() { nav_init_stage(); return; } + + + + #endif /* BOOZ_NAV_H */ diff --git a/sw/airborne/booz_nav_hover.c b/sw/airborne/booz_nav_hover.c index 1a6f401119..ee710aa639 100644 --- a/sw/airborne/booz_nav_hover.c +++ b/sw/airborne/booz_nav_hover.c @@ -79,17 +79,12 @@ void booz_nav_hover_run(void) { void booz_nav_hover_read_setpoints_from_rc(void) { #ifndef DISABLE_NAV booz_nav_hover_z_sp = -1. - 10. / MAX_PPRZ * (float)rc_values[RADIO_THROTTLE]; - booz_nav_hover_psi_sp = -RadOfDeg(30.) / MAX_PPRZ * (float)rc_values[RADIO_YAW]; #if defined STICK_MODE && STICK_MODE == STICK_ABSOLUTE_POS booz_nav_horizontal_x_sp = -20. / MAX_PPRZ * (float)rc_values[RADIO_PITCH]; /* +/- 20m */ booz_nav_horizontal_y_sp = -20. / MAX_PPRZ * (float)rc_values[RADIO_ROLL]; /* warning RC roll negativ to the right !! +/- 20m */ - booz_nav_horizontal_u_sp = 0.; - booz_nav_horizontal_v_sp = 0.; #elif defined STICK_MODE && STICK_MODE == STICK_ABSOLUTE_SPEED booz_nav_horizontal_x_sp += -5. * DT_READ_SETPOINTS / MAX_PPRZ * (float)rc_values[RADIO_PITCH]; /* +/-5 m/s */ booz_nav_horizontal_y_sp += 5. * DT_READ_SETPOINTS / MAX_PPRZ * (float)rc_values[RADIO_ROLL]; /* +/-5 m/s */ - booz_nav_horizontal_u_sp = 0.; - booz_nav_horizontal_v_sp = 0.; #elif defined STICK_MODE && STICK_MODE == STICK_RELATIVE_SPEED float booz_nav_hover_dx_bod = -5. / MAX_PPRZ * (float)rc_values[RADIO_PITCH]; float booz_nav_hover_dy_bod = -5. / MAX_PPRZ * (float)rc_values[RADIO_ROLL]; /* warning RC roll negativ to the right !! +/- 5m/s */ @@ -100,6 +95,8 @@ void booz_nav_hover_read_setpoints_from_rc(void) { booz_nav_hover_dx_bod * booz_estimator_dcm[AXIS_Y][AXIS_Y]; booz_nav_hover_x_sp += (booz_nav_hover_dx_earth * DT_READ_SETPOINTS); booz_nav_hover_y_sp += (booz_nav_hover_dy_earth * DT_READ_SETPOINTS); + booz_nav_hover_psi_sp += (-RadOfDeg(60.) * DT_READ_SETPOINTS / MAX_PPRZ * (float)rc_values[RADIO_YAW]); + NormRadAngle(booz_nav_hover_psi_sp); #endif /* STICK_MODE */ #endif /* DISABLE_NAV */ } diff --git a/sw/simulator/booz_sensors_model.c b/sw/simulator/booz_sensors_model.c index 248cc689bd..e529e21ce2 100644 --- a/sw/simulator/booz_sensors_model.c +++ b/sw/simulator/booz_sensors_model.c @@ -1,4 +1,5 @@ #include "booz_sensors_model.h" +#include "booz_sensors_model_params.h" #include @@ -61,28 +62,28 @@ static void booz_sensors_model_accel_init(void) { bsm.accel->ve[AXIS_X] = 0.; bsm.accel->ve[AXIS_Y] = 0.; bsm.accel->ve[AXIS_Z] = 0.; - bsm.accel_resolution = 1024 * 32; + bsm.accel_resolution = BSM_ACCEL_RESOLUTION; bsm.accel_sensitivity = m_get(AXIS_NB, AXIS_NB); m_zero(bsm.accel_sensitivity); - bsm.accel_sensitivity->me[AXIS_X][AXIS_X] = -(double)(bsm.accel_resolution) / (6. * 9.81); - bsm.accel_sensitivity->me[AXIS_Y][AXIS_Y] = (double)(bsm.accel_resolution) / (6. * 9.81); - bsm.accel_sensitivity->me[AXIS_Z][AXIS_Z] = (double)(bsm.accel_resolution) / (6. * 9.81); + bsm.accel_sensitivity->me[AXIS_X][AXIS_X] = BSM_ACCEL_SENSITIVITY_XX; + bsm.accel_sensitivity->me[AXIS_Y][AXIS_Y] = BSM_ACCEL_SENSITIVITY_YY; + bsm.accel_sensitivity->me[AXIS_Z][AXIS_Z] = BSM_ACCEL_SENSITIVITY_ZZ; bsm.accel_neutral = v_get(AXIS_NB); - bsm.accel_neutral->ve[AXIS_X] = 538 * 32; - bsm.accel_neutral->ve[AXIS_Y] = 506 * 32; - bsm.accel_neutral->ve[AXIS_Z] = 506 * 32; + bsm.accel_neutral->ve[AXIS_X] = BSM_ACCEL_NEUTRAL_X; + bsm.accel_neutral->ve[AXIS_Y] = BSM_ACCEL_NEUTRAL_Y; + bsm.accel_neutral->ve[AXIS_Z] = BSM_ACCEL_NEUTRAL_Z; bsm.accel_noise_std_dev = v_get(AXIS_NB); - bsm.accel_noise_std_dev->ve[AXIS_X] = 2e-1; /* m2s-4 */ - bsm.accel_noise_std_dev->ve[AXIS_Y] = 2e-1; /* m2s-4 */ - bsm.accel_noise_std_dev->ve[AXIS_Z] = 2e-1; /* m2s-4 */ + bsm.accel_noise_std_dev->ve[AXIS_X] = BSM_ACCEL_NOISE_STD_DEV_X; + bsm.accel_noise_std_dev->ve[AXIS_Y] = BSM_ACCEL_NOISE_STD_DEV_Y; + bsm.accel_noise_std_dev->ve[AXIS_Z] = BSM_ACCEL_NOISE_STD_DEV_Z; bsm.accel_bias = v_get(AXIS_NB); - bsm.accel_bias->ve[AXIS_P] = 1e-3; /* ms-2 */ - bsm.accel_bias->ve[AXIS_Q] = 1e-3; /* ms-2 */ - bsm.accel_bias->ve[AXIS_R] = 1e-3; /* ms-2 */ + bsm.accel_bias->ve[AXIS_P] = BSM_ACCEL_BIAS_X; + bsm.accel_bias->ve[AXIS_Q] = BSM_ACCEL_BIAS_Y; + bsm.accel_bias->ve[AXIS_R] = BSM_ACCEL_BIAS_Z; } @@ -93,39 +94,33 @@ static void booz_sensors_model_gyro_init(void) { bsm.gyro->ve[AXIS_P] = 0.; bsm.gyro->ve[AXIS_Q] = 0.; bsm.gyro->ve[AXIS_R] = 0.; - bsm.gyro_resolution = 65536; + bsm.gyro_resolution = BSM_GYRO_RESOLUTION; bsm.gyro_sensitivity = m_get(AXIS_NB, AXIS_NB); m_zero(bsm.gyro_sensitivity); - bsm.gyro_sensitivity->me[AXIS_P][AXIS_P] = - (double)bsm.gyro_resolution / (2.*RadOfDeg(-413.41848)); /* degres/s - nominal 300 */ - bsm.gyro_sensitivity->me[AXIS_Q][AXIS_Q] = - (double)bsm.gyro_resolution / (2.*RadOfDeg(-403.65564)); /* degres/s - nominal 300 */ - bsm.gyro_sensitivity->me[AXIS_R][AXIS_R] = - (double)bsm.gyro_resolution / (2.*RadOfDeg( 395.01929)); /* degres/s - nominal 300 */ + bsm.gyro_sensitivity->me[AXIS_P][AXIS_P] = BSM_GYRO_SENSITIVITY_PP; + bsm.gyro_sensitivity->me[AXIS_Q][AXIS_Q] = BSM_GYRO_SENSITIVITY_QQ; + bsm.gyro_sensitivity->me[AXIS_R][AXIS_R] = BSM_GYRO_SENSITIVITY_RR; bsm.gyro_neutral = v_get(AXIS_NB); - bsm.gyro_neutral->ve[AXIS_P] = - (double)bsm.gyro_resolution * 0.6238556; /* ratio of full scale - nominal 0.5 */ - bsm.gyro_neutral->ve[AXIS_Q] = - (double)bsm.gyro_resolution * 0.6242371; /* ratio of full scale - nominal 0.5 */ - bsm.gyro_neutral->ve[AXIS_R] = - (double)bsm.gyro_resolution * 0.6035156; /* ratio of full scale - nominal 0.5 */ + bsm.gyro_neutral->ve[AXIS_P] = BSM_GYRO_NEUTRAL_P; + bsm.gyro_neutral->ve[AXIS_Q] = BSM_GYRO_NEUTRAL_Q; + bsm.gyro_neutral->ve[AXIS_R] = BSM_GYRO_NEUTRAL_R; bsm.gyro_noise_std_dev = v_get(AXIS_NB); - bsm.gyro_noise_std_dev->ve[AXIS_P] = RadOfDeg(.5); - bsm.gyro_noise_std_dev->ve[AXIS_Q] = RadOfDeg(.5); - bsm.gyro_noise_std_dev->ve[AXIS_R] = RadOfDeg(.5); + bsm.gyro_noise_std_dev->ve[AXIS_P] = BSM_GYRO_NOISE_STD_DEV_P; + bsm.gyro_noise_std_dev->ve[AXIS_Q] = BSM_GYRO_NOISE_STD_DEV_Q; + bsm.gyro_noise_std_dev->ve[AXIS_R] = BSM_GYRO_NOISE_STD_DEV_R; bsm.gyro_bias_initial = v_get(AXIS_NB); - bsm.gyro_bias_initial->ve[AXIS_P] = RadOfDeg( -0.2); - bsm.gyro_bias_initial->ve[AXIS_Q] = RadOfDeg( -0.5); - bsm.gyro_bias_initial->ve[AXIS_R] = RadOfDeg( 0.5); + bsm.gyro_bias_initial->ve[AXIS_P] = BSM_GYRO_BIAS_INITIAL_P; + bsm.gyro_bias_initial->ve[AXIS_Q] = BSM_GYRO_BIAS_INITIAL_Q; + bsm.gyro_bias_initial->ve[AXIS_R] = BSM_GYRO_BIAS_INITIAL_R; bsm.gyro_bias_random_walk_std_dev = v_get(AXIS_NB); - bsm.gyro_bias_random_walk_std_dev->ve[AXIS_P] = RadOfDeg(5.e-1); - bsm.gyro_bias_random_walk_std_dev->ve[AXIS_Q] = RadOfDeg(5.e-1); - bsm.gyro_bias_random_walk_std_dev->ve[AXIS_R] = RadOfDeg(5.e-1); + bsm.gyro_bias_random_walk_std_dev->ve[AXIS_P] = BSM_GYRO_BIAS_RANDOM_WALK_STD_DEV_P; + bsm.gyro_bias_random_walk_std_dev->ve[AXIS_Q] = BSM_GYRO_BIAS_RANDOM_WALK_STD_DEV_Q; + bsm.gyro_bias_random_walk_std_dev->ve[AXIS_R] = BSM_GYRO_BIAS_RANDOM_WALK_STD_DEV_R; bsm.gyro_bias_random_walk_value = v_get(AXIS_NB); bsm.gyro_bias_random_walk_value->ve[AXIS_P] = bsm.gyro_bias_initial->ve[AXIS_P]; diff --git a/sw/simulator/booz_sensors_model_params.h b/sw/simulator/booz_sensors_model_params.h new file mode 100644 index 0000000000..cb0b7d52fe --- /dev/null +++ b/sw/simulator/booz_sensors_model_params.h @@ -0,0 +1,56 @@ +#ifndef BOOZ_SENSORS_MODEL_PARAMS_H +#define BOOZ_SENSORS_MODEL_PARAMS_H + +/* + * Accelerometer +*/ +#define BSM_ACCEL_RESOLUTION (1024 * 32) +/* ms-2 */ +#define BSM_ACCEL_SENSITIVITY_XX -(1024. * 32.)/(2 * 6. * 9.81) +#define BSM_ACCEL_SENSITIVITY_YY (1024. * 32.)/(2 * 6. * 9.81) +#define BSM_ACCEL_SENSITIVITY_ZZ (1024. * 32.)/(2 * 6. * 9.81) +#define BSM_ACCEL_NEUTRAL_X (538. * 32.) +#define BSM_ACCEL_NEUTRAL_Y (506. * 32.) +#define BSM_ACCEL_NEUTRAL_Z (506. * 32.) +/* m2s-4 */ +#define BSM_ACCEL_NOISE_STD_DEV_X 2e-1 +#define BSM_ACCEL_NOISE_STD_DEV_Y 2e-1 +#define BSM_ACCEL_NOISE_STD_DEV_Z 2e-1 +/* ms-2 */ +#define BSM_ACCEL_BIAS_X 1e-3 +#define BSM_ACCEL_BIAS_Y 1e-3 +#define BSM_ACCEL_BIAS_Z 1e-3 + + + +/* + * Gyrometer + */ +#define BSM_GYRO_RESOLUTION 65536 +/* degres/s - nominal 300 */ +#define BSM_GYRO_SENSITIVITY_PP 65536. / (2.*RadOfDeg(-413.41848)); +#define BSM_GYRO_SENSITIVITY_QQ 65536. / (2.*RadOfDeg(-403.65564)); +#define BSM_GYRO_SENSITIVITY_RR 65536. / (2.*RadOfDeg( 395.01929)); + +#define BSM_GYRO_NEUTRAL_P 65536. * 0.6238556; +#define BSM_GYRO_NEUTRAL_Q 65536. * 0.6242371; +#define BSM_GYRO_NEUTRAL_R 65536. * 0.6035156; + +#define BSM_GYRO_NOISE_STD_DEV_P RadOfDeg(.5) +#define BSM_GYRO_NOISE_STD_DEV_Q RadOfDeg(.5) +#define BSM_GYRO_NOISE_STD_DEV_R RadOfDeg(.5) + +#define BSM_GYRO_BIAS_INITIAL_P RadOfDeg( .5) +#define BSM_GYRO_BIAS_INITIAL_Q RadOfDeg(-0.5) +#define BSM_GYRO_BIAS_INITIAL_R RadOfDeg( .25) + +#define BSM_GYRO_BIAS_RANDOM_WALK_STD_DEV_P RadOfDeg(5.e-1) +#define BSM_GYRO_BIAS_RANDOM_WALK_STD_DEV_Q RadOfDeg(5.e-1) +#define BSM_GYRO_BIAS_RANDOM_WALK_STD_DEV_R RadOfDeg(5.e-1) + + + + + + +#endif /* BOOZ_SENSORS_MODEL_PARAMS_H */ diff --git a/sw/simulator/main_booz_sim.c b/sw/simulator/main_booz_sim.c index 21b35cbcf8..fc041d69ce 100644 --- a/sw/simulator/main_booz_sim.c +++ b/sw/simulator/main_booz_sim.c @@ -201,9 +201,9 @@ static void booz_sim_set_ppm_from_joystick( void ) { #else ppm_pulses[RADIO_THROTTLE] = 1223 + 0.6 * (2050-1223); //BREAK_MTT(); - //WALK_OVAL(); + WALK_OVAL(); // CIRCLE(); - HOVER(); + //HOVER(); // TOUPIE(); // ATTITUDE_ROLL_STEPS(); //ATTITUDE_PITCH_STEPS();