Mirrors_Framework/paparazzi
1
0
Fork 0
mirror of https://github.com/paparazzi/paparazzi.git synced 2026-05-30 19:47:50 +08:00
Code Issues Actions 30 Packages Projects Releases Wiki Activity

[modules] pbn cleanup

Browse Source
This commit is contained in:
Felix Ruess
2014-10-23 14:28:39 +02:00
parent 39eae7c899
commit 4df83d5e58
3 changed files with 54 additions and 58 deletions
Download Patch File Download Diff File Expand all files Collapse all files
conf/modules/pbn.xml
+1 -1
View File
@@ -19,7 +19,7 @@
<settings> <settings>
<dl_settings> <dl_settings>
<dl_settings NAME="pbn"> <dl_settings NAME="pbn">
<dl_setting MAX="30" MIN="0" STEP="1" module="sensors/pressure_board_navarro" VAR="airspeed_filter"/> <dl_setting MAX="30" MIN="0" STEP="1" module="sensors/pressure_board_navarro" VAR="pbn.airspeed_filter"/>
</dl_settings> </dl_settings>
</dl_settings> </dl_settings>
</settings> </settings>
sw/airborne/modules/sensors/pressure_board_navarro.c
+38 -44
View File
@@ -73,39 +73,31 @@ PRINT_CONFIG_MSG("USE_AIRSPEED_PBN automatically set to TRUE")
// Global variables // Global variables
uint16_t altitude_adc; struct PBNState pbn;
uint16_t airspeed_adc;
bool_t data_valid;
struct i2c_transaction pbn_trans; struct i2c_transaction pbn_trans;
static uint16_t offset_cnt;
static uint16_t startup_delay;
uint32_t airspeed_offset_tmp; void pbn_init(void)
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 ) {
startup_delay = PBN_START_DELAY; startup_delay = PBN_START_DELAY;
altitude_offset = 0; pbn.altitude_offset = 0;
airspeed_offset = 0; pbn.airspeed_offset = 0;
airspeed_adc = 0; pbn.airspeed_adc = 0;
altitude_adc = 0; pbn.altitude_adc = 0;
data_valid = TRUE; pbn.data_valid = TRUE;
offset_cnt = OFFSET_NBSAMPLES_AVRG; offset_cnt = OFFSET_NBSAMPLES_AVRG;
pbn_airspeed = 0.; pbn.airspeed = 0.;
pbn_altitude = 0.; pbn.altitude = 0.;
airspeed_filter = PBN_OFFSET_FILTER; pbn.airspeed_filter = PBN_OFFSET_FILTER;
} }
void pbn_periodic( void ) { void pbn_periodic(void)
{
if ( startup_delay > 0 ) { if (startup_delay > 0) {
--startup_delay; --startup_delay;
return; return;
} }
@@ -116,51 +108,53 @@ void pbn_periodic( void ) {
} }
void pbn_read_event( void ) { void pbn_read_event(void)
{
pbn_trans.status = I2CTransDone; pbn_trans.status = I2CTransDone;
// Get raw values from buffer // Get raw values from buffer
airspeed_adc = ((uint16_t)(pbn_trans.buf[0]) << 8) | (uint16_t)(pbn_trans.buf[1]); pbn.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.altitude_adc = ((uint16_t)(pbn_trans.buf[2]) << 8) | (uint16_t)(pbn_trans.buf[3]);
// Consider 0 as a wrong value // Consider 0 as a wrong value
if (airspeed_adc == 0 || altitude_adc == 0) { if (pbn.airspeed_adc == 0 || pbn.altitude_adc == 0) {
data_valid = FALSE; pbn.data_valid = FALSE;
} } else {
else { pbn.data_valid = TRUE;
data_valid = TRUE;
if (offset_cnt > 0) { if (offset_cnt > 0) {
// IIR filter to compute an initial offset // IIR filter to compute an initial offset
#ifndef PBN_AIRSPEED_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 #else
airspeed_offset = PBN_AIRSPEED_OFFSET; pbn.airspeed_offset = PBN_AIRSPEED_OFFSET;
#endif #endif
#ifndef PBN_ALTITUDE_OFFSET #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 #else
altitude_offset = PBN_ALTITUDE_OFFSET; pbn.altitude_offset = PBN_ALTITUDE_OFFSET;
#endif #endif
// decrease init counter // decrease init counter
--offset_cnt; --offset_cnt;
} } else {
else {
// Compute pressure // 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); AbiSendMsgBARO_ABS(BARO_PBN_SENDER_ID, &pressure);
// Compute airspeed and altitude // Compute airspeed and altitude
//pbn_airspeed = (-4.45 + sqrtf(19.84-0.57*(float)(airspeed_offset-airspeed_adc)))/0.28; //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); float tmp_airspeed = sqrtf((float)diff * PBN_AIRSPEED_SCALE);
pbn_altitude = PBN_ALTITUDE_SCALE*(float)(altitude_adc-altitude_offset); pbn.airspeed = (pbn.airspeed_filter * pbn.airspeed + tmp_airspeed) /
(pbn.airspeed_filter + 1.);
pbn_airspeed = (airspeed_filter*pbn_airspeed + tmp_airspeed) / (airspeed_filter + 1.);
#if USE_AIRSPEED_PBN #if USE_AIRSPEED_PBN
stateSetAirspeed_f(&pbn_airspeed); stateSetAirspeed_f(&pbn.airspeed);
#endif #endif
pbn.altitude = PBN_ALTITUDE_SCALE * (float)(pbn.altitude_adc - pbn.altitude_offset);
} }
} }
sw/airborne/modules/sensors/pressure_board_navarro.h
+15 -13
View File
@@ -32,29 +32,31 @@
* *
*/ */
#ifndef PRESSURE_BOARD_NAVARRO_H #ifndef PRESSURE_BOARD_NAVARRO_H
#define PRESSURE_BOARD_NAVARRO_H #define PRESSURE_BOARD_NAVARRO_H
#include "std.h" #include "std.h"
#include "mcu_periph/i2c.h" #include "mcu_periph/i2c.h"
extern uint16_t altitude_adc; struct PBNState {
extern uint16_t airspeed_adc; uint16_t altitude_adc;
extern uint16_t altitude_offset; uint16_t airspeed_adc;
extern uint16_t airspeed_offset; uint16_t altitude_offset;
extern float pbn_altitude, pbn_airspeed; uint16_t airspeed_offset;
extern float airspeed_filter; 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 struct i2c_transaction pbn_trans;
extern void pbn_init( void ); extern void pbn_init(void);
extern void pbn_periodic( void ); extern void pbn_periodic(void);
extern void pbn_read_event( void ); extern void pbn_read_event(void);
#define PbnEvent() { if (pbn_trans.status == I2CTransSuccess) pbn_read_event(); } #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 #endif // PRESSURE_BOARD_NAVARRO_H