diff --git a/sw/airborne/AMI601.c b/sw/airborne/AMI601.c index 0597016212..dfeeeac6cb 100644 --- a/sw/airborne/AMI601.c +++ b/sw/airborne/AMI601.c @@ -36,12 +36,12 @@ volatile uint32_t ami601_nb_err; void ami601_init( void ) { /* configure TRG pin as output and assert it*/ - SetBit(AMI601_TRG_IODIR , AMI601_TRG_PIN); - SetBit(AMI601_TRG_IOSET , AMI601_TRG_PIN); + // SetBit(AMI601_TRG_IODIR , AMI601_TRG_PIN); + // SetBit(AMI601_TRG_IOSET , AMI601_TRG_PIN); /* configure RST pin as output and set it low*/ - SetBit(AMI601_RST_IODIR , AMI601_RST_PIN); - SetBit(AMI601_RST_IOCLR , AMI601_RST_PIN); + // SetBit(AMI601_RST_IODIR , AMI601_RST_PIN); + // SetBit(AMI601_RST_IOCLR , AMI601_RST_PIN); uint8_t i; diff --git a/sw/airborne/AMI601.h b/sw/airborne/AMI601.h index f0988ef8de..f659ab87af 100644 --- a/sw/airborne/AMI601.h +++ b/sw/airborne/AMI601.h @@ -41,7 +41,7 @@ extern volatile uint32_t ami601_nb_err; case AMI601_SENDING_REQ : \ if ( ami601_i2c_done ) { \ /* trigger delay for measurement */ \ - T0MR1 = T0TC + SYS_TICS_OF_USEC(4096); \ + T0MR1 = T0TC + SYS_TICS_OF_USEC(12288); \ /* clear match 1 interrupt */ \ T0IR = TIR_MR1I; \ /* enable match 1 interrupt */ \ @@ -57,7 +57,7 @@ extern volatile uint32_t ami601_nb_err; uint8_t i; \ for (i=0; i< AMI601_NB_CHAN; i++) { \ ami601_val[i] = i2c_buf[3 + 2 * i]; \ - ami601_val[i] += i2c_buf[3 + 2 * i + 1] * 256; \ + ami601_val[i] += i2c_buf[3 + 2 * i + 1] * 256; \ } \ ami601_status = AMI601_DATA_AVAILABLE; \ } \ diff --git a/sw/airborne/motor_bench/main_motor_bench.c b/sw/airborne/motor_bench/main_motor_bench.c index ae538b9586..ca4da5d764 100644 --- a/sw/airborne/motor_bench/main_motor_bench.c +++ b/sw/airborne/motor_bench/main_motor_bench.c @@ -7,7 +7,7 @@ #include "mb_tacho.h" #include "mb_servo.h" #include "i2c.h" -#include "mb_twi_controller.h" +#include "mb_twi_controller_asctech.h" #include "mb_current.h" #include "mb_scale.h" diff --git a/sw/airborne/motor_bench/mb_twi_controller_asctech.c b/sw/airborne/motor_bench/mb_twi_controller_asctech.c index a010a3508b..2ce8e8e57a 100644 --- a/sw/airborne/motor_bench/mb_twi_controller_asctech.c +++ b/sw/airborne/motor_bench/mb_twi_controller_asctech.c @@ -1,4 +1,4 @@ -#include "mb_twi_controller.h" +//#include "mb_twi_controller.h" #include "mb_twi_controller_asctech.h" #include "i2c.h" diff --git a/sw/airborne/motor_bench/mb_twi_controller_asctech.h b/sw/airborne/motor_bench/mb_twi_controller_asctech.h index c48b523738..4d5431affc 100644 --- a/sw/airborne/motor_bench/mb_twi_controller_asctech.h +++ b/sw/airborne/motor_bench/mb_twi_controller_asctech.h @@ -1,6 +1,10 @@ #ifndef MB_TWI_CONTROLLER_ASCTECH_H #define MB_TWI_CONTROLLER_ASCTECH_H +#include "std.h" + +extern void mb_twi_controller_init(void); +extern void mb_twi_controller_set( float throttle ); #define MB_TWI_CONTROLLER_COMMAND_NONE 0 #define MB_TWI_CONTROLLER_COMMAND_TEST 1 diff --git a/sw/simulator/booz_flight_model_params.h b/sw/simulator/booz_flight_model_params.h index 756f654f65..c30e6b83ce 100644 --- a/sw/simulator/booz_flight_model_params.h +++ b/sw/simulator/booz_flight_model_params.h @@ -16,7 +16,7 @@ /* gravity in m/s2 */ #define G 9.81 /* mass in kg */ -#define MASS 0.5 +#define MASS 0.724 /* inertia on x axis in kg * m2 */ #define Ix .007 /* inertia on y axis in kg * m2 */ @@ -35,9 +35,9 @@ */ #define BAT_VOLTAGE 11. -#define THAU 1. -#define Kq 0.079 -#define Kv 1000. +#define THAU 0.05 +#define Kq 0.12 +#define Kv 304. diff --git a/sw/simulator/booz_sensors_model.h b/sw/simulator/booz_sensors_model.h index 1c5f7ae17a..f98143d5fa 100644 --- a/sw/simulator/booz_sensors_model.h +++ b/sw/simulator/booz_sensors_model.h @@ -1,3 +1,27 @@ +/* + * $Id$ + * + * Copyright (C) 2008 Antoine Drouin + * + * This file is part of paparazzi. + * + * paparazzi is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 2, or (at your option) + * any later version. + * + * paparazzi is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * 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. + * + */ + #ifndef BOOZ_SENSORS_MODEL_H #define BOOZ_SENSORS_MODEL_H @@ -57,17 +81,25 @@ struct BoozSensorsModel { double rangemeter_next_update; int rangemeter_available; + /* Barometer */ double baro; unsigned int baro_resolution; double baro_next_update; int baro_available; + /* GPS */ VEC* gps_speed; + double gps_speed_course; + double gps_speed_gspeed; + double gps_speed_climb; VEC* gps_speed_noise_std_dev; GSList* gps_speed_history; VEC* gps_pos; + double gps_pos_utm_north; + double gps_pos_utm_east; + double gps_pos_utm_alt; VEC* gps_pos_noise_std_dev; VEC* gps_pos_bias_initial; VEC* gps_pos_bias_random_walk_std_dev; diff --git a/sw/simulator/booz_sensors_model_gps.c b/sw/simulator/booz_sensors_model_gps.c index 458f391732..25a5fb0159 100644 --- a/sw/simulator/booz_sensors_model_gps.c +++ b/sw/simulator/booz_sensors_model_gps.c @@ -57,7 +57,7 @@ void booz_sensors_model_gps_init( double time ) { } - +/* We store our positions like the fdm and convert to UTM and funcky course, gspeed, climb */ void booz_sensors_model_gps_run( double time, MAT* dcm_t ) { if (time < bsm.gps_next_update) return; @@ -78,6 +78,15 @@ void booz_sensors_model_gps_run( double time, MAT* dcm_t ) { cur_speed_reading); UpdateSensorLatency(time, cur_speed_reading, bsm.gps_speed_history, BSM_GPS_SPEED_LATENCY, bsm.gps_speed); + /* course, gspeed, climb convertion */ + bsm.gps_speed_course = DegOfRad(atan2(bsm.gps_speed->ve[AXIS_Y], bsm.gps_speed->ve[AXIS_X]))*10.; + bsm.gps_speed_course = rint(bsm.gps_speed_course); + bsm.gps_speed_gspeed = sqrt(bsm.gps_speed->ve[AXIS_X] * bsm.gps_speed->ve[AXIS_X] + + 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 */ @@ -103,6 +112,15 @@ void booz_sensors_model_gps_run( double time, MAT* dcm_t ) { UpdateSensorLatency(time, cur_pos_reading, bsm.gps_pos_history, BSM_GPS_POS_LATENCY, bsm.gps_pos); + /* UTM conversion */ + bsm.gps_pos_utm_north = bsm.gps_pos->ve[AXIS_X] * 100. + BSM_GPS_POS_INITIAL_UTM_EAST; + bsm.gps_pos_utm_north = rint(bsm.gps_pos_utm_north); + bsm.gps_pos_utm_east = bsm.gps_pos->ve[AXIS_Y] * 100. + BSM_GPS_POS_INITIAL_UTM_NORTH; + bsm.gps_pos_utm_east = rint(bsm.gps_pos_utm_east); + bsm.gps_pos_utm_alt = bsm.gps_pos->ve[AXIS_Z] * 100. + BSM_GPS_POS_INITIAL_UTM_ALT; + bsm.gps_pos_utm_alt = rint(bsm.gps_pos_utm_alt); + + bsm.gps_next_update += BSM_GPS_DT; bsm.gps_available = TRUE; diff --git a/sw/simulator/booz_sensors_model_params.h b/sw/simulator/booz_sensors_model_params.h index 0f176649f7..2cda7e577f 100644 --- a/sw/simulator/booz_sensors_model_params.h +++ b/sw/simulator/booz_sensors_model_params.h @@ -124,6 +124,10 @@ #define BSM_GPS_POS_BIAS_RANDOM_WALK_STD_DEV_Y 1e-1 #define BSM_GPS_POS_BIAS_RANDOM_WALK_STD_DEV_Z 1e-1 #define BSM_GPS_POS_LATENCY 0.25 +#define BSM_GPS_POS_INITIAL_UTM_EAST 37728000 +#define BSM_GPS_POS_INITIAL_UTM_NORTH 482464300 +#define BSM_GPS_POS_INITIAL_UTM_ALT 15200 + #define BSM_GPS_DT (1./4.) #endif /* BOOZ_SENSORS_MODEL_PARAMS_H */ diff --git a/sw/simulator/main_booz_sim.c b/sw/simulator/main_booz_sim.c index ca6fdad5ca..d022129800 100644 --- a/sw/simulator/main_booz_sim.c +++ b/sw/simulator/main_booz_sim.c @@ -1,7 +1,7 @@ /* * $Id$ * - * Copyright (C) 2008 Antoine Drouin + * Copyright (C) 2008 Antoine Drouin * * This file is part of paparazzi. * @@ -72,6 +72,7 @@ static void on_DL_SETTING(IvyClientPtr app, void *user_data, int argc, char *arg #include "booz_imu.h" +#include "gps.h" #include "booz_estimator.h" #include "radio_control.h" #include "actuators.h" @@ -84,13 +85,13 @@ static gboolean booz_sim_periodic(gpointer data __attribute__ ((unused))) { booz_sim_read_actuators(); /* run our models */ - if (sim_time > 3.) - bfm.on_ground = FALSE; + // if (sim_time > 3.) + // bfm.on_ground = FALSE; /* no fdm at start to allow for filter initialisation */ /* it sucks, I know */ booz_flight_model_run(DT, booz_sim_actuators_values); - booz_sensors_model_run(DT); + booz_sensors_model_run(sim_time); booz_wind_model_run(DT); @@ -100,23 +101,24 @@ static gboolean booz_sim_periodic(gpointer data __attribute__ ((unused))) { booz_filter_main_periodic_task(); /* feed sensors */ - if (booz_sensor_model_gyro_available()) { - max1167_hw_feed_value(sim_time, bsm.gyro, bsm.accel); + if (booz_sensors_model_gyro_available()) { + max1167_hw_feed_value(bsm.gyro, bsm.accel); booz_filter_main_event_task(); } - if (booz_sensor_model_mag_available()) { - micromag_hw_feed_value(sim_time, bsm.mag); + if (booz_sensors_model_mag_available()) { + micromag_hw_feed_value(bsm.mag); booz_filter_main_event_task(); } - if (booz_sensor_model_baro_available()) { - scp1000_hw_feed_value(sim_time, bsm.baro); + if (booz_sensors_model_baro_available()) { + scp1000_hw_feed_value(bsm.baro); booz_filter_main_event_task(); } - if (booz_sensor_model_gps_available()) { - // scp1000_hw_feed_value(sim_time, bsm.baro); + if (booz_sensors_model_gps_available()) { + gps_feed_values(bsm.gps_pos_utm_north, bsm.gps_pos_utm_east, bsm.gps_pos_utm_alt, + bsm.gps_speed_gspeed, bsm.gps_speed_course, bsm.gps_speed_climb); booz_filter_main_event_task(); } @@ -127,19 +129,6 @@ static gboolean booz_sim_periodic(gpointer data __attribute__ ((unused))) { /* process the BoozLinkMcuEvent */ /* this will update the controller estimator */ booz_controller_main_event_task(); -#if 0 - /* cheat in simulation : psi not available from filter yet */ - // booz_estimator_set_psi(bfm.state->ve[BFMS_PSI]); - /* in simulation compute dcm as a helper for for nav */ - booz_estimator_compute_dcm(); - /* in simulation feed speed and pos estimations ( with a pos sensor :( ) */ - booz_estimator_set_speed_and_pos(bsm.speed_sensor->ve[AXIS_X], - bsm.speed_sensor->ve[AXIS_Y], - bsm.speed_sensor->ve[AXIS_Z], - bsm.pos_sensor->ve[AXIS_X], - bsm.pos_sensor->ve[AXIS_Y], - bsm.pos_sensor->ve[AXIS_Z] ); -#endif /* post a radio control event */ booz_sim_set_ppm_from_joystick(); @@ -165,7 +154,7 @@ int main ( int argc, char** argv) { booz_flight_model_init(); - booz_sensors_model_init(); + booz_sensors_model_init(sim_time); booz_wind_model_init(); @@ -229,7 +218,7 @@ static inline void booz_sim_display(void) { DegOfRad(bsm.gyro_bias_random_walk_value->ve[AXIS_Q]), DegOfRad(bsm.gyro_bias_random_walk_value->ve[AXIS_R])); IvySendMsg("148 BOOZ_SIM_RANGE_METER %f", - bsm.range_meter); + bsm.rangemeter); // IvySendMsg("148 BOOZ_SIM_WIND %f %f %f", // bwm.velocity->ve[AXIS_X], // bwm.velocity->ve[AXIS_Y],