Mirrors_Framework/paparazzi
1
0
Fork 0
mirror of https://github.com/paparazzi/paparazzi.git synced 2026-02-06 02:52:42 +08:00
Code Issues Actions 2 Packages Projects Releases Wiki Activity

add event makros for ets sensors again to prevent unnecessary function calls

Browse Source
This commit is contained in:
Felix Ruess
2010-11-01 20:53:42 +00:00
parent fe980a0943
commit e67bfff08d
7 changed files with 99 additions and 101 deletions
Download Patch File Download Diff File Expand all files Collapse all files

2
conf/modules/airspeed_adc.xml
View File

@@ -16,7 +16,7 @@
<periodic fun="airspeed_adc_update()" freq="10."/>
<makefile>
<file name="airspeed_adc.c"/>
<file name="airspeed_adc.c"/>
</makefile>
<makefile target="ap">
<flag name="ADC_CHANNEL_AIRSPEED" value="$(ADC_AIRSPEED)"/>

2
conf/modules/airspeed_ets.xml
View File

@@ -16,7 +16,7 @@
</header>
<init fun="airspeed_ets_init()"/>
<periodic fun="airspeed_ets_read_periodic()" freq="10."/>
<event fun="airspeed_ets_event()"/>
<event fun="AirspeedEtsEvent()"/>
<makefile>
<file name="airspeed_ets.c"/>

2
conf/modules/baro_ets.xml
View File

@@ -12,7 +12,7 @@
</header>
<init fun="baro_ets_init()"/>
<periodic fun="baro_ets_read_periodic()" freq="10."/>
<event fun="baro_ets_event()"/>
<event fun="BaroEtsEvent()"/>
<makefile>
<file name="baro_ets.c"/>

108
sw/airborne/modules/sensors/airspeed_ets.c
View File

@@ -109,68 +109,64 @@ void airspeed_ets_read_periodic( void ) {
#endif //SITL
}
void airspeed_ets_event( void ) {
void airspeed_ets_read_event( void ) {
int n;
float airspeed_tmp = 0.0;
if (airspeed_ets_i2c_trans.status == I2CTransSuccess) {
int n;
float airspeed_tmp = 0.0;
// 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;
// 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;
// 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;
// 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;
}
// 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;
// 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;
#ifdef USE_AIRSPEED
EstimatorSetAirspeed(airspeed_ets);
EstimatorSetAirspeed(airspeed_ets);
#endif
#ifdef SENSOR_SYNC_SEND
DOWNLINK_SEND_AIRSPEED_ETS(DefaultChannel, &airspeed_ets_raw, &airspeed_ets_offset, &airspeed_ets);
DOWNLINK_SEND_AIRSPEED_ETS(DefaultChannel, &airspeed_ets_raw, &airspeed_ets_offset, &airspeed_ets);
#endif
} else {
airspeed_ets = 0.0;
}
// Transaction has been read
airspeed_ets_i2c_trans.status = I2CTransDone;
} else {
airspeed_ets = 0.0;
}
// Transaction has been read
airspeed_ets_i2c_trans.status = I2CTransDone;
}

4
sw/airborne/modules/sensors/airspeed_ets.h
View File

@@ -50,6 +50,8 @@ 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_event( void );
extern void airspeed_ets_read_event( void );
#define AirspeedEtsEvent() { if (airspeed_ets_i2c_trans.status == I2CTransSuccess) airspeed_ets_read_event(); }
#endif // AIRSPEED_ETS_H

78
sw/airborne/modules/sensors/baro_ets.c
View File

@@ -102,48 +102,46 @@ void baro_ets_read_periodic( void ) {
#endif
}
void baro_ets_event( void ) {
if (baro_ets_i2c_trans.status == I2CTransSuccess) {
// Get raw altimeter from buffer
baro_ets_adc = ((uint16_t)(baro_ets_i2c_trans.buf[1]) << 8) | (uint16_t)(baro_ets_i2c_trans.buf[0]);
// Check if this is valid altimeter
if (baro_ets_adc == 0)
baro_ets_valid = FALSE;
else
baro_ets_valid = TRUE;
void baro_ets_read_event( void ) {
// Get raw altimeter from buffer
baro_ets_adc = ((uint16_t)(baro_ets_i2c_trans.buf[1]) << 8) | (uint16_t)(baro_ets_i2c_trans.buf[0]);
// Check if this is valid altimeter
if (baro_ets_adc == 0)
baro_ets_valid = FALSE;
else
baro_ets_valid = TRUE;
// Continue only if a new altimeter value was received
if (baro_ets_valid) {
// Calculate offset average if not done already
if (!baro_ets_offset_init) {
--baro_ets_cnt;
// Check if averaging completed
if (baro_ets_cnt == 0) {
// Calculate average
baro_ets_offset = (uint16_t)(baro_ets_offset_tmp / BARO_ETS_OFFSET_NBSAMPLES_AVRG);
// Limit offset
if (baro_ets_offset < BARO_ETS_OFFSET_MIN)
baro_ets_offset = BARO_ETS_OFFSET_MIN;
if (baro_ets_offset > BARO_ETS_OFFSET_MAX)
baro_ets_offset = BARO_ETS_OFFSET_MAX;
baro_ets_offset_init = TRUE;
}
// Check if averaging needs to continue
else if (baro_ets_cnt <= BARO_ETS_OFFSET_NBSAMPLES_AVRG)
baro_ets_offset_tmp += baro_ets_adc;
// Continue only if a new altimeter value was received
if (baro_ets_valid) {
// Calculate offset average if not done already
if (!baro_ets_offset_init) {
--baro_ets_cnt;
// Check if averaging completed
if (baro_ets_cnt == 0) {
// Calculate average
baro_ets_offset = (uint16_t)(baro_ets_offset_tmp / BARO_ETS_OFFSET_NBSAMPLES_AVRG);
// Limit offset
if (baro_ets_offset < BARO_ETS_OFFSET_MIN)
baro_ets_offset = BARO_ETS_OFFSET_MIN;
if (baro_ets_offset > BARO_ETS_OFFSET_MAX)
baro_ets_offset = BARO_ETS_OFFSET_MAX;
baro_ets_offset_init = TRUE;
}
// Convert raw to m/s
if (baro_ets_offset_init)
baro_ets_altitude = BARO_ETS_SCALE * (float)(baro_ets_offset-baro_ets_adc);
else
baro_ets_altitude = 0.0;
// New value available
EstimatorSetAlt(baro_ets_altitude);
} else {
baro_ets_altitude = 0.0;
// Check if averaging needs to continue
else if (baro_ets_cnt <= BARO_ETS_OFFSET_NBSAMPLES_AVRG)
baro_ets_offset_tmp += baro_ets_adc;
}
// Transaction has been read
baro_ets_i2c_trans.status = I2CTransDone;
// Convert raw to m/s
if (baro_ets_offset_init)
baro_ets_altitude = BARO_ETS_SCALE * (float)(baro_ets_offset-baro_ets_adc);
else
baro_ets_altitude = 0.0;
// New value available
EstimatorSetAlt(baro_ets_altitude);
} else {
baro_ets_altitude = 0.0;
}
// Transaction has been read
baro_ets_i2c_trans.status = I2CTransDone;
}

4
sw/airborne/modules/sensors/baro_ets.h
View File

@@ -56,6 +56,8 @@ extern struct i2c_transaction baro_ets_i2c_trans;
extern void baro_ets_init( void );
extern void baro_ets_read_periodic( void );
extern void baro_ets_event( void );
extern void baro_ets_read_event( void );
#define BaroEtsEvent() { if (baro_ets_i2c_trans.status == I2CTransSuccess) baro_ets_read_event(); }
#endif // BARO_ETS_H