diff --git a/conf/modules/pbn.xml b/conf/modules/pbn.xml index 5459ff9063..fe3d070fb9 100644 --- a/conf/modules/pbn.xml +++ b/conf/modules/pbn.xml @@ -19,7 +19,7 @@ - + diff --git a/sw/airborne/modules/sensors/pressure_board_navarro.c b/sw/airborne/modules/sensors/pressure_board_navarro.c index 6a1396d6eb..7a98370cf3 100644 --- a/sw/airborne/modules/sensors/pressure_board_navarro.c +++ b/sw/airborne/modules/sensors/pressure_board_navarro.c @@ -73,39 +73,31 @@ PRINT_CONFIG_MSG("USE_AIRSPEED_PBN automatically set to TRUE") // Global variables -uint16_t altitude_adc; -uint16_t airspeed_adc; -bool_t data_valid; +struct PBNState pbn; struct i2c_transaction pbn_trans; +static uint16_t offset_cnt; +static uint16_t startup_delay; -uint32_t airspeed_offset_tmp; -uint32_t altitude_offset_tmp; -uint16_t offset_cnt; -uint16_t altitude_offset; -uint16_t airspeed_offset; -float pbn_altitude; -float pbn_airspeed; -float airspeed_filter; -uint16_t startup_delay; - -void pbn_init( void ) { +void pbn_init(void) +{ startup_delay = PBN_START_DELAY; - altitude_offset = 0; - airspeed_offset = 0; - airspeed_adc = 0; - altitude_adc = 0; - data_valid = TRUE; + pbn.altitude_offset = 0; + pbn.airspeed_offset = 0; + pbn.airspeed_adc = 0; + pbn.altitude_adc = 0; + pbn.data_valid = TRUE; offset_cnt = OFFSET_NBSAMPLES_AVRG; - pbn_airspeed = 0.; - pbn_altitude = 0.; - airspeed_filter = PBN_OFFSET_FILTER; + pbn.airspeed = 0.; + pbn.altitude = 0.; + pbn.airspeed_filter = PBN_OFFSET_FILTER; } -void pbn_periodic( void ) { +void pbn_periodic(void) +{ - if ( startup_delay > 0 ) { + if (startup_delay > 0) { --startup_delay; return; } @@ -116,51 +108,53 @@ void pbn_periodic( void ) { } -void pbn_read_event( void ) { +void pbn_read_event(void) +{ pbn_trans.status = I2CTransDone; // Get raw values from buffer - airspeed_adc = ((uint16_t)(pbn_trans.buf[0]) << 8) | (uint16_t)(pbn_trans.buf[1]); - altitude_adc = ((uint16_t)(pbn_trans.buf[2]) << 8) | (uint16_t)(pbn_trans.buf[3]); + pbn.airspeed_adc = ((uint16_t)(pbn_trans.buf[0]) << 8) | (uint16_t)(pbn_trans.buf[1]); + pbn.altitude_adc = ((uint16_t)(pbn_trans.buf[2]) << 8) | (uint16_t)(pbn_trans.buf[3]); // Consider 0 as a wrong value - if (airspeed_adc == 0 || altitude_adc == 0) { - data_valid = FALSE; - } - else { - data_valid = TRUE; + if (pbn.airspeed_adc == 0 || pbn.altitude_adc == 0) { + pbn.data_valid = FALSE; + } else { + pbn.data_valid = TRUE; if (offset_cnt > 0) { // IIR filter to compute an initial offset #ifndef PBN_AIRSPEED_OFFSET - airspeed_offset = (PBN_OFFSET_FILTER * airspeed_offset + airspeed_adc) / (PBN_OFFSET_FILTER + 1); + pbn.airspeed_offset = (PBN_OFFSET_FILTER * pbn.airspeed_offset + pbn.airspeed_adc) / + (PBN_OFFSET_FILTER + 1); #else - airspeed_offset = PBN_AIRSPEED_OFFSET; + pbn.airspeed_offset = PBN_AIRSPEED_OFFSET; #endif #ifndef PBN_ALTITUDE_OFFSET - altitude_offset = (PBN_OFFSET_FILTER * altitude_offset + altitude_adc) / (PBN_OFFSET_FILTER + 1); + pbn.altitude_offset = (PBN_OFFSET_FILTER * pbn.altitude_offset + pbn.altitude_adc) / + (PBN_OFFSET_FILTER + 1); #else - altitude_offset = PBN_ALTITUDE_OFFSET; + pbn.altitude_offset = PBN_ALTITUDE_OFFSET; #endif // decrease init counter --offset_cnt; - } - else { + } else { // Compute pressure - float pressure = PBN_ALTITUDE_SCALE * (float) altitude_adc + PBN_PRESSURE_OFFSET; + float pressure = PBN_ALTITUDE_SCALE * (float) pbn.altitude_adc + PBN_PRESSURE_OFFSET; AbiSendMsgBARO_ABS(BARO_PBN_SENDER_ID, &pressure); // Compute airspeed and altitude //pbn_airspeed = (-4.45 + sqrtf(19.84-0.57*(float)(airspeed_offset-airspeed_adc)))/0.28; - uint16_t diff = Max(airspeed_adc-airspeed_offset, 0); + uint16_t diff = Max(pbn.airspeed_adc - pbn.airspeed_offset, 0); float tmp_airspeed = sqrtf((float)diff * PBN_AIRSPEED_SCALE); - pbn_altitude = PBN_ALTITUDE_SCALE*(float)(altitude_adc-altitude_offset); - - pbn_airspeed = (airspeed_filter*pbn_airspeed + tmp_airspeed) / (airspeed_filter + 1.); + pbn.airspeed = (pbn.airspeed_filter * pbn.airspeed + tmp_airspeed) / + (pbn.airspeed_filter + 1.); #if USE_AIRSPEED_PBN - stateSetAirspeed_f(&pbn_airspeed); + stateSetAirspeed_f(&pbn.airspeed); #endif + + pbn.altitude = PBN_ALTITUDE_SCALE * (float)(pbn.altitude_adc - pbn.altitude_offset); } } diff --git a/sw/airborne/modules/sensors/pressure_board_navarro.h b/sw/airborne/modules/sensors/pressure_board_navarro.h index ff16f5da43..f8f1eb6593 100644 --- a/sw/airborne/modules/sensors/pressure_board_navarro.h +++ b/sw/airborne/modules/sensors/pressure_board_navarro.h @@ -32,29 +32,31 @@ * */ - #ifndef PRESSURE_BOARD_NAVARRO_H #define PRESSURE_BOARD_NAVARRO_H #include "std.h" #include "mcu_periph/i2c.h" -extern uint16_t altitude_adc; -extern uint16_t airspeed_adc; -extern uint16_t altitude_offset; -extern uint16_t airspeed_offset; -extern float pbn_altitude, pbn_airspeed; -extern float airspeed_filter; +struct PBNState { + uint16_t altitude_adc; + uint16_t airspeed_adc; + uint16_t altitude_offset; + uint16_t airspeed_offset; + float altitude; + float airspeed; + float airspeed_filter; + bool_t data_valid; +}; + +extern struct PBNState pbn; -extern bool_t data_valid; extern struct i2c_transaction pbn_trans; -extern void pbn_init( void ); -extern void pbn_periodic( void ); -extern void pbn_read_event( void ); +extern void pbn_init(void); +extern void pbn_periodic(void); +extern void pbn_read_event(void); #define PbnEvent() { if (pbn_trans.status == I2CTransSuccess) pbn_read_event(); } -#define PERIODIC_SEND_PBN(_chan) DOWNLINK_SEND_PBN(DefaultChannel, DefaultDevice,&airspeed_adc,&altitude_adc,&pbn_airspeed,&pbn_altitude,&airspeed_offset,&altitude_offset); - #endif // PRESSURE_BOARD_NAVARRO_H