Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-27 19:18:28 -04:00
Code Issues Packages Projects Releases Wiki Activity

SITL: fixed up airspeed parameters

Browse Source 
use a separate airspeed structure and separate ratio per sensor
This commit is contained in:
Andrew Tridgell 2022-05-16 15:25:55 +10:00
parent 61a27698da
commit 2dea725d80
3 changed files with 38 additions and 29 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
libraries/SITL/SIM_MS5525.cpp
View File

@ -73,6 +73,6 @@ void MS5525::get_pressure_temperature_readings(float &P_Pa, float &Temp_C)
// To Do: Add a sensor board temperature offset parameter
Temp_C = (KELVIN_TO_C(SSL_AIR_TEMPERATURE * theta)) + 25.0;
const uint8_t sensor_ofs = 0; // TODO: work out which SITL parameter to use
P_Pa = AP::sitl()->state.airspeed_raw_pressure[sensor_ofs] + AP::sitl()->arspd_offset[sensor_ofs];
const uint8_t instance = 0; // TODO: work out which sensor this is
P_Pa = AP::sitl()->state.airspeed_raw_pressure[instance] + AP::sitl()->airspeed[instance].offset;
}

33
libraries/SITL/SITL.cpp
View File

@ -66,7 +66,6 @@ const AP_Param::GroupInfo SIM::var_info[] = {
AP_GROUPINFO("TERRAIN", 34, SIM, terrain_enable, 1),
AP_GROUPINFO("FLOW_RATE", 35, SIM, flow_rate, 10),
AP_GROUPINFO("FLOW_DELAY", 36, SIM, flow_delay, 0),
AP_GROUPINFO("WIND_DELAY", 40, SIM, wind_delay, 0),
AP_GROUPINFO("ADSB_COUNT", 45, SIM, adsb_plane_count, -1),
AP_GROUPINFO("ADSB_RADIUS", 46, SIM, adsb_radius_m, 10000),
AP_GROUPINFO("ADSB_ALT", 47, SIM, adsb_altitude_m, 1000),
@ -282,22 +281,11 @@ const AP_Param::GroupInfo SIM::var_info3[] = {
AP_GROUPINFO("ESC_TELEM", 40, SIM, esc_telem, 1),
// user settable parameters for the 1st airspeed sensor
AP_GROUPINFO("ARSPD_RND", 50, SIM, arspd_noise[0], 2.0),
AP_GROUPINFO("ARSPD_OFS", 51, SIM, arspd_offset[0], 2013),
AP_GROUPINFO("ARSPD_FAIL", 52, SIM, arspd_fail[0], 0),
AP_GROUPINFO("ARSPD_FAILP", 53, SIM, arspd_fail_pressure[0], 0),
AP_GROUPINFO("ARSPD_PITOT", 54, SIM, arspd_fail_pitot_pressure[0], 0),
AP_SUBGROUPINFO(airspeed[0], "ARSPD_", 50, SIM, SIM::AirspeedParm),
#if AIRSPEED_MAX_SENSORS > 1
AP_SUBGROUPINFO(airspeed[1], "ARSPD2_", 51, SIM, SIM::AirspeedParm),
#endif
// user settable parameters for the 2nd airspeed sensor
AP_GROUPINFO("ARSPD2_RND", 56, SIM, arspd_noise[1], 2.0),
AP_GROUPINFO("ARSPD2_OFS", 57, SIM, arspd_offset[1], 2013),
AP_GROUPINFO("ARSPD2_FAIL", 58, SIM, arspd_fail[1], 0),
AP_GROUPINFO("ARSPD2_FAILP", 59, SIM, arspd_fail_pressure[1], 0),
AP_GROUPINFO("ARSPD2_PITOT", 60, SIM, arspd_fail_pitot_pressure[1], 0),
// user settable common airspeed parameters
AP_GROUPINFO("ARSPD_SIGN", 62, SIM, arspd_signflip, 0),
#ifdef SFML_JOYSTICK
AP_SUBGROUPEXTENSION("", 63, SIM, var_sfml_joystick),
@ -323,6 +311,19 @@ const AP_Param::GroupInfo SIM::BaroParm::var_info[] = {
AP_GROUPEND
};
// user settable parameters for airspeed sensors
const AP_Param::GroupInfo SIM::AirspeedParm::var_info[] = {
// user settable parameters for the 1st airspeed sensor
AP_GROUPINFO("RND", 1, SIM::AirspeedParm, noise, 2.0),
AP_GROUPINFO("OFS", 2, SIM::AirspeedParm, offset, 2013),
AP_GROUPINFO("FAIL", 3, SIM::AirspeedParm, fail, 0),
AP_GROUPINFO("FAILP", 4, SIM::AirspeedParm, fail_pressure, 0),
AP_GROUPINFO("PITOT", 5, SIM::AirspeedParm, fail_pitot_pressure, 0),
AP_GROUPINFO("SIGN", 6, SIM::AirspeedParm, signflip, 0),
AP_GROUPINFO("RATIO", 7, SIM::AirspeedParm, ratio, 1.99),
AP_GROUPEND
};
#if HAL_SIM_GPS_ENABLED
// GPS SITL parameters
const AP_Param::GroupInfo SIM::var_gps[] = {

30
libraries/SITL/SITL.h
View File

@ -7,6 +7,7 @@
#include <AP_Math/AP_Math.h>
#include <GCS_MAVLink/GCS_MAVLink.h>
#include <AP_Baro/AP_Baro.h>
#include <AP_Airspeed/AP_Airspeed.h>
#include <AP_Common/Location.h>
#include <AP_Compass/AP_Compass.h>
#include <AP_InertialSensor/AP_InertialSensor.h>
@ -78,7 +79,7 @@ struct sitl_fdm {
} scanner;
float rangefinder_m[RANGEFINDER_MAX_INSTANCES];
float airspeed_raw_pressure[2];
float airspeed_raw_pressure[AIRSPEED_MAX_SENSORS];
struct {
float speed;
@ -113,6 +114,9 @@ public:
for (uint8_t i=0; i<BARO_MAX_INSTANCES; i++) {
AP_Param::setup_object_defaults(&baro[i], baro[i].var_info);
}
for (uint8_t i=0; i<AIRSPEED_MAX_SENSORS; i++) {
AP_Param::setup_object_defaults(&airspeed[i], airspeed[i].var_info);
}
if (_singleton != nullptr) {
AP_HAL::panic("Too many SITL instances");
}
@ -163,12 +167,6 @@ public:
Matrix3f ahrs_rotation;
Matrix3f ahrs_rotation_inv;
AP_Float arspd_noise[2]; // pressure noise
AP_Float arspd_fail[2]; // airspeed value in m/s to fail to
AP_Float arspd_fail_pressure[2]; // pitot tube failure pressure in Pa
AP_Float arspd_fail_pitot_pressure[2]; // pitot tube failure pressure in Pa
AP_Float arspd_offset[2]; // airspeed sensor offset in m/s
AP_Float mag_noise; // in mag units (earth field is 818)
AP_Vector3f mag_mot; // in mag units per amp
AP_Vector3f mag_ofs[HAL_COMPASS_MAX_SENSORS]; // in mag units
@ -255,6 +253,20 @@ public:
};
BaroParm baro[BARO_MAX_INSTANCES];
// airspeed parameters
class AirspeedParm {
public:
static const struct AP_Param::GroupInfo var_info[];
AP_Float noise; // pressure noise
AP_Float fail; // airspeed value in m/s to fail to
AP_Float fail_pressure; // pitot tube failure pressure in Pa
AP_Float fail_pitot_pressure; // pitot tube failure pressure in Pa
AP_Float offset; // airspeed sensor offset in m/s
AP_Float ratio; // airspeed ratios
AP_Int8 signflip;
};
AirspeedParm airspeed[AIRSPEED_MAX_SENSORS];
// EFI type
enum EFIType {
EFI_TYPE_NONE = 0,
@ -282,7 +294,6 @@ public:
AP_Float wind_type_coef;
AP_Int16 mag_delay; // magnetometer data delay in ms
AP_Int16 wind_delay; // windspeed data delay in ms
// ADSB related run-time options
AP_Int16 adsb_plane_count;
@ -308,9 +319,6 @@ public:
AP_Int8 thermal_scenario;
// differential pressure sensor tube order
AP_Int8 arspd_signflip;
// weight on wheels pin
AP_Int8 wow_pin;