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

*** empty log message ***

Browse Source
This commit is contained in:
Antoine Drouin
2008-04-10 15:08:14 +00:00
parent d48bf6ab33
commit 1e398a2f34
10 changed files with 87 additions and 40 deletions
Download Patch File Download Diff File Expand all files Collapse all files
sw/airborne/AMI601.c
+4 -4
View File
@@ -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;
sw/airborne/AMI601.h
+2 -2
View File
@@ -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; \
} \
sw/airborne/motor_bench/main_motor_bench.c
+1 -1
View File
@@ -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"
sw/airborne/motor_bench/mb_twi_controller_asctech.c
+1 -1
View File
@@ -1,4 +1,4 @@
#include "mb_twi_controller.h"
//#include "mb_twi_controller.h"
#include "mb_twi_controller_asctech.h"
#include "i2c.h"
sw/airborne/motor_bench/mb_twi_controller_asctech.h
+4
View File
@@ -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
sw/simulator/booz_flight_model_params.h
+4 -4
View File
@@ -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.
sw/simulator/booz_sensors_model.h
+32
View File
@@ -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;
sw/simulator/booz_sensors_model_gps.c
+19 -1
View File
@@ -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;
sw/simulator/booz_sensors_model_params.h
+4
View File
@@ -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 */
sw/simulator/main_booz_sim.c
+16 -27
View File
@@ -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],