diff --git a/conf/messages.xml b/conf/messages.xml index 273c463bec..18b2938eff 100644 --- a/conf/messages.xml +++ b/conf/messages.xml @@ -489,9 +489,33 @@ - - - + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/conf/modules/airspeed_amsys.xml b/conf/modules/airspeed_amsys.xml new file mode 100755 index 0000000000..87e6d92d61 --- /dev/null +++ b/conf/modules/airspeed_amsys.xml @@ -0,0 +1,25 @@ + + + + + + +
+ +
+ + + + + + + + + diff --git a/conf/modules/baro_amsys.xml b/conf/modules/baro_amsys.xml new file mode 100755 index 0000000000..e7364b4a16 --- /dev/null +++ b/conf/modules/baro_amsys.xml @@ -0,0 +1,21 @@ + + + + + + +
+ +
+ + + + + + + + + diff --git a/conf/modules/flight_benchmark.xml b/conf/modules/flight_benchmark.xml new file mode 100644 index 0000000000..bbd59403bb --- /dev/null +++ b/conf/modules/flight_benchmark.xml @@ -0,0 +1,14 @@ + + + +
+ +
+ + + + + + + + diff --git a/conf/settings/airspeed_amsys.xml b/conf/settings/airspeed_amsys.xml new file mode 100755 index 0000000000..875cc5a9b7 --- /dev/null +++ b/conf/settings/airspeed_amsys.xml @@ -0,0 +1,12 @@ + + + + + + + + + + + + diff --git a/conf/settings/baro_amsys.xml b/conf/settings/baro_amsys.xml new file mode 100755 index 0000000000..162b81c06b --- /dev/null +++ b/conf/settings/baro_amsys.xml @@ -0,0 +1,11 @@ + + + + + + + + + + + diff --git a/conf/settings/benchmark.xml b/conf/settings/benchmark.xml new file mode 100755 index 0000000000..31d3b08c1e --- /dev/null +++ b/conf/settings/benchmark.xml @@ -0,0 +1,13 @@ + + + + + + + + + + + + + diff --git a/sw/airborne/modules/benchmark/flight_benchmark.c b/sw/airborne/modules/benchmark/flight_benchmark.c new file mode 100644 index 0000000000..a965c21b86 --- /dev/null +++ b/sw/airborne/modules/benchmark/flight_benchmark.c @@ -0,0 +1,138 @@ +//Author: Bruzzlee + +//This module allows a quantitative assessment of the flight. +//It calculates the sum of squared error of the two-dimensional course (x / y), the altitude and true airspeed. +//The sum of squared error of the course and altitude were separated, because they are regulated separately, and so they dependent on various parameters. +//The module was written to optimize the control parameters and has already been used successfully. + + +#include "firmwares/fixedwing/guidance/guidance_v.h" +#include "estimator.h" +#include "messages.h" +#include "downlink.h" +#include "mcu_periph/uart.h" +#include "generated/airframe.h" +#include "subsystems/nav.h" +// #include "math/pprz_algebra_int.h" +// #include "math/pprz_algebra_float.h" + +// For Downlink +#ifndef DOWNLINK_DEVICE +#define DOWNLINK_DEVICE DOWNLINK_AP_DEVICE +#endif + + + +float SquareSumErr_airspeed; +float SquareSumErr_altitude; +float SquareSumErr_position; +float ToleranceAispeed; +float ToleranceAltitude; +float TolerancePosition; +bool_t benchm_reset; +bool_t benchm_go; + + +//uint8_t numOfCount; + + + +void flight_benchmark_init( void ) { + SquareSumErr_airspeed = 0; + SquareSumErr_altitude = 0; + SquareSumErr_position = 0; + ToleranceAispeed = BENCHMARK_TOLERANCE_AIRSPEED; + ToleranceAltitude = BENCHMARK_TOLERANCE_ALTITUDE; + TolerancePosition = BENCHMARK_TOLERANCE_POSITION; + benchm_reset = 0; + benchm_go = 0; +} + +void flight_benchmark_periodic( void ) { + float Err_airspeed = 0; + float Err_altitude = 0; + float Err_position = 0; + + if (benchm_reset){ + flight_benchmark_reset(); + benchm_reset = 0; + } + + if (benchm_go){ + #if defined(USE_AIRSPEED) && defined(BENCHMARK_AIRSPEED) + Err_airspeed = fabs(estimator_airspeed - v_ctl_auto_airspeed_setpoint); + if (Err_airspeed>ToleranceAispeed){ + Err_airspeed = Err_airspeed-ToleranceAispeed; + SquareSumErr_airspeed += (Err_airspeed * Err_airspeed); + } + #endif + + #ifdef BENCHMARK_ALTITUDE + Err_altitude = fabs(estimator_z - v_ctl_altitude_setpoint); + if (Err_altitude>ToleranceAltitude){ + Err_altitude = Err_altitude-ToleranceAltitude; + SquareSumErr_altitude += (Err_altitude * Err_altitude); + } + #endif + + #ifdef BENCHMARK_POSITION + + //---------------This part is a waste of memory and calculation power - but it works - feel free to optimize it ;-) ----------------- + + // err_temp = waypoints[target].x - estimator_x; + float deltaPlaneX = 0; + float deltaPlaneY = 0; + float Err_position_segment = 0; + float Err_position_circle = 0; + +// if (nav_in_segment){ + float deltaX = nav_segment_x_2 - nav_segment_x_1; + float deltaY = nav_segment_y_2 - nav_segment_y_1; + float anglePath = atan2(deltaX,deltaY); + deltaPlaneX = nav_segment_x_2 - estimator_x; + deltaPlaneY = nav_segment_y_2 - estimator_y; + float anglePlane = atan2(deltaPlaneX,deltaPlaneY); + float angleDiff = fabs(anglePlane - anglePath); + Err_position_segment = fabs(sin(angleDiff)*sqrt(deltaPlaneX*deltaPlaneX+deltaPlaneY*deltaPlaneY)); +// } + +// if (nav_in_circle){ + deltaPlaneX = nav_circle_x - estimator_x; + deltaPlaneY = nav_circle_y - estimator_y; + Err_position_circle = fabs(sqrt(deltaPlaneX*deltaPlaneX+deltaPlaneY*deltaPlaneY)-nav_circle_radius); +// } + if (Err_position_circle < Err_position_segment){ + Err_position = Err_position_circle; + } + else + Err_position = Err_position_segment; + + if (Err_position>TolerancePosition){ + SquareSumErr_position += (Err_position * Err_position); + } + #endif + } + + DOWNLINK_SEND_FLIGHT_BENCHMARK(DefaultChannel, &SquareSumErr_airspeed, &SquareSumErr_altitude, &SquareSumErr_position, &Err_airspeed, &Err_altitude, &Err_position) + +} + +void flight_benchmark_reset( void ) { + SquareSumErr_airspeed = 0; + SquareSumErr_altitude = 0; + SquareSumErr_position = 0; +} + + + + + + + + + + + + + + diff --git a/sw/airborne/modules/benchmark/flight_benchmark.h b/sw/airborne/modules/benchmark/flight_benchmark.h new file mode 100644 index 0000000000..8bef0d3976 --- /dev/null +++ b/sw/airborne/modules/benchmark/flight_benchmark.h @@ -0,0 +1,16 @@ +#ifndef FLIGHTBENCHMARK_H +#define FLIGHTBENCHMARK_H + +#include + +void flight_benchmark_init( void ); +void flight_benchmark_periodic( void ); +void flight_benchmark_reset( void ); + +extern float ToleranceAispeed; +extern float ToleranceAltitude; +extern float TolerancePosition; +extern bool_t benchm_reset; +extern bool_t benchm_go; + +#endif /* FLIGHTBENCHMARK_H */ diff --git a/sw/airborne/modules/sensors/airspeed_amsys.c b/sw/airborne/modules/sensors/airspeed_amsys.c new file mode 100755 index 0000000000..7ed75a25cd --- /dev/null +++ b/sw/airborne/modules/sensors/airspeed_amsys.c @@ -0,0 +1,161 @@ +/* + * Driver for a Amsys Differential Presure Sensor I2C + * AMS 5812-0003-D + * + * Copyright (C) 2010 The Paparazzi Team + * + * 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. + */ + +#include "sensors/airspeed_amsys.h" +#include "estimator.h" +#include "mcu_periph/i2c.h" +#include "mcu_periph/uart.h" +#include "messages.h" +#include "downlink.h" +#include +//#include + +#ifndef USE_AIRSPEED +// Just a Warning --> We do't use it. +//#ifndef SENSOR_SYNC_SEND +//#warning either set USE_AIRSPEED or SENSOR_SYNC_SEND to use amsys_airspeed +//#endif +#endif + +#define AIRSPEED_AMSYS_ADDR 0xF4 // original F0 +#ifndef AIRSPEED_AMSYS_SCALE +#define AIRSPEED_AMSYS_SCALE 1 +#endif +#ifndef AIRSPEED_AMSYS_OFFSET +#define AIRSPEED_AMSYS_OFFSET 0 +#endif +#define AIRSPEED_AMSYS_OFFSET_MAX 29491 +#define AIRSPEED_AMSYS_OFFSET_MIN 3277 +#define AIRSPEED_AMSYS_OFFSET_NBSAMPLES_INIT 40 +#define AIRSPEED_AMSYS_OFFSET_NBSAMPLES_AVRG 60 +#define AIRSPEED_AMSYS_NBSAMPLES_AVRG 10 +#ifndef AIRSPEED_AMSYS_MAXPRESURE +#define AIRSPEED_AMSYS_MAXPRESURE 2068//2073 //Pascal +#endif +#ifndef AIRSPEED_AMSYS_I2C_DEV +#define AIRSPEED_AMSYS_I2C_DEV i2c0 +#endif +#ifdef MEASURE_AMSYS_TEMPERATURE +#define TEMPERATURE_AMSYS_OFFSET_MAX 29491 +#define TEMPERATURE_AMSYS_OFFSET_MIN 3277 +#define TEMPERATURE_AMSYS_MAX 110 +#define TEMPERATURE_AMSYS_MIN -25 +#endif + +#ifndef DOWNLINK_DEVICE +#define DOWNLINK_DEVICE DOWNLINK_AP_DEVICE +#endif + +// Global variables +uint16_t airspeed_amsys_raw; +uint16_t tempAS_amsys_raw; +bool_t airspeed_amsys_valid; +float airspeed_tmp; +float pressure_amsys; //Pascal +float airspeed_amsys; //mps +float airspeed_scale; +float airspeed_filter; +struct i2c_transaction airspeed_amsys_i2c_trans; + +// Local variables +volatile bool_t airspeed_amsys_i2c_done; +float airspeed_temperature = 0.0; +float airspeed_old = 0.0; + + +void airspeed_amsys_init( void ) { + airspeed_amsys_raw = 0; + airspeed_amsys = 0.0; + pressure_amsys = 0.0; + airspeed_amsys_i2c_done = TRUE; + airspeed_amsys_valid = TRUE; + airspeed_scale = AIRSPEED_SCALE; + airspeed_filter = AIRSPEED_FILTER; + airspeed_amsys_i2c_trans.status = I2CTransDone; +} + +void airspeed_amsys_read_periodic( void ) { +#ifndef SITL + if (airspeed_amsys_i2c_trans.status == I2CTransDone) +#ifndef MEASURE_AMSYS_TEMPERATURE + I2CReceive(AIRSPEED_AMSYS_I2C_DEV, airspeed_amsys_i2c_trans, AIRSPEED_AMSYS_ADDR, 2); +#else + I2CReceive(AIRSPEED_AMSYS_I2C_DEV, airspeed_amsys_i2c_trans, AIRSPEED_AMSYS_ADDR, 4); +#endif + +#else // SITL + extern float sim_air_speed; + EstimatorSetAirspeed(sim_air_speed); +#endif //SITL +} + +void airspeed_amsys_read_event( void ) { + + // Get raw airspeed from buffer + airspeed_amsys_raw = 0; + airspeed_amsys_raw = (airspeed_amsys_i2c_trans.buf[0]<<8) | airspeed_amsys_i2c_trans.buf[1]; +#ifdef MEASURE_AMSYS_TEMPERATURE + tempAS_amsys_raw = (airspeed_amsys_i2c_trans.buf[2]<<8) | airspeed_amsys_i2c_trans.buf[3]; + airspeed_temperature = (float)((float)(tempAS_amsys_raw-TEMPERATURE_AMSYS_OFFSET_MIN)/((float)(TEMPERATURE_AMSYS_OFFSET_MAX-TEMPERATURE_AMSYS_OFFSET_MIN)/TEMPERATURE_AMSYS_MAX)+TEMPERATURE_AMSYS_MIN);// Tmin=-25, Tmax=85 +#endif + + // Check if this is valid airspeed + if (airspeed_amsys_raw == 0) + airspeed_amsys_valid = FALSE; + else + airspeed_amsys_valid = TRUE; + + // Continue only if a new airspeed value was received + if (airspeed_amsys_valid) { + + // raw not under offest min + if (airspeed_amsys_rawAIRSPEED_AMSYS_OFFSET_MAX) + airspeed_amsys_raw = AIRSPEED_AMSYS_OFFSET_MAX; + + // calculate raw to pressure + pressure_amsys = (float)(airspeed_amsys_raw-AIRSPEED_AMSYS_OFFSET_MIN)*AIRSPEED_AMSYS_MAXPRESURE/(float)(AIRSPEED_AMSYS_OFFSET_MAX-AIRSPEED_AMSYS_OFFSET_MIN); + + airspeed_tmp = sqrtf(2*(pressure_amsys)*airspeed_scale/1.2041); //without offset + + // Lowpass filter + airspeed_amsys = airspeed_filter * airspeed_old + (1 - airspeed_filter) * airspeed_tmp; + airspeed_old = airspeed_amsys; + +#ifdef USE_AIRSPEED + EstimatorSetAirspeed(airspeed_amsys); +#endif +#ifdef SENSOR_SYNC_SEND + DOWNLINK_SEND_AMSYS_AIRSPEED(DefaultChannel, &airspeed_amsys_raw, &pressure_amsys, &airspeed_tmp, &airspeed_amsys, &airspeed_temperature); +#else + RunOnceEvery(10, DOWNLINK_SEND_AMSYS_AIRSPEED(DefaultChannel, &airspeed_amsys_raw, &pressure_amsys, &airspeed_tmp, &airspeed_amsys, &airspeed_temperature)); +#endif + } + + // Transaction has been read + airspeed_amsys_i2c_trans.status = I2CTransDone; +} + diff --git a/sw/airborne/modules/sensors/airspeed_amsys.h b/sw/airborne/modules/sensors/airspeed_amsys.h new file mode 100755 index 0000000000..ac518b750a --- /dev/null +++ b/sw/airborne/modules/sensors/airspeed_amsys.h @@ -0,0 +1,42 @@ +/* + * Driver for a Amsys Differential Presure Sensor I2C + * AMS 5812-0003-D + * + * Copyright (C) 2010 The Paparazzi Team + * + * 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 AIRSPEED_AMSYS_H +#define AIRSPEED_AMSYS_H + +#include "std.h" +#include "mcu_periph/i2c.h" + +extern float airspeed_scale; +extern float airspeed_filter; + +extern struct i2c_transaction airspeed_amsys_i2c_trans; + +extern void airspeed_amsys_init( void ); +extern void airspeed_amsys_read_periodic( void ); +extern void airspeed_amsys_read_event( void ); + +#define AirspeedAmsysEvent() { if (airspeed_amsys_i2c_trans.status == I2CTransSuccess) airspeed_amsys_read_event(); } + +#endif // AIRSPEED_AMSYS_H diff --git a/sw/airborne/modules/sensors/baro_amsys.c b/sw/airborne/modules/sensors/baro_amsys.c new file mode 100755 index 0000000000..64102bc0eb --- /dev/null +++ b/sw/airborne/modules/sensors/baro_amsys.c @@ -0,0 +1,202 @@ +/* + * Driver for a Amsys Barometric Sensor I2C + * AMS 5812-0150-A + * ----measuring only--- + * + * Copyright (C) 2010 The Paparazzi Team + * + * 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. + */ + +#include "sensors/baro_amsys.h" +#include "mcu_periph/i2c.h" +#include "estimator.h" +#include +#include "generated/flight_plan.h" // for ground alt + +#ifdef SITL +#include "subsystems/gps.h" +#endif + +//Messages +#include "mcu_periph/uart.h" +#include "messages.h" +#include "downlink.h" +//#include "gps.h" +#ifndef DOWNLINK_DEVICE +#define DOWNLINK_DEVICE DOWNLINK_AP_DEVICE +#endif + +#ifdef SITL +#include "gps.h" +#endif + +#define BARO_AMSYS_ADDR 0xF2 +#define BARO_AMSYS_REG 0x07 +#define BARO_AMSYS_SCALE 0.32 +#define BARO_AMSYS_MAX_PRESSURE 103400 // Pascal +#define BARO_AMSYS_OFFSET_MAX 29491 +#define BARO_AMSYS_OFFSET_MIN 3277 +#define BARO_AMSYS_OFFSET_NBSAMPLES_INIT 40 +#define BARO_AMSYS_OFFSET_NBSAMPLES_AVRG 60 + +#ifdef MEASURE_AMSYS_TEMPERATURE +#define TEMPERATURE_AMSYS_OFFSET_MAX 29491 +#define TEMPERATURE_AMSYS_OFFSET_MIN 3277 +#define TEMPERATURE_AMSYS_MAX 110 +#define TEMPERATURE_AMSYS_MIN -25 +#endif + +//#define BARO_AMSYS_R 0.5 +//#define BARO_AMSYS_SIGMA2 0.1 +//#define BARO_ALT_CORRECTION 0.0 +#ifndef BARO_AMSYS_I2C_DEV +#define BARO_AMSYS_I2C_DEV i2c0 +#endif + +// Global variables +uint16_t pBaroRaw; +uint16_t tBaroRaw; +float baro_amsys_offset; +bool_t baro_amsys_valid; +float baro_amsys_altitude; +float baro_amsys_temp; +float baro_amsys_p; +float baro_amsys_offset_altitude; +float baro_amsys_abs_altitude; +float ref_alt_init; //Altitude by initialising +float baro_filter; +float baro_old; + +//float baro_amsys_r; +//float baro_amsys_sigma2; + + +struct i2c_transaction baro_amsys_i2c_trans; + +// Local variables +bool_t baro_amsys_offset_init; +double baro_amsys_offset_tmp; +uint16_t baro_amsys_cnt; + +void baro_amsys_init( void ) { + baro_filter=BARO_FILTER; + pBaroRaw = 0; + tBaroRaw = 0; + baro_amsys_altitude = 0.0; + baro_amsys_p=0.0; + baro_amsys_offset = 0; + baro_amsys_offset_tmp = 0; + baro_amsys_valid = TRUE; + baro_amsys_offset_init = FALSE; +// baro_amsys_enabled = TRUE; + baro_amsys_cnt = BARO_AMSYS_OFFSET_NBSAMPLES_INIT + BARO_AMSYS_OFFSET_NBSAMPLES_AVRG; +// baro_amsys_r = BARO_AMSYS_R; +// baro_amsys_sigma2 = BARO_AMSYS_SIGMA2; +// baro_head=0; + ref_alt_init = 0; + baro_amsys_i2c_trans.status = I2CTransDone; +} + +void baro_amsys_read_periodic( void ) { + // Initiate next read +#ifndef SITL + if (baro_amsys_i2c_trans.status == I2CTransDone){ +#ifndef MEASURE_AMSYS_TEMPERATURE + I2CReceive(BARO_AMSYS_I2C_DEV, baro_amsys_i2c_trans, BARO_AMSYS_ADDR, 2); +#else + I2CReceive(BARO_AMSYS_I2C_DEV, baro_amsys_i2c_trans, BARO_AMSYS_ADDR, 4); +#endif + } +#else // SITL + pBaroRaw = 0; + baro_amsys_altitude = gps.hmsl / 1000.0; + baro_amsys_valid = TRUE; + EstimatorSetAlt(baro_amsys_altitude); +#endif +} + +void baro_amsys_read_event( void ) { + pBaroRaw = 0; + // Get raw altimeter from buffer + pBaroRaw = (baro_amsys_i2c_trans.buf[0] << 8) | baro_amsys_i2c_trans.buf[1]; +#ifdef MEASURE_AMSYS_TEMPERATURE + tBaroRaw = (baro_amsys_i2c_trans.buf[2] << 8) | baro_amsys_i2c_trans.buf[3]; + baro_amsys_temp = (float)(tBaroRaw-TEMPERATURE_AMSYS_OFFSET_MIN)*TEMPERATURE_AMSYS_MAX/(float)(TEMPERATURE_AMSYS_OFFSET_MAX-TEMPERATURE_AMSYS_OFFSET_MIN)+(float)TEMPERATURE_AMSYS_MIN; +#endif + // Check if this is valid altimeter + if (pBaroRaw == 0) + baro_amsys_valid = FALSE; + else + baro_amsys_valid = TRUE; + + + // Continue only if a new altimeter value was received + //if (baro_amsys_valid && GpsFixValid()) { + if (baro_amsys_valid) { + //Cut RAW Min and Max + if (pBaroRaw < BARO_AMSYS_OFFSET_MIN) + pBaroRaw = BARO_AMSYS_OFFSET_MIN; + if (pBaroRaw > BARO_AMSYS_OFFSET_MAX) + pBaroRaw = BARO_AMSYS_OFFSET_MAX; + + //Convert to pressure + baro_amsys_p = (float)(pBaroRaw-BARO_AMSYS_OFFSET_MIN)*BARO_AMSYS_MAX_PRESSURE/(float)(BARO_AMSYS_OFFSET_MAX-BARO_AMSYS_OFFSET_MIN); + if (!baro_amsys_offset_init) { + --baro_amsys_cnt; + // Check if averaging completed + if (baro_amsys_cnt == 0) { + // Calculate average + baro_amsys_offset = (float)(baro_amsys_offset_tmp / BARO_AMSYS_OFFSET_NBSAMPLES_AVRG); + ref_alt_init = GROUND_ALT; + baro_amsys_offset_init = TRUE; + + // hight over Sea level at init point + //baro_amsys_offset_altitude = 288.15 / 0.0065 * (1 - pow((baro_amsys_p)/1013.25 , 1/5.255)); + } + // Check if averaging needs to continue + else if (baro_amsys_cnt <= BARO_AMSYS_OFFSET_NBSAMPLES_AVRG) + baro_amsys_offset_tmp += baro_amsys_p; + + baro_amsys_altitude = 0.0; + + } + else { + // Lowpassfiltering and convert pressure to altitude + baro_amsys_altitude = baro_filter * baro_old + (1 - baro_filter) * (baro_amsys_offset-baro_amsys_p)/(1.2041*9.81); + baro_old = baro_amsys_altitude; + + + //New value available + //EstimatorSetAlt(baro_amsys_abs_altitude); + } + baro_amsys_abs_altitude=baro_amsys_altitude+ref_alt_init; + } /*else { + baro_amsys_abs_altitude = 0.0; + }*/ + + + // Transaction has been read + baro_amsys_i2c_trans.status = I2CTransDone; +#ifdef SENSOR_SYNC_SEND + DOWNLINK_SEND_AMSYS_BARO(DefaultChannel, &pBaroRaw, &baro_amsys_p, &baro_amsys_offset, &ref_alt_init, &baro_amsys_abs_altitude, &baro_amsys_altitude, &baro_amsys_temp) +#else + RunOnceEvery(10, DOWNLINK_SEND_AMSYS_BARO(DefaultChannel, &pBaroRaw, &baro_amsys_p, &baro_amsys_offset, &ref_alt_init, &baro_amsys_abs_altitude, &baro_amsys_altitude, &baro_amsys_temp)); +#endif + +} diff --git a/sw/airborne/modules/sensors/baro_amsys.h b/sw/airborne/modules/sensors/baro_amsys.h new file mode 100755 index 0000000000..9e0884944d --- /dev/null +++ b/sw/airborne/modules/sensors/baro_amsys.h @@ -0,0 +1,51 @@ +/* + * Driver for a Amsys Barometric Sensor I2C + * AMS 5812-0150-A + * + * Copyright (C) 2010 The Paparazzi Team + * + * 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 BARO_AMSYS_H +#define BARO_AMSYS_H + +#include "std.h" +#include "mcu_periph/i2c.h" + +#define BARO_AMSYS_DT 0.05 + +// extern uint16_t baro_amsys_adc; +// extern float baro_amsys_offset; +// extern bool_t baro_amsys_valid; +// extern bool_t baro_amsys_updated; +// extern bool_t baro_amsys_enabled; +extern float baro_amsys_altitude; +// extern float baro_amsys_r; +// extern float baro_amsys_sigma2; +extern float baro_filter; + +extern struct i2c_transaction baro_amsys_i2c_trans; + +extern void baro_amsys_init( void ); +extern void baro_amsys_read_periodic( void ); +extern void baro_amsys_read_event( void ); + +#define BaroAmsysEvent() { if (baro_amsys_i2c_trans.status == I2CTransSuccess) baro_amsys_read_event(); } + +#endif // BARO_AMSYS_H diff --git a/sw/tools/gen_flight_plan.ml b/sw/tools/gen_flight_plan.ml index bf24fff948..34378230e1 100644 --- a/sw/tools/gen_flight_plan.ml +++ b/sw/tools/gen_flight_plan.ml @@ -437,6 +437,13 @@ let rec print_stage = fun index_of_waypoints x -> let r = parsed_attrib x "radius" in let _vmode = output_vmode x center "" in lprintf "Eight(%s, %s, %s);\n" center turn_about r; + begin + try + let c = parsed_attrib x "until" in + lprintf "if (%s) NextStageAndBreak();\n" c + with + ExtXml.Error _ -> () + end; lprintf "break;\n" | "oval" -> stage ();