Mirrors_Framework/paparazzi
1
0
Fork 0
mirror of https://github.com/paparazzi/paparazzi.git synced 2026-05-23 04:45:37 +08:00
Code Issues Actions 24 Packages Projects Releases Wiki Activity

remove airspeed ets event makro and move transaction status check into event function

Browse Source
This commit is contained in:
Felix Ruess
2010-10-26 11:21:13 +00:00
parent 2d2e083b79
commit f317d3ccdd
3 changed files with 57 additions and 56 deletions
Download Patch File Download Diff File Expand all files Collapse all files
conf/modules/airspeed_ets.xml
+1 -1
View File
@@ -13,7 +13,7 @@
</header>
<init fun="airspeed_ets_init()"/>
<periodic fun="airspeed_ets_read_periodic()" freq="10."/>
<event fun="AirspeedEtsEvent()"/>
<event fun="airspeed_ets_event()"/>
<makefile>
<file name="airspeed_ets.c"/>
sw/airborne/modules/sensors/airspeed_ets.c
+55 -52
View File
@@ -100,60 +100,63 @@ void airspeed_ets_read_periodic( void ) {
#endif //SITL
}
void airspeed_ets_read_event( void ) {
int n;
float airspeed_tmp = 0.0;
void airspeed_ets_event( void ) {
// 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;
if (airspeed_ets_i2c_trans.status == I2CTransSuccess) {
int n;
float airspeed_tmp = 0.0;
// 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;
// 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_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;
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;
}
// Transaction has been read
airspeed_ets_i2c_trans.status = I2CTransDone;
}
sw/airborne/modules/sensors/airspeed_ets.h
+1 -3
View File
@@ -50,8 +50,6 @@ 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(); }
extern void airspeed_ets_event( void );
#endif // AIRSPEED_ETS_H