diff --git a/conf/modules/airspeed_ets.xml b/conf/modules/airspeed_ets.xml new file mode 100644 index 0000000000..dfe3695c24 --- /dev/null +++ b/conf/modules/airspeed_ets.xml @@ -0,0 +1,23 @@ + + + + + + +
+ +
+ + + + + + + + + + diff --git a/conf/modules/baro_ets.xml b/conf/modules/baro_ets.xml new file mode 100644 index 0000000000..9a8003de05 --- /dev/null +++ b/conf/modules/baro_ets.xml @@ -0,0 +1,21 @@ + + + + + + +
+ +
+ + + + + + + + + + diff --git a/sw/airborne/modules/sensors/airspeed_ets.c b/sw/airborne/modules/sensors/airspeed_ets.c new file mode 100644 index 0000000000..29939da85e --- /dev/null +++ b/sw/airborne/modules/sensors/airspeed_ets.c @@ -0,0 +1,163 @@ +/* + * Driver for the EagleTree Systems Airspeed Sensor + * Has only been tested with V3 of the sensor hardware + * + * Notes: + * Connect directly to TWOG/Tiny I2C port. Multiple sensors can be chained together. + * Sensor should be in the proprietary mode (default) and not in 3rd party mode. + * + * Sensor module wire assignments: + * Red wire: 5V + * White wire: Ground + * Yellow wire: SDA + * Brown wire: SCL + * + * Copyright (C) 2009 Vassilis Varveropoulos + * Modified by Mark Griffin on 8 September 2010 to work with new i2c transaction routines. + * Converted by Gautier Hattenberger to modules (10/2010) + * + * 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_ets.h" +#include "estimator.h" +#include + +#define AIRSPEED_ETS_ADDR 0xEA +#ifndef AIRSPEED_ETS_SCALE +#define AIRSPEED_ETS_SCALE 1.8 +#endif +#ifndef AIRSPEED_ETS_OFFSET +#define AIRSPEED_ETS_OFFSET 0 +#endif +#define AIRSPEED_ETS_OFFSET_MAX 1750 +#define AIRSPEED_ETS_OFFSET_MIN 1450 +#define AIRSPEED_ETS_OFFSET_NBSAMPLES_INIT 40 +#define AIRSPEED_ETS_OFFSET_NBSAMPLES_AVRG 60 +#define AIRSPEED_ETS_NBSAMPLES_AVRG 10 + +// Global variables +uint16_t airspeed_ets_raw; +uint16_t airspeed_ets_offset; +bool_t airspeed_ets_valid; +float airspeed_ets; +int airspeed_ets_buffer_idx; +float airspeed_ets_buffer[AIRSPEED_ETS_NBSAMPLES_AVRG]; + +struct i2c_transaction airspeed_ets_i2c_trans; + +// Local variables +volatile bool_t airspeed_ets_i2c_done; +bool_t airspeed_ets_offset_init; +uint32_t airspeed_ets_offset_tmp; +uint16_t airspeed_ets_cnt; + +void airspeed_ets_init( void ) { + int n; + airspeed_ets_raw = 0; + airspeed_ets = 0.0; + airspeed_ets_offset = 0; + airspeed_ets_offset_tmp = 0; + airspeed_ets_i2c_done = TRUE; + airspeed_ets_valid = TRUE; + airspeed_ets_offset_init = FALSE; + airspeed_ets_cnt = AIRSPEED_ETS_OFFSET_NBSAMPLES_INIT + AIRSPEED_ETS_OFFSET_NBSAMPLES_AVRG; + + airspeed_ets_buffer_idx = 0; + for (n=0; n < AIRSPEED_ETS_NBSAMPLES_AVRG; ++n) + airspeed_ets_buffer[n] = 0.0; + + airspeed_ets_i2c_trans.status = I2CTransSuccess; + airspeed_ets_i2c_trans.slave_addr = AIRSPEED_ETS_ADDR; + airspeed_ets_i2c_trans.stop_after_transmit = TRUE; +} + +void airspeed_ets_read_periodic( void ) { +#ifndef SITL + if (airspeed_ets_i2c_trans.status == I2CTransSuccess) { + airspeed_ets_i2c_trans.type = I2CTransRx; + airspeed_ets_i2c_trans.len_r = 2; + i2c_submit(&i2c0, &airspeed_ets_i2c_trans); + } +#else // SITL + extern float sim_air_speed; + EstimatorSetAirspeed(sim_air_speed); +#endif //SITL +} + +void airspeed_ets_read_event( void ) { + int n; + float airspeed_tmp = 0.0; + + // Get raw airspeed from buffer + airspeed_ets_raw = ((uint16_t)(airspeed_ets_i2c_trans.buf[1]) << 8) | (uint16_t)(airspeed_ets_i2c_trans.buf[0]); + // Check if this is valid airspeed + if (airspeed_ets_raw == 0) + airspeed_ets_valid = FALSE; + else + airspeed_ets_valid = TRUE; + + // Continue only if a new airspeed value was received + if (airspeed_ets_valid) { + // Calculate offset average if not done already + if (!airspeed_ets_offset_init) { + --airspeed_ets_cnt; + // Check if averaging completed + if (airspeed_ets_cnt == 0) { + // Calculate average + airspeed_ets_offset = (uint16_t)(airspeed_ets_offset_tmp / AIRSPEED_ETS_OFFSET_NBSAMPLES_AVRG); + // Limit offset + if (airspeed_ets_offset < AIRSPEED_ETS_OFFSET_MIN) + airspeed_ets_offset = AIRSPEED_ETS_OFFSET_MIN; + if (airspeed_ets_offset > AIRSPEED_ETS_OFFSET_MAX) + airspeed_ets_offset = AIRSPEED_ETS_OFFSET_MAX; + airspeed_ets_offset_init = TRUE; + } + // Check if averaging needs to continue + else if (airspeed_ets_cnt <= AIRSPEED_ETS_OFFSET_NBSAMPLES_AVRG) + airspeed_ets_offset_tmp += airspeed_ets_raw; + } + // Convert raw to m/s + if (airspeed_ets_offset_init && airspeed_ets_raw > airspeed_ets_offset) + airspeed_tmp = AIRSPEED_ETS_SCALE * sqrtf( (float)(airspeed_ets_raw-airspeed_ets_offset) ) - AIRSPEED_ETS_OFFSET; + else + airspeed_tmp = 0.0; + // Airspeed should always be positive + if (airspeed_tmp < 0.0) + airspeed_tmp = 0.0; + // Moving average + airspeed_ets_buffer[airspeed_ets_buffer_idx++] = airspeed_tmp; + if (airspeed_ets_buffer_idx >= AIRSPEED_ETS_NBSAMPLES_AVRG) + airspeed_ets_buffer_idx = 0; + airspeed_ets = 0.0; + for (n = 0; n < AIRSPEED_ETS_NBSAMPLES_AVRG; ++n) + airspeed_ets += airspeed_ets_buffer[n]; + airspeed_ets = airspeed_ets / (float)AIRSPEED_ETS_NBSAMPLES_AVRG; + EstimatorSetAirspeed(airspeed_ets); + } else { + airspeed_ets = 0.0; + } + + // Transaction has been read + airspeed_ets_i2c_trans.status = I2CTransDone; + +} + + + + diff --git a/sw/airborne/modules/sensors/airspeed_ets.h b/sw/airborne/modules/sensors/airspeed_ets.h new file mode 100644 index 0000000000..6ce9db430c --- /dev/null +++ b/sw/airborne/modules/sensors/airspeed_ets.h @@ -0,0 +1,57 @@ +/* + * Driver for the EagleTree Systems Airspeed Sensor + * Has only been tested with V3 of the sensor hardware + * + * Notes: + * Connect directly to TWOG/Tiny I2C port. Multiple sensors can be chained together. + * Sensor should be in the proprietary mode (default) and not in 3rd party mode. + * + * Sensor module wire assignments: + * Red wire: 5V + * White wire: Ground + * Yellow wire: SDA + * Brown wire: SCL + * + * Copyright (C) 2009 Vassilis Varveropoulos + * Modified by Mark Griffin on 8 September 2010 to work with new i2c transaction routines. + * Converted by Gautier Hattenberger to modules (10/2010) + * + * 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_ETS_H +#define AIRSPEED_ETS_H + +#include "std.h" +#include "i2c.h" + +extern uint16_t airspeed_ets_raw; +extern uint16_t airspeed_ets_offset; +extern bool_t airspeed_ets_valid; +extern float airspeed_ets; + +extern struct i2c_transaction airspeed_ets_i2c_trans; + +extern void airspeed_ets_init( void ); +extern void airspeed_ets_read_periodic( void ); +extern void airspeed_ets_read_event( void ); + +#define AirspeedEtsEvent() { if (airspeed_ets_i2c_trans.status == I2CTransSuccess) airspeed_ets_read_event(); } + +#endif // AIRSPEED_ETS_H diff --git a/sw/airborne/modules/sensors/baro_ets.c b/sw/airborne/modules/sensors/baro_ets.c new file mode 100644 index 0000000000..0d3064b11e --- /dev/null +++ b/sw/airborne/modules/sensors/baro_ets.c @@ -0,0 +1,142 @@ +/* + * Driver for the EagleTree Systems Altitude Sensor + * Has only been tested with V3 of the sensor hardware + * + * Notes: + * Connect directly to TWOG/Tiny I2C port. Multiple sensors can be chained together. + * Sensor should be in the proprietary mode (default) and not in 3rd party mode. + * Pitch gains may need to be updated. + * + * + * Sensor module wire assignments: + * Red wire: 5V + * White wire: Ground + * Yellow wire: SDA + * Brown wire: SCL + * + * Copyright (C) 2009 Vassilis Varveropoulos + * 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_ets.h" +#include "i2c.h" +#include "estimator.h" +#include + +#ifdef SITL +#include "gps.h" +#endif + +#define BARO_ETS_ADDR 0xE8 +#define BARO_ETS_REG 0x07 +#define BARO_ETS_SCALE 0.32 +#define BARO_ETS_OFFSET_MAX 30000 +#define BARO_ETS_OFFSET_MIN 10 +#define BARO_ETS_OFFSET_NBSAMPLES_INIT 20 +#define BARO_ETS_OFFSET_NBSAMPLES_AVRG 40 +#define BARO_ETS_R 0.5 +#define BARO_ETS_SIGMA2 0.1 + +// Global variables +uint16_t baro_ets_adc; +uint16_t baro_ets_offset; +bool_t baro_ets_valid; +float baro_ets_altitude; +bool_t baro_ets_enabled; +float baro_ets_r; +float baro_ets_sigma2; + +struct i2c_transaction baro_ets_i2c_trans; + +// Local variables +bool_t baro_ets_offset_init; +uint32_t baro_ets_offset_tmp; +uint16_t baro_ets_cnt; + +void baro_ets_init( void ) { + baro_ets_adc = 0; + baro_ets_altitude = 0.0; + baro_ets_offset = 0; + baro_ets_offset_tmp = 0; + baro_ets_valid = TRUE; + baro_ets_offset_init = FALSE; + baro_ets_enabled = TRUE; + baro_ets_cnt = BARO_ETS_OFFSET_NBSAMPLES_INIT + BARO_ETS_OFFSET_NBSAMPLES_AVRG; + baro_ets_r = BARO_ETS_R; + baro_ets_sigma2 = BARO_ETS_SIGMA2; +} + +void baro_ets_read_periodic( void ) { + // Initiate next read +#ifndef SITL + I2CReceive(i2c0, baro_ets_i2c_trans, BARO_ETS_ADDR, 2); +#else // SITL + baro_ets_adc = 0; + baro_ets_altitude = gps_alt / 100.0; + baro_ets_valid = TRUE; + EstimatorSetAlt(baro_ets_altitude); +#endif +} + +void baro_ets_read_event( void ) { + // Get raw altimeter from buffer + baro_ets_adc = ((uint16_t)(baro_ets_i2c_trans.buf[1]) << 8) | (uint16_t)(baro_ets_i2c_trans.buf[0]); + // Check if this is valid altimeter + if (baro_ets_adc == 0) + baro_ets_valid = FALSE; + else + baro_ets_valid = TRUE; + + // Continue only if a new altimeter value was received + if (baro_ets_valid) { + // Calculate offset average if not done already + if (!baro_ets_offset_init) { + --baro_ets_cnt; + // Check if averaging completed + if (baro_ets_cnt == 0) { + // Calculate average + baro_ets_offset = (uint16_t)(baro_ets_offset_tmp / BARO_ETS_OFFSET_NBSAMPLES_AVRG); + // Limit offset + if (baro_ets_offset < BARO_ETS_OFFSET_MIN) + baro_ets_offset = BARO_ETS_OFFSET_MIN; + if (baro_ets_offset > BARO_ETS_OFFSET_MAX) + baro_ets_offset = BARO_ETS_OFFSET_MAX; + baro_ets_offset_init = TRUE; + } + // Check if averaging needs to continue + else if (baro_ets_cnt <= BARO_ETS_OFFSET_NBSAMPLES_AVRG) + baro_ets_offset_tmp += baro_ets_adc; + } + // Convert raw to m/s + if (baro_ets_offset_init) + baro_ets_altitude = BARO_ETS_SCALE * (float)(baro_ets_offset-baro_ets_adc); + else + baro_ets_altitude = 0.0; + // New value available + EstimatorSetAlt(baro_ets_altitude); + } else { + baro_ets_altitude = 0.0; + } + + // Transaction has been read + baro_ets_i2c_trans.status = I2CTransDone; + +} + diff --git a/sw/airborne/modules/sensors/baro_ets.h b/sw/airborne/modules/sensors/baro_ets.h new file mode 100644 index 0000000000..337c4c041c --- /dev/null +++ b/sw/airborne/modules/sensors/baro_ets.h @@ -0,0 +1,63 @@ +/* + * Driver for the EagleTree Systems Altitude Sensor + * Has only been tested with V3 of the sensor hardware + * + * Notes: + * Connect directly to TWOG/Tiny I2C port. Multiple sensors can be chained together. + * Sensor should be in the proprietary mode (default) and not in 3rd party mode. + * Pitch gains may need to be updated. + * + * + * Sensor module wire assignments: + * Red wire: 5V + * White wire: Ground + * Yellow wire: SDA + * Brown wire: SCL + * + * Copyright (C) 2009 Vassilis Varveropoulos + * 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_ETS_H +#define BARO_ETS_H + +#include "std.h" +#include "i2c.h" + +#define BARO_ETS_DT 0.05 + +extern uint16_t baro_ets_adc; +extern uint16_t baro_ets_offset; +extern bool_t baro_ets_valid; +extern bool_t baro_ets_updated; +extern bool_t baro_ets_enabled; +extern float baro_ets_altitude; +extern float baro_ets_r; +extern float baro_ets_sigma2; + +extern struct i2c_transaction baro_ets_i2c_trans; + +extern void baro_ets_init( void ); +extern void baro_ets_read_periodic( void ); +extern void baro_ets_read_event( void ); + +#define BaroEtsEvent() { if (baro_ets_i2c_trans.status == I2CTransSuccess) baro_ets_read_event(); } + +#endif // BARO_ETS_H