From b5089580a9d4f7f5abe62b50f34832ba61739fc7 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Wed, 20 Jan 2021 11:53:58 +1100 Subject: [PATCH] SITL: cleanup IMU parameters allow for biases per IMU --- libraries/SITL/SITL.cpp | 28 +++++++++++-------- libraries/SITL/SITL.h | 24 ++++++++-------- .../Params/Rolladen-Schneider-LS8b.param | 13 --------- .../Params/Schleicher-ASW27B.param | 13 --------- 4 files changed, 28 insertions(+), 50 deletions(-) diff --git a/libraries/SITL/SITL.cpp b/libraries/SITL/SITL.cpp index 4f18d0fa5c..60f6f27a1e 100644 --- a/libraries/SITL/SITL.cpp +++ b/libraries/SITL/SITL.cpp @@ -44,8 +44,6 @@ SITL *SITL::_singleton = nullptr; // table of user settable parameters const AP_Param::GroupInfo SITL::var_info[] = { - AP_GROUPINFO("GYR_RND", 1, SITL, gyro_noise, 0), - AP_GROUPINFO("ACC_RND", 2, SITL, accel_noise, 0), AP_GROUPINFO("DRIFT_SPEED", 5, SITL, drift_speed, 0.05f), AP_GROUPINFO("DRIFT_TIME", 6, SITL, drift_time, 5), AP_GROUPINFO("ENGINE_MUL", 8, SITL, engine_mul, 1), @@ -55,20 +53,16 @@ const AP_Param::GroupInfo SITL::var_info[] = { AP_GROUPINFO("SERVO_SPEED", 16, SITL, servo_speed, 0.14), AP_GROUPINFO("BATT_VOLTAGE", 19, SITL, batt_voltage, 12.6f), AP_GROUPINFO("BATT_CAP_AH", 20, SITL, batt_capacity_ah, 0), - AP_GROUPINFO("ACCEL_FAIL", 21, SITL, accel_fail, 0), AP_GROUPINFO("SONAR_GLITCH", 23, SITL, sonar_glitch, 0), AP_GROUPINFO("SONAR_RND", 24, SITL, sonar_noise, 0), AP_GROUPINFO("RC_FAIL", 25, SITL, rc_fail, 0), AP_GROUPINFO("FLOAT_EXCEPT", 28, SITL, float_exception, 1), - AP_GROUPINFO("ACC_BIAS", 30, SITL, accel_bias, 0), AP_GROUPINFO("SONAR_SCALE", 32, SITL, sonar_scale, 12.1212f), AP_GROUPINFO("FLOW_ENABLE", 33, SITL, flow_enable, 0), AP_GROUPINFO("TERRAIN", 34, SITL, terrain_enable, 1), AP_GROUPINFO("FLOW_RATE", 35, SITL, flow_rate, 10), AP_GROUPINFO("FLOW_DELAY", 36, SITL, flow_delay, 0), AP_GROUPINFO("WIND_DELAY", 40, SITL, wind_delay, 0), - AP_GROUPINFO("ACC2_RND", 42, SITL, accel2_noise, 0), - AP_GROUPINFO("GYR_SCALE", 44, SITL, gyro_scale, 0), AP_GROUPINFO("ADSB_COUNT", 45, SITL, adsb_plane_count, -1), AP_GROUPINFO("ADSB_RADIUS", 46, SITL, adsb_radius_m, 10000), AP_GROUPINFO("ADSB_ALT", 47, SITL, adsb_altitude_m, 1000), @@ -79,7 +73,6 @@ const AP_Param::GroupInfo SITL::var_info[] = { AP_SUBGROUPEXTENSION("", 54, SITL, var_ins), AP_GROUPINFO("SONAR_POS", 55, SITL, rngfnd_pos_offset, 0), AP_GROUPINFO("FLOW_POS", 56, SITL, optflow_pos_offset, 0), - AP_GROUPINFO("ACC2_BIAS", 57, SITL, accel2_bias, 0), AP_GROUPINFO("ENGINE_FAIL", 58, SITL, engine_fail, 0), AP_SUBGROUPINFO(shipsim, "SHIP_", 59, SITL, ShipSim), AP_SUBGROUPEXTENSION("", 60, SITL, var_mag), @@ -136,10 +129,6 @@ const AP_Param::GroupInfo SITL::var_info2[] = { // optical flow sensor measurement noise in rad/sec AP_GROUPINFO("FLOW_RND", 34, SITL, flow_noise, 0.05f), - // accel and gyro fail masks - AP_GROUPINFO("GYR_FAIL_MSK", 35, SITL, gyro_fail_mask, 0), - AP_GROUPINFO("ACC_FAIL_MSK", 36, SITL, accel_fail_mask, 0), - AP_GROUPINFO("TWIST_X", 37, SITL, twist.x, 0), AP_GROUPINFO("TWIST_Y", 38, SITL, twist.y, 0), AP_GROUPINFO("TWIST_Z", 39, SITL, twist.z, 0), @@ -380,6 +369,23 @@ const AP_Param::GroupInfo SITL::var_ins[] = { AP_SUBGROUPINFO(imu_tcal[0], "IMUT1_", 5, SITL, AP_InertialSensor::TCal), AP_SUBGROUPINFO(imu_tcal[1], "IMUT2_", 6, SITL, AP_InertialSensor::TCal), AP_SUBGROUPINFO(imu_tcal[2], "IMUT3_", 7, SITL, AP_InertialSensor::TCal), + AP_GROUPINFO("ACC1_BIAS", 8, SITL, accel_bias[0], 0), + AP_GROUPINFO("ACC2_BIAS", 9, SITL, accel_bias[1], 0), + AP_GROUPINFO("ACC3_BIAS", 10, SITL, accel_bias[2], 0), + AP_GROUPINFO("GYR1_RND", 11, SITL, gyro_noise[0], 0), + AP_GROUPINFO("GYR2_RND", 12, SITL, gyro_noise[1], 0), + AP_GROUPINFO("GYR3_RND", 13, SITL, gyro_noise[2], 0), + AP_GROUPINFO("ACC1_RND", 14, SITL, accel_noise[0], 0), + AP_GROUPINFO("ACC2_RND", 15, SITL, accel_noise[1], 0), + AP_GROUPINFO("ACC3_RND", 16, SITL, accel_noise[2], 0), + AP_GROUPINFO("GYR1_SCALE", 17, SITL, gyro_scale[0], 0), + AP_GROUPINFO("GYR2_SCALE", 18, SITL, gyro_scale[1], 0), + AP_GROUPINFO("GYR3_SCALE", 19, SITL, gyro_scale[2], 0), + AP_GROUPINFO("ACCEL1_FAIL", 20, SITL, accel_fail[0], 0), + AP_GROUPINFO("ACCEL2_FAIL", 21, SITL, accel_fail[1], 0), + AP_GROUPINFO("ACCEL3_FAIL", 22, SITL, accel_fail[2], 0), + AP_GROUPINFO("GYR_FAIL_MSK", 23, SITL, gyro_fail_mask, 0), + AP_GROUPINFO("ACC_FAIL_MSK", 24, SITL, accel_fail_mask, 0), AP_GROUPEND }; diff --git a/libraries/SITL/SITL.h b/libraries/SITL/SITL.h index 9dc74c813a..4511311375 100644 --- a/libraries/SITL/SITL.h +++ b/libraries/SITL/SITL.h @@ -163,14 +163,6 @@ public: Matrix3f ahrs_rotation; Matrix3f ahrs_rotation_inv; - // noise levels for simulated sensors - AP_Float gyro_noise; // in degrees/second - AP_Vector3f gyro_scale; // percentage - AP_Float accel_noise; // in m/s/s - AP_Float accel2_noise; // in m/s/s - AP_Vector3f accel_bias; // in m/s/s - AP_Vector3f accel2_bias; // in m/s/s - 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 @@ -213,7 +205,6 @@ public: AP_Float batt_voltage; // battery voltage base AP_Float batt_capacity_ah; // battery capacity in Ah - AP_Float accel_fail; // accelerometer failure value AP_Int8 rc_fail; // fail RC input AP_Int8 rc_chancount; // channel count AP_Int8 float_exception; // enable floating point exception checks @@ -327,10 +318,6 @@ public: // minimum throttle for addition of ins noise AP_Float ins_noise_throttle_min; - // gyro and accel fail masks - AP_Int8 gyro_fail_mask; - AP_Int8 accel_fail_mask; - struct { AP_Float x; AP_Float y; @@ -456,6 +443,17 @@ public: AP_Float imu_temp_tconst; AP_Float imu_temp_fixed; AP_InertialSensor::TCal imu_tcal[INS_MAX_INSTANCES]; + + // IMU control parameters + AP_Float gyro_noise[INS_MAX_INSTANCES]; // in degrees/second + AP_Vector3f gyro_scale[INS_MAX_INSTANCES]; // percentage + AP_Float accel_noise[INS_MAX_INSTANCES]; // in m/s/s + AP_Vector3f accel_bias[INS_MAX_INSTANCES]; // in m/s/s + AP_Float accel_fail[INS_MAX_INSTANCES]; // accelerometer failure value + // gyro and accel fail masks + AP_Int8 gyro_fail_mask; + AP_Int8 accel_fail_mask; + }; } // namespace SITL diff --git a/libraries/SITL/examples/SilentWings/Params/Rolladen-Schneider-LS8b.param b/libraries/SITL/examples/SilentWings/Params/Rolladen-Schneider-LS8b.param index fff121472a..dbe887fbf7 100755 --- a/libraries/SITL/examples/SilentWings/Params/Rolladen-Schneider-LS8b.param +++ b/libraries/SITL/examples/SilentWings/Params/Rolladen-Schneider-LS8b.param @@ -610,15 +610,6 @@ SERVO9_MAX,1900 SERVO9_MIN,1100 SERVO9_REVERSED,0 SERVO9_TRIM,1500 -SIM_ACC_BIAS_X,0 -SIM_ACC_BIAS_Y,0 -SIM_ACC_BIAS_Z,0 -SIM_ACC_RND,0 -SIM_ACC2_BIAS_X,0 -SIM_ACC2_BIAS_Y,0 -SIM_ACC2_BIAS_Z,0 -SIM_ACC2_RND,0 -SIM_ACCEL_FAIL,0 SIM_ADSB_ALT,1000 SIM_ADSB_COUNT,-1 SIM_ADSB_RADIUS,10000 @@ -666,10 +657,6 @@ SIM_GPS_NUMSATS,10 SIM_GPS_TYPE,1 SIM_GPS2_ENABLE,0 SIM_GPS2_TYPE,1 -SIM_GYR_RND,0 -SIM_GYR_SCALE_X,0 -SIM_GYR_SCALE_Y,0 -SIM_GYR_SCALE_Z,0 SIM_IMU_POS_X,0 SIM_IMU_POS_Y,0 SIM_IMU_POS_Z,0 diff --git a/libraries/SITL/examples/SilentWings/Params/Schleicher-ASW27B.param b/libraries/SITL/examples/SilentWings/Params/Schleicher-ASW27B.param index d59849357c..9f71cfba0c 100755 --- a/libraries/SITL/examples/SilentWings/Params/Schleicher-ASW27B.param +++ b/libraries/SITL/examples/SilentWings/Params/Schleicher-ASW27B.param @@ -630,15 +630,6 @@ SERVO9_MAX,1900 SERVO9_MIN,1100 SERVO9_REVERSED,0 SERVO9_TRIM,1500 -SIM_ACC_BIAS_X,0 -SIM_ACC_BIAS_Y,0 -SIM_ACC_BIAS_Z,0 -SIM_ACC_RND,0 -SIM_ACC2_BIAS_X,0 -SIM_ACC2_BIAS_Y,0 -SIM_ACC2_BIAS_Z,0 -SIM_ACC2_RND,0 -SIM_ACCEL_FAIL,0 SIM_ADSB_ALT,1000 SIM_ADSB_COUNT,-1 SIM_ADSB_RADIUS,10000 @@ -693,10 +684,6 @@ SIM_GRPS_GRAB,2000 SIM_GRPS_PIN,-1 SIM_GRPS_RELEASE,1000 SIM_GRPS_REVERSE,0 -SIM_GYR_RND,0 -SIM_GYR_SCALE_X,0 -SIM_GYR_SCALE_Y,0 -SIM_GYR_SCALE_Z,0 SIM_IMU_POS_X,0 SIM_IMU_POS_Y,0 SIM_IMU_POS_Z,0