zrzk
/
ardupilot
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity
Browse Source

HAL_SITL: cleanup SITL airspeed handling 

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
apm_2208
Andrew Tridgell 3 years ago
parent
2dea725d80
commit
7dc5da1247
5 changed files with 53 additions and 106 deletions
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Split View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 16
      libraries/AP_HAL/SIMState.h
  2. 4
      libraries/AP_HAL_SITL/AnalogIn.cpp
  3. 3
      libraries/AP_HAL_SITL/SITL_Periph_State.h
  4. 4
      libraries/AP_HAL_SITL/SITL_State.h
  5. 132
      libraries/AP_HAL_SITL/sitl_airspeed.cpp

16
libraries/AP_HAL/SIMState.h
Unescape View File

@ -109,22 +109,6 @@ private: @@ -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
Unescape View File

@ -35,10 +35,10 @@ float ADCSource::read_latest() { @@ -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
Unescape View File

@ -41,8 +41,7 @@ public: @@ -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
Unescape View File

@ -96,8 +96,7 @@ public: @@ -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: @@ -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;

132
libraries/AP_HAL_SITL/sitl_airspeed.cpp
Unescape View File

@ -20,101 +20,67 @@ extern const AP_HAL::HAL& hal; @@ -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())));
// 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));
}
// 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())));
float airspeed_pressure = (airspeed * airspeed) / airspeed_ratio;
float airspeed2_pressure = (airspeed2 * airspeed2) / airspeed_ratio;
// check sensor failure
if (is_positive(arspd.fail)) {
airspeed = arspd.fail;
}
// flip sign here for simulating reversed pitot/static connections
if (_sitl->arspd_signflip) airspeed_pressure *= -1;
if (_sitl->arspd_signflip) airspeed2_pressure *= -1;
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));
}
float airspeed_pressure = (airspeed * airspeed) / arspd.ratio;
// 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];
// flip sign here for simulating reversed pitot/static connections
if (arspd.signflip) {
airspeed_pressure *= -1;
}
_sitl->state.airspeed_raw_pressure[0] = airspeed_pressure;
_sitl->state.airspeed_raw_pressure[1] = airspeed2_pressure;
// apply airspeed sensor offset in m/s
airspeed_raw = airspeed_pressure + arspd.offset;
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;
}
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
}
_sitl->state.airspeed_raw_pressure[i] = airspeed_pressure;
// 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;
}
airspeed_pin_value[i] = MIN(0xFFFF, airspeed_raw / 4);
}
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;
}
airspeed_pin_value = airspeed_raw / 4;
airspeed_2_pin_value = airspeed2_raw / 4;
}
#endif

Write Preview
Loading…
Cancel
Save