aepko/Ardupilot2
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

HAL_SITL: cleanup SITL airspeed handling

Browse Source 
fixed handling of EAS2TAS, and fixed ratio per sensor.

Removed the wind delay code (which was never being used). We should
add a generic delay filter if we need this again
This commit is contained in:
Andrew Tridgell 2022-05-16 15:27:01 +10:00
parent 2dea725d80
commit 7dc5da1247
5 changed files with 55 additions and 108 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

16
libraries/AP_HAL/SIMState.h
View File

@ -109,22 +109,6 @@ private:
const char *_fg_address;
// delay buffer variables
static const uint8_t wind_buffer_length = 50;
// airspeed sensor delay buffer variables
struct readings_wind {
uint32_t time;
float data;
};
uint8_t store_index_wind;
uint32_t last_store_time_wind;
VectorN<readings_wind,wind_buffer_length> buffer_wind;
VectorN<readings_wind,wind_buffer_length> buffer_wind_2;
uint32_t time_delta_wind;
uint32_t delayed_time_wind;
uint32_t wind_start_delay_micros;
// internal SITL model
SITL::Aircraft *sitl_model;

4
libraries/AP_HAL_SITL/AnalogIn.cpp
View File

@ -35,10 +35,10 @@ float ADCSource::read_latest() {
return _sitlState->sonar_pin_value;
case 1:
return _sitlState->airspeed_pin_value;
return _sitlState->airspeed_pin_value[0];
case 2:
return _sitlState->airspeed_2_pin_value;
return _sitlState->airspeed_pin_value[1];
case 12:
return _sitlState->current_pin_value;

3
libraries/AP_HAL_SITL/SITL_Periph_State.h
View File

@ -41,8 +41,7 @@ public:
// simulated airspeed, sonar and battery monitor
uint16_t sonar_pin_value; // pin 0
uint16_t airspeed_pin_value; // pin 1
uint16_t airspeed_2_pin_value; // pin 2
uint16_t airspeed_pin_value[2]; // pin 1
uint16_t voltage_pin_value; // pin 13
uint16_t current_pin_value; // pin 12
uint16_t voltage2_pin_value; // pin 15

4
libraries/AP_HAL_SITL/SITL_State.h
View File

@ -96,8 +96,7 @@ public:
// simulated airspeed, sonar and battery monitor
uint16_t sonar_pin_value; // pin 0
uint16_t airspeed_pin_value; // pin 1
uint16_t airspeed_2_pin_value; // pin 2
uint16_t airspeed_pin_value[AIRSPEED_MAX_SENSORS]; // pin 1
uint16_t voltage_pin_value; // pin 13
uint16_t current_pin_value; // pin 12
uint16_t voltage2_pin_value; // pin 15
@ -186,7 +185,6 @@ private:
uint8_t store_index_wind;
uint32_t last_store_time_wind;
VectorN<readings_wind,wind_buffer_length> buffer_wind;
VectorN<readings_wind,wind_buffer_length> buffer_wind_2;
uint32_t time_delta_wind;
uint32_t delayed_time_wind;
uint32_t wind_start_delay_micros;

136
libraries/AP_HAL_SITL/sitl_airspeed.cpp
View File

@ -20,101 +20,67 @@ extern const AP_HAL::HAL& hal;
using namespace HALSITL;
// return current scale factor that converts from equivalent to true airspeed
// valid for altitudes up to 10km AMSL
// assumes standard atmosphere lapse rate
static float get_EAS2TAS(float altitude)
{
float pressure = AP::baro().get_pressure();
if (is_zero(pressure)) {
return 1.0f;
}
float sigma, delta, theta;
AP_Baro::SimpleAtmosphere(altitude * 0.001, sigma, delta, theta);
float tempK = C_TO_KELVIN(25) - ISA_LAPSE_RATE * altitude;
const float eas2tas_squared = SSL_AIR_DENSITY / (pressure / (ISA_GAS_CONSTANT * tempK));
if (!is_positive(eas2tas_squared)) {
return 1.0;
}
return sqrtf(eas2tas_squared);
}
/*
convert airspeed in m/s to an airspeed sensor value
*/
void SITL_State::_update_airspeed(float airspeed)
void SITL_State::_update_airspeed(float true_airspeed)
{
float airspeed2 = airspeed;
const float airspeed_ratio = 1.9936f;
const float diff_pressure = sq(airspeed) * 0.5;
for (uint8_t i=0; i<AIRSPEED_MAX_SENSORS; i++) {
const auto &arspd = _sitl->airspeed[i];
float airspeed = true_airspeed / get_EAS2TAS(_sitl->state.altitude);
const float diff_pressure = sq(airspeed) / arspd.ratio;
float airspeed_raw;
// apply noise to the differential pressure. This emulates the way
// airspeed noise reduces with speed
airspeed = sqrtf(fabsf(2*(diff_pressure + _sitl->arspd_noise[0] * rand_float())));
airspeed2 = sqrtf(fabsf(2*(diff_pressure + _sitl->arspd_noise[1] * rand_float())));
// apply noise to the differential pressure. This emulates the way
// airspeed noise reduces with speed
airspeed = sqrtf(fabsf(arspd.ratio*(diff_pressure + arspd.noise * rand_float())));
// check sensor failure
if (is_positive(_sitl->arspd_fail[0])) {
airspeed = _sitl->arspd_fail[0];
}
if (is_positive(_sitl->arspd_fail[1])) {
airspeed2 = _sitl->arspd_fail[1];
}
if (!is_zero(_sitl->arspd_fail_pressure[0])) {
// compute a realistic pressure report given some level of trapper air pressure in the tube and our current altitude
// algorithm taken from https://en.wikipedia.org/wiki/Calibrated_airspeed#Calculation_from_impact_pressure
float tube_pressure = fabsf(_sitl->arspd_fail_pressure[0] - AP::baro().get_pressure() + _sitl->arspd_fail_pitot_pressure[0]);
airspeed = 340.29409348 * sqrt(5 * (pow((tube_pressure / SSL_AIR_PRESSURE + 1), 2.0/7.0) - 1.0));
}
if (!is_zero(_sitl->arspd_fail_pressure[1])) {
// compute a realistic pressure report given some level of trapper air pressure in the tube and our current altitude
// algorithm taken from https://en.wikipedia.org/wiki/Calibrated_airspeed#Calculation_from_impact_pressure
float tube_pressure = fabsf(_sitl->arspd_fail_pressure[1] - AP::baro().get_pressure() + _sitl->arspd_fail_pitot_pressure[1]);
airspeed2 = 340.29409348 * sqrt(5 * (pow((tube_pressure / SSL_AIR_PRESSURE + 1), 2.0/7.0) - 1.0));
}
float airspeed_pressure = (airspeed * airspeed) / airspeed_ratio;
float airspeed2_pressure = (airspeed2 * airspeed2) / airspeed_ratio;
// flip sign here for simulating reversed pitot/static connections
if (_sitl->arspd_signflip) airspeed_pressure *= -1;
if (_sitl->arspd_signflip) airspeed2_pressure *= -1;
// apply airspeed sensor offset in m/s
float airspeed_raw = airspeed_pressure + _sitl->arspd_offset[0];
float airspeed2_raw = airspeed2_pressure + _sitl->arspd_offset[1];
_sitl->state.airspeed_raw_pressure[0] = airspeed_pressure;
_sitl->state.airspeed_raw_pressure[1] = airspeed2_pressure;
if (airspeed_raw / 4 > 0xFFFF) {
airspeed_pin_value = 0xFFFF;
return;
}
if (airspeed2_raw / 4 > 0xFFFF) {
airspeed_2_pin_value = 0xFFFF;
return;
}
// add 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.
last_store_time_wind = now;
if (store_index_wind > wind_buffer_length - 1) { // reset buffer index if index greater than size of buffer
store_index_wind = 0;
// check sensor failure
if (is_positive(arspd.fail)) {
airspeed = arspd.fail;
}
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
buffer_wind_2[store_index_wind].data = airspeed2_raw; // add data to current index
buffer_wind_2[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
// find data corresponding to delayed time in buffer
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));
// if this difference is smaller than last delta, store this time
if (time_delta_wind < best_time_delta_wind) {
best_index_wind = i;
best_time_delta_wind = time_delta_wind;
if (!is_zero(arspd.fail_pressure)) {
// compute a realistic pressure report given some level of trapper air pressure in the tube and our current altitude
// algorithm taken from https://en.wikipedia.org/wiki/Calibrated_airspeed#Calculation_from_impact_pressure
float tube_pressure = fabsf(arspd.fail_pressure - AP::baro().get_pressure() + arspd.fail_pitot_pressure);
airspeed = 340.29409348 * sqrt(5 * (pow((tube_pressure / SSL_AIR_PRESSURE + 1), 2.0/7.0) - 1.0));
}
}
if (best_time_delta_wind < 200) { // only output stored state if < 200 msec retrieval error
airspeed_raw = buffer_wind[best_index_wind].data;
airspeed2_raw = buffer_wind_2[best_index_wind].data;
}
float airspeed_pressure = (airspeed * airspeed) / arspd.ratio;
airspeed_pin_value = airspeed_raw / 4;
airspeed_2_pin_value = airspeed2_raw / 4;
// flip sign here for simulating reversed pitot/static connections
if (arspd.signflip) {
airspeed_pressure *= -1;
}
// apply airspeed sensor offset in m/s
airspeed_raw = airspeed_pressure + arspd.offset;
_sitl->state.airspeed_raw_pressure[i] = airspeed_pressure;
airspeed_pin_value[i] = MIN(0xFFFF, airspeed_raw / 4);
}
}
#endif