diff --git a/libraries/SITL/SIM_MS5525.cpp b/libraries/SITL/SIM_MS5525.cpp index 21eff6de8d..24e2000d52 100644 --- a/libraries/SITL/SIM_MS5525.cpp +++ b/libraries/SITL/SIM_MS5525.cpp @@ -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; } diff --git a/libraries/SITL/SITL.cpp b/libraries/SITL/SITL.cpp index ef539a26b1..b9b387c373 100644 --- a/libraries/SITL/SITL.cpp +++ b/libraries/SITL/SITL.cpp @@ -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), - - // 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), + 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 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[] = { diff --git a/libraries/SITL/SITL.h b/libraries/SITL/SITL.h index e34d9f15f9..091257cb44 100644 --- a/libraries/SITL/SITL.h +++ b/libraries/SITL/SITL.h @@ -7,6 +7,7 @@ #include #include #include +#include #include #include #include @@ -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