diff --git a/conf/airframes/AGGIEAIR/aggieair_iris_indi.xml b/conf/airframes/AGGIEAIR/aggieair_iris_indi.xml index 3ffe2d87d3..aaae1aa14b 100644 --- a/conf/airframes/AGGIEAIR/aggieair_iris_indi.xml +++ b/conf/airframes/AGGIEAIR/aggieair_iris_indi.xml @@ -90,8 +90,6 @@ - - + diff --git a/conf/modules/lidar_lite.xml b/conf/modules/lidar_lite.xml index 376174576c..7e1715aadd 100644 --- a/conf/modules/lidar_lite.xml +++ b/conf/modules/lidar_lite.xml @@ -11,6 +11,7 @@ + @@ -35,11 +36,13 @@ + + diff --git a/conf/modules/px4flow_i2c.xml b/conf/modules/px4flow_i2c.xml index b5f7c8ba7d..86c874a756 100644 --- a/conf/modules/px4flow_i2c.xml +++ b/conf/modules/px4flow_i2c.xml @@ -9,6 +9,7 @@ + @@ -34,6 +35,7 @@ + @@ -41,6 +43,7 @@ + diff --git a/conf/modules/sonar_bebop.xml b/conf/modules/sonar_bebop.xml index a363397f0a..1efa68a94c 100644 --- a/conf/modules/sonar_bebop.xml +++ b/conf/modules/sonar_bebop.xml @@ -7,6 +7,7 @@ Reads an anlog sonar sensor and outputs sonar distance in [m] +
@@ -21,7 +22,8 @@ - + + include $(CFG_SHARED)/spi_master.makefile diff --git a/sw/airborne/boards/ardrone/baro_board.c b/sw/airborne/boards/ardrone/baro_board.c index 208c6f4404..137c5ce0c3 100644 --- a/sw/airborne/boards/ardrone/baro_board.c +++ b/sw/airborne/boards/ardrone/baro_board.c @@ -44,7 +44,7 @@ struct MedianFilterInt baro_median; void baro_init(void) { #if USE_BARO_MEDIAN_FILTER - init_median_filter(&baro_median); + init_median_filter_i(&baro_median, MEDIAN_DEFAULT_SIZE); #endif } @@ -98,7 +98,7 @@ void ardrone_baro_event(void) AbiSendMsgTEMPERATURE(BARO_BOARD_SENDER_ID, temp_deg); int32_t press_pascal = baro_apply_calibration(navdata.measure.pressure); #if USE_BARO_MEDIAN_FILTER - press_pascal = update_median_filter(&baro_median, press_pascal); + press_pascal = update_median_filter_i(&baro_median, press_pascal); #endif float pressure = (float)press_pascal; AbiSendMsgBARO_ABS(BARO_BOARD_SENDER_ID, pressure); diff --git a/sw/airborne/boards/baro_board_ms5611_i2c.c b/sw/airborne/boards/baro_board_ms5611_i2c.c index df62004f81..791b8b8b7b 100644 --- a/sw/airborne/boards/baro_board_ms5611_i2c.c +++ b/sw/airborne/boards/baro_board_ms5611_i2c.c @@ -28,6 +28,7 @@ */ #include "subsystems/sensors/baro.h" +#include "boards/bebop/baro_board.h" #include "peripherals/ms5611_i2c.h" #include "mcu_periph/sys_time.h" diff --git a/sw/airborne/boards/krooz/imu_krooz.c b/sw/airborne/boards/krooz/imu_krooz.c index 21dc0a5c81..a30a68911b 100644 --- a/sw/airborne/boards/krooz/imu_krooz.c +++ b/sw/airborne/boards/krooz/imu_krooz.c @@ -74,9 +74,9 @@ void imu_krooz_init(void) // Init median filters #if IMU_KROOZ_USE_ACCEL_MEDIAN_FILTER - InitMedianFilterVect3Int(median_accel); + InitMedianFilterVect3Int(median_accel, MEDIAN_DEFAULT_SIZE); #endif - InitMedianFilterVect3Int(median_mag); + InitMedianFilterVect3Int(median_mag, MEDIAN_DEFAULT_SIZE); RATES_ASSIGN(imu_krooz.rates_sum, 0, 0, 0); VECT3_ASSIGN(imu_krooz.accel_sum, 0, 0, 0); diff --git a/sw/airborne/boards/krooz/imu_krooz_memsic.c b/sw/airborne/boards/krooz/imu_krooz_memsic.c index b6f002a89e..203d471e48 100644 --- a/sw/airborne/boards/krooz/imu_krooz_memsic.c +++ b/sw/airborne/boards/krooz/imu_krooz_memsic.c @@ -79,9 +79,9 @@ void imu_krooz_init(void) // Init median filters #if IMU_KROOZ_USE_ACCEL_MEDIAN_FILTER - InitMedianFilterVect3Int(median_accel); + InitMedianFilterVect3Int(median_accel, MEDIAN_DEFAULT_SIZE); #endif - InitMedianFilterVect3Int(median_mag); + InitMedianFilterVect3Int(median_mag, MEDIAN_DEFAULT_SIZE); RATES_ASSIGN(imu_krooz.rates_sum, 0, 0, 0); VECT3_ASSIGN(imu_krooz.accel_sum, 0, 0, 0); diff --git a/sw/airborne/boards/navgo/imu_navgo.c b/sw/airborne/boards/navgo/imu_navgo.c index 41f23fc395..3a1d8e96a3 100644 --- a/sw/airborne/boards/navgo/imu_navgo.c +++ b/sw/airborne/boards/navgo/imu_navgo.c @@ -84,9 +84,9 @@ void imu_navgo_init(void) #if NAVGO_USE_MEDIAN_FILTER // Init median filters - InitMedianFilterRatesInt(median_gyro); - InitMedianFilterVect3Int(median_accel); - InitMedianFilterVect3Int(median_mag); + InitMedianFilterRatesInt(median_gyro, MEDIAN_DEFAULT_SIZE); + InitMedianFilterVect3Int(median_accel, MEDIAN_DEFAULT_SIZE); + InitMedianFilterVect3Int(median_mag, MEDIAN_DEFAULT_SIZE); #endif } diff --git a/sw/airborne/filters/median_filter.h b/sw/airborne/filters/median_filter.h index 3d3692b32e..354ed84397 100644 --- a/sw/airborne/filters/median_filter.h +++ b/sw/airborne/filters/median_filter.h @@ -24,49 +24,54 @@ #ifndef MEDIAN_H #define MEDIAN_H -#ifndef MEDIAN_DATASIZE -#define MEDIAN_DATASIZE 5 -#endif +#define MAX_MEDIAN_DATASIZE 13 +#define MEDIAN_DEFAULT_SIZE 5 #include "std.h" #include "math/pprz_algebra_int.h" struct MedianFilterInt { - int32_t data[MEDIAN_DATASIZE], sortData[MEDIAN_DATASIZE]; - int8_t dataIndex; + int32_t data[MAX_MEDIAN_DATASIZE], sortData[MAX_MEDIAN_DATASIZE]; + uint8_t dataIndex; + uint8_t size; }; -inline void init_median_filter(struct MedianFilterInt *filter); -inline int32_t update_median_filter(struct MedianFilterInt *filter, int32_t new_data); -inline int32_t get_median_filter(struct MedianFilterInt *filter); +inline void init_median_filter_i(struct MedianFilterInt *filter, uint8_t size); +inline int32_t update_median_filter_i(struct MedianFilterInt *filter, int32_t new_data); +inline int32_t get_median_filter_i(struct MedianFilterInt *filter); -inline void init_median_filter(struct MedianFilterInt *filter) +inline void init_median_filter_i(struct MedianFilterInt *filter, uint8_t size) { - int i; - for (i = 0; i < MEDIAN_DATASIZE; i++) { + uint8_t i; + if (size > MAX_MEDIAN_DATASIZE){ + filter->size = MAX_MEDIAN_DATASIZE; + } else if ((size % 2) == 0) { + // force filter to have odd number of entries so that + // returned median is always an entry and not an average + filter->size = size + 1; + } else { + filter->size = size; + } + for (i = 0; i < filter->size; i++) { filter->data[i] = 0; filter->sortData[i] = 0; } filter->dataIndex = 0; } -inline int32_t update_median_filter(struct MedianFilterInt *filter, int32_t new_data) +inline int32_t update_median_filter_i(struct MedianFilterInt *filter, int32_t new_data) { int temp, i, j; // used to sort array // Insert new data into raw data array round robin style filter->data[filter->dataIndex] = new_data; - if (filter->dataIndex < (MEDIAN_DATASIZE - 1)) { - filter->dataIndex++; - } else { - filter->dataIndex = 0; - } + filter->dataIndex = (filter->dataIndex + 1) % filter->size; // Copy raw data to sort data array - memcpy(filter->sortData, filter->data, sizeof(filter->data)); + memcpy(filter->sortData, filter->data, sizeof(int32_t) * filter->size); // Insertion Sort - for (i = 1; i <= (MEDIAN_DATASIZE - 1); i++) { + for (i = 1; i < filter->size; i++) { temp = filter->sortData[i]; j = i - 1; while (j >= 0 && temp < filter->sortData[j]) { @@ -75,61 +80,179 @@ inline int32_t update_median_filter(struct MedianFilterInt *filter, int32_t new_ } filter->sortData[j + 1] = temp; } - return filter->sortData[(MEDIAN_DATASIZE) >> 1]; // return data value in middle of sorted array + // return data value in middle of sorted array + return get_median_filter_i(filter); } -inline int32_t get_median_filter(struct MedianFilterInt *filter) +inline int32_t get_median_filter_i(struct MedianFilterInt *filter) { - return filter->sortData[(MEDIAN_DATASIZE) >> 1]; + if (filter->size % 2){ + return filter->sortData[filter->size >> 1]; + } else { + // this should not be used if init_median_filter was used + return (filter->sortData[filter->size / 2] + filter->sortData[filter->size / 2 - 1]) / 2; + } } struct MedianFilter3Int { struct MedianFilterInt mf[3]; }; -#define InitMedianFilterVect3Int(_f) { \ - for (int i = 0; i < 3; i++) { \ - init_median_filter(&(_f.mf[i])); \ - } \ +#define InitMedianFilterVect3Int(_f, _n) { \ + for (int i = 0; i < 3; i++) { \ + init_median_filter_i(&(_f.mf[i]), _n); \ + } \ } -#define InitMedianFilterEulerInt(_f) InitMedianFilterVect3Int(_f) -#define InitMedianFilterRatesInt(_f) InitMedianFilterVect3Int(_f) +#define InitMedianFilterEulerInt(_f, _n) InitMedianFilterVect3Int(_f, _n) +#define InitMedianFilterRatesInt(_f, _n) InitMedianFilterVect3Int(_f, _n) #define UpdateMedianFilterVect3Int(_f, _v) { \ - (_v).x = update_median_filter(&(_f.mf[0]), (_v).x); \ - (_v).y = update_median_filter(&(_f.mf[1]), (_v).y); \ - (_v).z = update_median_filter(&(_f.mf[2]), (_v).z); \ + (_v).x = update_median_filter_i(&(_f.mf[0]), (_v).x); \ + (_v).y = update_median_filter_i(&(_f.mf[1]), (_v).y); \ + (_v).z = update_median_filter_i(&(_f.mf[2]), (_v).z); \ } #define UpdateMedianFilterEulerInt(_f, _v) { \ - (_v).phi = update_median_filter(&(_f.mf[0]), (_v).phi); \ - (_v).theta = update_median_filter(&(_f.mf[1]), (_v).theta); \ - (_v).psi = update_median_filter(&(_f.mf[2]), (_v).psi); \ + (_v).phi = update_median_filter_i(&(_f.mf[0]), (_v).phi); \ + (_v).theta = update_median_filter_i(&(_f.mf[1]), (_v).theta); \ + (_v).psi = update_median_filter_i(&(_f.mf[2]), (_v).psi); \ } #define UpdateMedianFilterRatesInt(_f, _v) { \ - (_v).p = update_median_filter(&(_f.mf[0]), (_v).p); \ - (_v).q = update_median_filter(&(_f.mf[1]), (_v).q); \ - (_v).r = update_median_filter(&(_f.mf[2]), (_v).r); \ + (_v).p = update_median_filter_i(&(_f.mf[0]), (_v).p); \ + (_v).q = update_median_filter_i(&(_f.mf[1]), (_v).q); \ + (_v).r = update_median_filter_i(&(_f.mf[2]), (_v).r); \ } #define GetMedianFilterVect3Int(_f, _v) { \ - (_v).x = get_median_filter(&(_f.mf[0])); \ - (_v).y = get_median_filter(&(_f.mf[1])); \ - (_v).z = get_median_filter(&(_f.mf[2])); \ + (_v).x = get_median_filter_i(&(_f.mf[0])); \ + (_v).y = get_median_filter_i(&(_f.mf[1])); \ + (_v).z = get_median_filter_i(&(_f.mf[2])); \ } #define GetMedianFilterEulerInt(_f, _v) { \ - (_v).phi = get_median_filter(&(_f.mf[0])); \ - (_v).theta = get_median_filter(&(_f.mf[1])); \ - (_v).psi = get_median_filter(&(_f.mf[2])); \ + (_v).phi = get_median_filter_i(&(_f.mf[0])); \ + (_v).theta = get_median_filter_i(&(_f.mf[1])); \ + (_v).psi = get_median_filter_i(&(_f.mf[2])); \ } #define GetMedianFilterRatesInt(_f, _v) { \ - (_v).p = get_median_filter(&(_f.mf[0])); \ - (_v).q = get_median_filter(&(_f.mf[1])); \ - (_v).r = get_median_filter(&(_f.mf[2])); \ + (_v).p = get_median_filter_i(&(_f.mf[0])); \ + (_v).q = get_median_filter_i(&(_f.mf[1])); \ + (_v).r = get_median_filter_i(&(_f.mf[2])); \ + } + +struct MedianFilterFloat { + float data[MAX_MEDIAN_DATASIZE], sortData[MAX_MEDIAN_DATASIZE]; + uint8_t dataIndex; + uint8_t size; +}; + +inline void init_median_filter_f(struct MedianFilterFloat *filter, uint8_t size); +inline float update_median_filter_f(struct MedianFilterFloat *filter, float new_data); +inline float get_median_filter_f(struct MedianFilterFloat *filter); + +inline void init_median_filter_f(struct MedianFilterFloat *filter, uint8_t size) +{ + uint8_t i; + if (size > MAX_MEDIAN_DATASIZE){ + filter->size = MAX_MEDIAN_DATASIZE; + } else if ((size % 2) == 0){ + filter->size = size + 1; + } else { + filter->size = size; + } + for (i = 0; i < filter->size; i++) { + filter->data[i] = 0.f; + filter->sortData[i] = 0.f; + } + filter->dataIndex = 0; +} + +inline float update_median_filter_f(struct MedianFilterFloat *filter, float new_data) +{ + float temp; + int i, j; // used to sort array + + // Insert new data into raw data array round robin style + filter->data[filter->dataIndex] = new_data; + filter->dataIndex = (filter->dataIndex + 1) % filter->size; + + // Copy raw data to sort data array + memcpy(filter->sortData, filter->data, sizeof(float) * filter->size); + + // Insertion Sort + for (i = 1; i < filter->size; i++) { + temp = filter->sortData[i]; + j = i - 1; + while (j >= 0 && temp < filter->sortData[j]) { + filter->sortData[j + 1] = filter->sortData[j]; + j = j - 1; + } + filter->sortData[j + 1] = temp; + } + // return data value in middle of sorted array + return get_median_filter_f(filter); +} + +inline float get_median_filter_f(struct MedianFilterFloat *filter) +{ + if (filter->size % 2){ + return filter->sortData[filter->size >> 1]; + } else { + // this should not be used if init_median_filter was used + return (filter->sortData[filter->size / 2] + filter->sortData[filter->size / 2 - 1]) / 2; + } +} + +struct MedianFilter3Float { + struct MedianFilterFloat mf[3]; +}; + +#define InitMedianFilterVect3Float(_f, _n) { \ + for (int i = 0; i < 3; i++) { \ + init_median_filter_f(&(_f.mf[i]), _n); \ + } \ + } + +#define InitMedianFilterEulerFloat(_f, _n) InitMedianFilterVect3Float(_f, _n) +#define InitMedianFilterRatesFloat(_f, _n) InitMedianFilterVect3Float(_f, _n) + +#define UpdateMedianFilterVect3Float(_f, _v) { \ + (_v).x = update_median_filter_f(&(_f.mf[0]), (_v).x); \ + (_v).y = update_median_filter_f(&(_f.mf[1]), (_v).y); \ + (_v).z = update_median_filter_f(&(_f.mf[2]), (_v).z); \ + } + +#define UpdateMedianFilterEulerFloat(_f, _v) { \ + (_v).phi = update_median_filter_f(&(_f.mf[0]), (_v).phi); \ + (_v).theta = update_median_filter_f(&(_f.mf[1]), (_v).theta); \ + (_v).psi = update_median_filter_f(&(_f.mf[2]), (_v).psi); \ + } + +#define UpdateMedianFilterRatesFloat(_f, _v) { \ + (_v).p = update_median_filter_f(&(_f.mf[0]), (_v).p); \ + (_v).q = update_median_filter_f(&(_f.mf[1]), (_v).q); \ + (_v).r = update_median_filter_f(&(_f.mf[2]), (_v).r); \ + } + +#define GetMedianFilterVect3Float(_f, _v) { \ + (_v).x = get_median_filter_f(&(_f.mf[0])); \ + (_v).y = get_median_filter_f(&(_f.mf[1])); \ + (_v).z = get_median_filter_f(&(_f.mf[2])); \ + } + +#define GetMedianFilterEulerFloat(_f, _v) { \ + (_v).phi = get_median_filter_f(&(_f.mf[0])); \ + (_v).theta = get_median_filter_f(&(_f.mf[1])); \ + (_v).psi = get_median_filter_f(&(_f.mf[2])); \ + } + +#define GetMedianFilterRatesFloat(_f, _v) { \ + (_v).p = get_median_filter_f(&(_f.mf[0])); \ + (_v).q = get_median_filter_f(&(_f.mf[1])); \ + (_v).r = get_median_filter_f(&(_f.mf[2])); \ } #endif diff --git a/sw/airborne/modules/computer_vision/opticflow/opticflow_calculator.c b/sw/airborne/modules/computer_vision/opticflow/opticflow_calculator.c index 3c0063e0cf..f6d30c243a 100644 --- a/sw/airborne/modules/computer_vision/opticflow/opticflow_calculator.c +++ b/sw/airborne/modules/computer_vision/opticflow/opticflow_calculator.c @@ -266,8 +266,8 @@ void calc_fast9_lukas_kanade(struct opticflow_t *opticflow, struct opticflow_sta FLOAT_RATES_ZERO(opticflow->prev_rates); // Init median filters with zeros - init_median_filter(&vel_x_filt); - init_median_filter(&vel_y_filt); + init_median_filter_i(&vel_x_filt, MEDIAN_DEFAULT_SIZE); + init_median_filter_i(&vel_y_filt, MEDIAN_DEFAULT_SIZE); } // variables for size_divergence: @@ -473,8 +473,8 @@ void calc_fast9_lukas_kanade(struct opticflow_t *opticflow, struct opticflow_sta //Apply a median filter to the velocity if wanted if (opticflow->median_filter == true) { - result->vel_x = (float)update_median_filter(&vel_x_filt, (int32_t)(vel_x * 1000)) / 1000; - result->vel_y = (float)update_median_filter(&vel_y_filt, (int32_t)(vel_y * 1000)) / 1000; + result->vel_x = (float)update_median_filter_i(&vel_x_filt, (int32_t)(vel_x * 1000)) / 1000; + result->vel_y = (float)update_median_filter_i(&vel_y_filt, (int32_t)(vel_y * 1000)) / 1000; } else { result->vel_x = vel_x; result->vel_y = vel_y; @@ -656,8 +656,8 @@ void calc_edgeflow_tot(struct opticflow_t *opticflow, struct opticflow_state_t * //Apply a median filter to the velocity if wanted if (opticflow->median_filter == true) { - result->vel_x = (float)update_median_filter(&vel_x_filt, (int32_t)(vel_x * 1000)) / 1000; - result->vel_y = (float)update_median_filter(&vel_y_filt, (int32_t)(vel_y * 1000)) / 1000; + result->vel_x = (float)update_median_filter_i(&vel_x_filt, (int32_t)(vel_x * 1000)) / 1000; + result->vel_y = (float)update_median_filter_i(&vel_y_filt, (int32_t)(vel_y * 1000)) / 1000; } else { result->vel_x = vel_x; result->vel_y = vel_y; diff --git a/sw/airborne/modules/lidar/lidar_lite.c b/sw/airborne/modules/lidar/lidar_lite.c index 73ab809759..2c04201f18 100644 --- a/sw/airborne/modules/lidar/lidar_lite.c +++ b/sw/airborne/modules/lidar/lidar_lite.c @@ -64,7 +64,7 @@ void lidar_lite_init(void) lidar_lite.distance = 0; lidar_lite.distance_raw = 0; - init_median_filter(&lidar_lite_filter); + init_median_filter_i(&lidar_lite_filter, LIDAR_LITE_MEDIAN_LENGTH); } /** @@ -139,7 +139,7 @@ void lidar_lite_periodic(void) break; case LIDAR_LITE_PARSE: // filter data - lidar_lite.distance_raw = update_median_filter( + lidar_lite.distance_raw = update_median_filter_i( &lidar_lite_filter, (uint32_t)((lidar_lite.trans.buf[0] << 8) | lidar_lite.trans.buf[1])); lidar_lite.distance = ((float)lidar_lite.distance_raw)/100.0; diff --git a/sw/airborne/modules/lidar/lidar_sf11.c b/sw/airborne/modules/lidar/lidar_sf11.c index 98cdfc7988..b18e46215c 100644 --- a/sw/airborne/modules/lidar/lidar_sf11.c +++ b/sw/airborne/modules/lidar/lidar_sf11.c @@ -52,7 +52,7 @@ void lidar_sf11_init(void) lidar_sf11.distance = 0; lidar_sf11.distance_raw = 0; - init_median_filter(&lidar_sf11_filter); + init_median_filter_i(&lidar_sf11_filter, MEDIAN_DEFAULT_SIZE); } /** @@ -100,7 +100,7 @@ void lidar_sf11_periodic(void) case LIDAR_SF11_READ_OK: // process results // filter data - lidar_sf11.distance_raw = update_median_filter( + lidar_sf11.distance_raw = update_median_filter_i( &lidar_sf11_filter, (uint32_t)((lidar_sf11.trans.buf[0] << 8) | lidar_sf11.trans.buf[1])); lidar_sf11.distance = ((float)lidar_sf11.distance_raw)/100.0; diff --git a/sw/airborne/modules/optical_flow/px4flow_i2c.c b/sw/airborne/modules/optical_flow/px4flow_i2c.c index bee1115bdd..c47dfa4483 100644 --- a/sw/airborne/modules/optical_flow/px4flow_i2c.c +++ b/sw/airborne/modules/optical_flow/px4flow_i2c.c @@ -98,7 +98,7 @@ static inline void px4flow_i2c_frame_cb(void) static float ground_distance_float = 0.0; // update filter - ground_distance = update_median_filter(&sonar_filter, (int32_t)px4flow.i2c_frame.ground_distance); + ground_distance = update_median_filter_i(&sonar_filter, (int32_t)px4flow.i2c_frame.ground_distance); ground_distance_float = ((float)ground_distance) / 1000.0; // compensate AGL measurement for body rotation @@ -141,7 +141,7 @@ void px4flow_i2c_init(void) px4flow.compensate_rotation = PX4FLOW_COMPENSATE_ROTATION; px4flow.stddev = PX4FLOW_NOISE_STDDEV; - init_median_filter(&sonar_filter); + init_median_filter_i(&sonar_filter, PX4FLOW_MEDIAN_LENGTH); } /** diff --git a/sw/airborne/modules/sonar/sonar_bebop.c b/sw/airborne/modules/sonar/sonar_bebop.c index 1fe7dcb0e6..717122e292 100644 --- a/sw/airborne/modules/sonar/sonar_bebop.c +++ b/sw/airborne/modules/sonar/sonar_bebop.c @@ -22,6 +22,14 @@ /** @file modules/sonar/sonar_bebop.c * @brief Parrot Bebop Sonar driver + * + * This file contains the ADC driver for the sonar on the Parrot Bebop + * and Bebop 2 quadrotors. The sonar is both transmitter and receiver. + * An SPI interface is used to communicate with the 12 bit ADC which + * operates at 160kHz. The required waveform of the sonar is sent during + * the SPI transaction, with the returned data the recorded signal from + * the sonar. Two waveforms have been implemented dependent on the + * operating altitude of the Bebop. */ #include "sonar_bebop.h" @@ -32,26 +40,72 @@ #include #include "subsystems/datalink/downlink.h" +#include "filters/median_filter.h" + +struct MedianFilterFloat sonar_filt; + #ifdef SITL #include "state.h" #endif +/** SONAR_BEBOP_INX_DIFF_TO_DIST conversion from index difference to distance based on time of flight + * ADC speed = 160kHz + * speed of sound = 340m/s */ +#define SONAR_BEBOP_INX_DIFF_TO_DIST 340./(2.*160000.) + +/** SONAR_BEBOP_ADC_MAX_VALUE maximum ADC output (12 bit ADC) */ +#define SONAR_BEBOP_ADC_MAX_VALUE 4095 + +/** SONAR_BEBOP_ADC_BUFFER_SIZE ADC buffer size */ +#define SONAR_BEBOP_ADC_BUFFER_SIZE 8192 + +/** mode holds the current sonar mode + * mode = 0 used at high altitude, uses 16 wave patterns + * mode = 1 used at low altitude, uses 4 wave patterns + */ +static uint8_t mode; + +/** SONAR_BEBOP_TRANSITION_HIGH_TO_LOW below this altitude we should use mode 0 */ +#define SONAR_BEBOP_TRANSITION_HIGH_TO_LOW 0.8 + +/** SONAR_BEBOP_TRANSITION_LOW_TO_HIGH above this altitude we should use mode 1 */ +#define SONAR_BEBOP_TRANSITION_LOW_TO_HIGH 1.2 + +/** SONAR_BEBOP_TRANSITION_COUNT number of samples before switching mode */ +#define SONAR_BEBOP_TRANSITION_COUNT 7 + +static uint8_t pulse_transition_counter; + +/** SONAR_BEBOP_PEAK_THRESHOLD minimum samples from broadcast stop */ +#define SONAR_BEBOP_PEAK_THRESHOLD 250 + +/** sonar_bebop_spi_d the waveforms emitted by the sonar + * waveform 0 is long pulse used at high altitude + * waveform 1 is shorter pulse used at low altitude + */ +static uint8_t sonar_bebop_spi_d[2][16] = {{ 0xF0, 0xF0, 0xF0, 0xF0, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00 }, + { 0xF0, 0xF0, 0xF0, 0xF0, 0xF0, 0xF0, 0xF0, 0xF0, 0xF0, 0xF0, 0xF0, 0xF0, 0x00, 0x00, 0x00, 0x00 }}; struct SonarBebop sonar_bebop; -static uint8_t sonar_bebop_spi_d[16] = { 0xF0,0xF0,0xF0,0xF0,0xF0,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00 }; + static struct spi_transaction sonar_bebop_spi_t; -static UNUSED pthread_t sonar_bebop_thread; -static UNUSED void *sonar_bebop_read(void *data); +void *sonar_bebop_read(void *data); +#ifdef USE_SONAR +static pthread_t sonar_bebop_thread; +#endif void sonar_bebop_init(void) { + mode = 0; // default mode is low altitude + pulse_transition_counter = 0; + sonar_bebop.meas = 0; sonar_bebop.offset = 0; sonar_bebop_spi_t.status = SPITransDone; sonar_bebop_spi_t.select = SPISelectUnselect; sonar_bebop_spi_t.dss = SPIDss8bit; - sonar_bebop_spi_t.output_buf = sonar_bebop_spi_d; + sonar_bebop_spi_t.output_buf = sonar_bebop_spi_d[mode]; sonar_bebop_spi_t.output_length = 16; sonar_bebop_spi_t.input_buf = NULL; sonar_bebop_spi_t.input_length = 0; @@ -59,82 +113,95 @@ void sonar_bebop_init(void) #if USE_SONAR pthread_create(&sonar_bebop_thread, NULL, sonar_bebop_read, NULL); #endif + + init_median_filter_f(&sonar_filt, 3); } -/** +uint16_t adc_buffer[SONAR_BEBOP_ADC_BUFFER_SIZE]; +/** sonar_bebop_read * Read ADC value to update sonar measurement */ -static void *sonar_bebop_read(void *data __attribute__((unused))) +void *sonar_bebop_read(void *data __attribute__((unused))) { - while(true) { - + while (true) { #ifndef SITL uint16_t i; - uint16_t adc_buffer[8192]; - /* Start ADC and send sonar output */ adc_enable(&adc0, 1); sonar_bebop_spi_t.status = SPITransDone; + sonar_bebop_spi_t.output_buf = sonar_bebop_spi_d[mode]; spi_submit(&spi0, &sonar_bebop_spi_t); - while(sonar_bebop_spi_t.status != SPITransSuccess); - adc_read(&adc0, adc_buffer, 8192); + while (sonar_bebop_spi_t.status != SPITransSuccess); + adc_read(&adc0, adc_buffer, SONAR_BEBOP_ADC_BUFFER_SIZE); adc_enable(&adc0, 0); - /* Find the peeks */ + /* Find the peaks */ uint16_t start_send = 0; uint16_t stop_send = 0; - uint16_t first_peek = 0; - uint16_t lowest_value = 4095; - for(i = 0; i < 8192; i++){ - uint16_t adc_val = adc_buffer[i]>>4; - if(start_send == 0 && adc_val == 4095) + uint16_t first_peak = 0; + uint16_t lowest_value = SONAR_BEBOP_ADC_MAX_VALUE; + uint16_t peak_value = 0; + + for (i = 0; i < SONAR_BEBOP_ADC_BUFFER_SIZE; i++) { + uint16_t adc_val = adc_buffer[i] >> 4; + if (start_send == 0 && adc_val == SONAR_BEBOP_ADC_MAX_VALUE) { start_send = i; - else if(start_send != 0 && stop_send == 0 && adc_val != 4095) - { - stop_send = i-1; - i += 300; + } else if (start_send != 0 && stop_send == 0 && adc_val != SONAR_BEBOP_ADC_MAX_VALUE) { + stop_send = i - 1; + i += SONAR_BEBOP_PEAK_THRESHOLD; continue; } - if(start_send != 0 && stop_send != 0 && first_peek == 0 && adc_val < lowest_value) - lowest_value = adc_val; - else if (start_send != 0 && stop_send != 0 && adc_val > lowest_value + 100) { - first_peek = i; - lowest_value = adc_val - 100; - } - else if(start_send != 0 && stop_send != 0 && first_peek != 0 && adc_val+100 < (adc_buffer[first_peek]>>4)) { - break; + if (start_send != 0 && stop_send != 0 && first_peak == 0 && adc_val < lowest_value) { + lowest_value = adc_val; // find trough from initial broadcast signal + } else if (start_send != 0 && stop_send != 0 && adc_val > lowest_value + 100 && adc_val > peak_value) { + first_peak = i; + peak_value = adc_val; } } /* Calculate the distance from the peeks */ uint16_t diff = stop_send - start_send; - int16_t peek_distance = first_peek - (stop_send - diff/2); - if(first_peek <= stop_send || diff > 250) - peek_distance = 0; + float peak_distance; + if (diff > 250) { + peak_distance = 0; + } else { + peak_distance = first_peak - (stop_send - diff / 2); + } + + sonar_bebop.distance = update_median_filter_f(&sonar_filt, peak_distance * SONAR_BEBOP_INX_DIFF_TO_DIST); + + // set sonar pulse mode for next pulse based on altitude + if (mode == 0 && sonar_bebop.distance > SONAR_BEBOP_TRANSITION_LOW_TO_HIGH) { + if (++pulse_transition_counter > SONAR_BEBOP_TRANSITION_COUNT) { + mode = 1; + pulse_transition_counter = 0; + } + } else if (mode == 1 && sonar_bebop.distance < SONAR_BEBOP_TRANSITION_HIGH_TO_LOW) { + if (++pulse_transition_counter > SONAR_BEBOP_TRANSITION_COUNT) { + mode = 0; + pulse_transition_counter = 0; + } + } else { + pulse_transition_counter = 0; + } - sonar_bebop.distance = peek_distance / 1000.0; #else // SITL sonar_bebop.distance = stateGetPositionEnu_f()->z; Bound(sonar_bebop.distance, 0.1f, 7.0f); - uint16_t peek_distance = 1; + uint16_t peak_distance = 1; #endif // SITL - usleep(10000); - - if(peek_distance > 0) - { + if (peak_distance > 0) { // Send ABI message AbiSendMsgAGL(AGL_SONAR_ADC_ID, sonar_bebop.distance); - #ifdef SENSOR_SYNC_SEND_SONAR // Send Telemetry report DOWNLINK_SEND_SONAR(DefaultChannel, DefaultDevice, &sonar_bebop.meas, &sonar_bebop.distance); #endif } } - + usleep(10000); //100Hz return NULL; } - diff --git a/sw/airborne/modules/stereocam/stereocam2state/stereocam2state.c b/sw/airborne/modules/stereocam/stereocam2state/stereocam2state.c index 48c9a92e32..5f1d6d848d 100644 --- a/sw/airborne/modules/stereocam/stereocam2state/stereocam2state.c +++ b/sw/airborne/modules/stereocam/stereocam2state/stereocam2state.c @@ -33,7 +33,7 @@ uint8_t stereocam_medianfilter_on = 1; #endif #include "filters/median_filter.h" -struct MedianFilterInt medianfilter_x, medianfilter_y, medianfilter_z; +struct MedianFilter3Float velFilter; #include "subsystems/datalink/telemetry.h" @@ -41,10 +41,7 @@ void stereocam_to_state(void); void stereo_to_state_init(void) { - - init_median_filter(&medianfilter_x); - init_median_filter(&medianfilter_y); - init_median_filter(&medianfilter_z); + InitMedianFilterVect3Float(velFilter, MEDIAN_DEFAULT_SIZE); } void stereo_to_state_periodic(void) @@ -120,9 +117,7 @@ void stereocam_to_state(void) if (stereocam_medianfilter_on == 1) { // Use a slight median filter to filter out the large outliers before sending it to state // TODO: if a float median filter exist, replace this version - vel_body_x_processed = (float)update_median_filter(&medianfilter_x, (int32_t)(quad_body_vel.x * 100)) / 100; - vel_body_y_processed = (float)update_median_filter(&medianfilter_y, (int32_t)(quad_body_vel.y * 100)) / 100; - vel_body_z_processed = (float)update_median_filter(&medianfilter_z, (int32_t)(quad_body_vel.z * 100)) / 100; + UpdateMedianFilterVect3Float(velFilter, quad_body_vel); } //Send velocities to state