SITL: added a separate param class for SITL baro params

makes it easier to add more
This commit is contained in:
Andrew Tridgell 2020-11-25 12:27:56 +11:00
parent 9563c1ed33
commit 3456bdb4eb
2 changed files with 32 additions and 21 deletions

View File

@ -229,23 +229,13 @@ const AP_Param::GroupInfo SITL::var_info3[] = {
// @Path: ./SIM_IntelligentEnergy24.cpp // @Path: ./SIM_IntelligentEnergy24.cpp
AP_SUBGROUPINFO(ie24_sim, "IE24_", 32, SITL, IntelligentEnergy24), AP_SUBGROUPINFO(ie24_sim, "IE24_", 32, SITL, IntelligentEnergy24),
// user settable parameters for the 1st barometer // user settable barometer parameters
AP_GROUPINFO("BARO_RND", 35, SITL, baro_noise[0], 0.2f), AP_GROUPINFO("BARO_COUNT", 33, SITL, baro_count, 2),
AP_GROUPINFO("BARO_DRIFT", 36, SITL, baro_drift[0], 0),
AP_GROUPINFO("BARO_DISABLE", 37, SITL, baro_disable[0], 0),
AP_GROUPINFO("BARO_GLITCH", 38, SITL, baro_glitch[0], 0),
AP_GROUPINFO("BARO_FREEZE", 39, SITL, baro_freeze[0], 0),
// user settable parameters for the 2nd barometer AP_SUBGROUPINFO(baro[0], "BARO_", 34, SITL, SITL::BaroParm),
AP_GROUPINFO("BARO2_RND", 41, SITL, baro_noise[1], 0.1f), AP_SUBGROUPINFO(baro[1], "BAR2_", 35, SITL, SITL::BaroParm),
AP_GROUPINFO("BARO2_DRIFT", 42, SITL, baro_drift[1], 0), AP_SUBGROUPINFO(baro[2], "BAR3_", 36, SITL, SITL::BaroParm),
AP_GROUPINFO("BARO2_DISABL", 43, SITL, baro_disable[1], 0),
AP_GROUPINFO("BARO2_GLITCH", 44, SITL, baro_glitch[1], 0),
AP_GROUPINFO("BARO2_FREEZE", 45, SITL, baro_freeze[1], 0),
// user settable common barometer parameters
AP_GROUPINFO("BARO_DELAY", 47, SITL, baro_delay, 0),
AP_GROUPINFO("BARO_COUNT", 48, SITL, baro_count, 1),
// user settable parameters for the 1st airspeed sensor // user settable parameters for the 1st airspeed sensor
AP_GROUPINFO("ARSPD_RND", 50, SITL, arspd_noise[0], 2.0), AP_GROUPINFO("ARSPD_RND", 50, SITL, arspd_noise[0], 2.0),
@ -271,6 +261,17 @@ const AP_Param::GroupInfo SITL::var_info3[] = {
AP_GROUPEND AP_GROUPEND
}; };
// user settable parameters for the barometers
const AP_Param::GroupInfo SITL::BaroParm::var_info[] = {
AP_GROUPINFO("RND", 1, SITL::BaroParm, noise, 0.2f),
AP_GROUPINFO("DRIFT", 2, SITL::BaroParm, drift, 0),
AP_GROUPINFO("DISABLE", 3, SITL::BaroParm, disable, 0),
AP_GROUPINFO("GLITCH", 4, SITL::BaroParm, glitch, 0),
AP_GROUPINFO("FREEZE", 5, SITL::BaroParm, freeze, 0),
AP_GROUPINFO("DELAY", 6, SITL::BaroParm, delay, 0),
AP_GROUPEND
};
// GPS SITL parameters // GPS SITL parameters
const AP_Param::GroupInfo SITL::var_gps[] = { const AP_Param::GroupInfo SITL::var_gps[] = {
AP_GROUPINFO("GPS_DISABLE", 1, SITL, gps_disable[0], 0), AP_GROUPINFO("GPS_DISABLE", 1, SITL, gps_disable[0], 0),

View File

@ -99,6 +99,9 @@ public:
#ifdef SFML_JOYSTICK #ifdef SFML_JOYSTICK
AP_Param::setup_object_defaults(this, var_sfml_joystick); AP_Param::setup_object_defaults(this, var_sfml_joystick);
#endif // SFML_JOYSTICK #endif // SFML_JOYSTICK
for (uint8_t i=0; i<BARO_MAX_INSTANCES; i++) {
AP_Param::setup_object_defaults(&baro[i], baro[i].var_info);
}
if (_singleton != nullptr) { if (_singleton != nullptr) {
AP_HAL::panic("Too many SITL instances"); AP_HAL::panic("Too many SITL instances");
} }
@ -156,10 +159,6 @@ public:
Matrix3f ahrs_rotation_inv; Matrix3f ahrs_rotation_inv;
// noise levels for simulated sensors // noise levels for simulated sensors
AP_Float baro_noise[BARO_MAX_INSTANCES]; // in metres
AP_Float baro_drift[BARO_MAX_INSTANCES]; // in metres per second
AP_Float baro_glitch[BARO_MAX_INSTANCES]; // glitch in meters
AP_Int8 baro_freeze[BARO_MAX_INSTANCES]; // freeze baro to last recorded altitude
AP_Float gyro_noise; // in degrees/second AP_Float gyro_noise; // in degrees/second
AP_Vector3f gyro_scale; // percentage AP_Vector3f gyro_scale; // percentage
AP_Float accel_noise; // in m/s/s AP_Float accel_noise; // in m/s/s
@ -212,7 +211,6 @@ public:
AP_Float accel_fail; // accelerometer failure value AP_Float accel_fail; // accelerometer failure value
AP_Int8 rc_fail; // fail RC input AP_Int8 rc_fail; // fail RC input
AP_Int8 rc_chancount; // channel count AP_Int8 rc_chancount; // channel count
AP_Int8 baro_disable[BARO_MAX_INSTANCES]; // disable simulated barometers
AP_Int8 float_exception; // enable floating point exception checks AP_Int8 float_exception; // enable floating point exception checks
AP_Int8 flow_enable; // enable simulated optflow AP_Int8 flow_enable; // enable simulated optflow
AP_Int16 flow_rate; // optflow data rate (Hz) AP_Int16 flow_rate; // optflow data rate (Hz)
@ -236,6 +234,19 @@ public:
AP_Int8 sfml_joystick_axis[8]; AP_Int8 sfml_joystick_axis[8];
#endif #endif
// baro parameters
class BaroParm {
public:
static const struct AP_Param::GroupInfo var_info[];
AP_Float noise; // in metres
AP_Float drift; // in metres per second
AP_Float glitch; // glitch in meters
AP_Int8 freeze; // freeze baro to last recorded altitude
AP_Int8 disable; // disable simulated barometers
AP_Int16 delay; // barometer data delay in ms
};
BaroParm baro[BARO_MAX_INSTANCES];
// EFI type // EFI type
enum EFIType { enum EFIType {
EFI_TYPE_NONE = 0, EFI_TYPE_NONE = 0,
@ -262,7 +273,6 @@ public:
AP_Float wind_type_alt; AP_Float wind_type_alt;
AP_Float wind_type_coef; AP_Float wind_type_coef;
AP_Int16 baro_delay; // barometer data delay in ms
AP_Int16 mag_delay; // magnetometer data delay in ms AP_Int16 mag_delay; // magnetometer data delay in ms
AP_Int16 wind_delay; // windspeed data delay in ms AP_Int16 wind_delay; // windspeed data delay in ms