Mirrors_Framework/paparazzi
1
0
Fork 0
mirror of https://github.com/paparazzi/paparazzi.git synced 2026-05-10 23:35:45 +08:00
Code Issues Actions 25 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>
<dl_settings>
<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>
</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
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);
}
}
sw/airborne/modules/sensors/pressure_board_navarro.h
+15 -13
View File
@@ -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