mirror of https://github.com/ArduPilot/ardupilot
AP_HAL_SITL: sitl_airspeed correct style
This commit is contained in:
parent
0afc907b0a
commit
775ca9303a
|
@ -34,40 +34,39 @@ using namespace HALSITL;
|
|||
void SITL_State::_update_airspeed(float airspeed)
|
||||
{
|
||||
const float airspeed_ratio = 1.9936f;
|
||||
const float airspeed_offset = 2013;
|
||||
float airspeed_pressure, airspeed_raw;
|
||||
const float airspeed_offset = 2013.0f;
|
||||
|
||||
// Check sensor failure
|
||||
airspeed = is_zero(_sitl->arspd_fail) ? airspeed : _sitl->arspd_fail;
|
||||
// Add noise
|
||||
airspeed = airspeed + (_sitl->arspd_noise * rand_float());
|
||||
|
||||
airspeed_pressure = (airspeed*airspeed) / airspeed_ratio;
|
||||
airspeed_raw = airspeed_pressure + airspeed_offset;
|
||||
if (airspeed_raw/4 > 0xFFFF) {
|
||||
const float airspeed_pressure = (airspeed * airspeed) / airspeed_ratio;
|
||||
float airspeed_raw = airspeed_pressure + airspeed_offset;
|
||||
if (airspeed_raw / 4 > 0xFFFF) {
|
||||
airspeed_pin_value = 0xFFFF;
|
||||
return;
|
||||
}
|
||||
// add delay
|
||||
uint32_t now = AP_HAL::millis();
|
||||
uint32_t best_time_delta_wind = 200; // initialise large time representing buffer entry closest to current time - delay.
|
||||
uint8_t best_index_wind = 0; // initialise number representing the index of the entry in buffer closest to delay.
|
||||
const uint32_t now = AP_HAL::millis();
|
||||
uint32_t best_time_delta_wind = 200; // initialise large time representing buffer entry closest to current time - delay.
|
||||
uint8_t best_index_wind = 0; // initialise number representing the index of the entry in buffer closest to delay.
|
||||
|
||||
// storing data from sensor to buffer
|
||||
if (now - last_store_time_wind >= 10) { // store data every 10 ms.
|
||||
if (now - last_store_time_wind >= 10) { // store data every 10 ms.
|
||||
last_store_time_wind = now;
|
||||
if (store_index_wind > wind_buffer_length-1) { // reset buffer index if index greater than size of buffer
|
||||
if (store_index_wind > wind_buffer_length - 1) { // reset buffer index if index greater than size of buffer
|
||||
store_index_wind = 0;
|
||||
}
|
||||
buffer_wind[store_index_wind].data = airspeed_raw; // add data to current index
|
||||
buffer_wind[store_index_wind].time = last_store_time_wind; // add time to current index
|
||||
store_index_wind = store_index_wind + 1; // increment index
|
||||
buffer_wind[store_index_wind].data = airspeed_raw; // add data to current index
|
||||
buffer_wind[store_index_wind].time = last_store_time_wind; // add time to current index
|
||||
store_index_wind = store_index_wind + 1; // increment index
|
||||
}
|
||||
|
||||
// return delayed measurement
|
||||
delayed_time_wind = now - _sitl->wind_delay; // get time corresponding to delay
|
||||
delayed_time_wind = now - _sitl->wind_delay; // get time corresponding to delay
|
||||
// find data corresponding to delayed time in buffer
|
||||
for (uint8_t i=0; i<=wind_buffer_length-1; i++) {
|
||||
for (uint8_t i = 0; i <= wind_buffer_length - 1; i++) {
|
||||
// find difference between delayed time and time stamp in buffer
|
||||
time_delta_wind = abs(
|
||||
(int32_t)(delayed_time_wind - buffer_wind[i].time));
|
||||
|
@ -77,11 +76,11 @@ void SITL_State::_update_airspeed(float airspeed)
|
|||
best_time_delta_wind = time_delta_wind;
|
||||
}
|
||||
}
|
||||
if (best_time_delta_wind < 200) { // only output stored state if < 200 msec retrieval error
|
||||
if (best_time_delta_wind < 200) { // only output stored state if < 200 msec retrieval error
|
||||
airspeed_raw = buffer_wind[best_index_wind].data;
|
||||
}
|
||||
|
||||
airspeed_pin_value = airspeed_raw/4;
|
||||
airspeed_pin_value = airspeed_raw / 4;
|
||||
}
|
||||
|
||||
#endif
|
||||
|
|
Loading…
Reference in New Issue