From a3cc0c240de91aa52bc430bb7fed2135853de899 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Wed, 2 Apr 2025 10:39:19 +1100 Subject: [PATCH 1/3] SITL: added SIM_IMU_ORIENT parameter for testing non-level flight controllers --- libraries/SITL/SIM_Aircraft.cpp | 2 ++ libraries/SITL/SITL.cpp | 6 ++++++ libraries/SITL/SITL.h | 2 ++ 3 files changed, 10 insertions(+) diff --git a/libraries/SITL/SIM_Aircraft.cpp b/libraries/SITL/SIM_Aircraft.cpp index 16997e3fa043fe..e7db0bca6a2ceb 100644 --- a/libraries/SITL/SIM_Aircraft.cpp +++ b/libraries/SITL/SIM_Aircraft.cpp @@ -411,6 +411,8 @@ void Aircraft::fill_fdm(struct sitl_fdm &fdm) memcpy(fdm.rcin, rcin, rcin_chan_count * sizeof(float)); fdm.bodyMagField = mag_bf; + fdm.bodyMagField.rotate(sitl->imu_orientation); + // copy laser scanner results fdm.scanner.points = scanner.points; fdm.scanner.ranges = scanner.ranges; diff --git a/libraries/SITL/SITL.cpp b/libraries/SITL/SITL.cpp index 5ba76239af668d..df1ac8fd8ca785 100644 --- a/libraries/SITL/SITL.cpp +++ b/libraries/SITL/SITL.cpp @@ -400,6 +400,12 @@ const AP_Param::GroupInfo SIM::var_info2[] = { // @Description: Ground behavior of aircraft (tailsitter, no movement, forward only) AP_GROUPINFO("GND_BEHAV", 41, SIM, gnd_behav, -1), + // @Param: IMU_ORIENT + // @CopyFieldsFrom: AHRS_ORIENTATION + // @DisplayName: IMU orientation + // @Description: Simulated orientation of the IMUs + AP_GROUPINFO("IMU_ORIENT", 42, SIM, imu_orientation, 0), + // sailboat wave and tide simulation parameters // @Param: WAVE_ENABLE diff --git a/libraries/SITL/SITL.h b/libraries/SITL/SITL.h index 3e065facc4a30f..676539d0d501a6 100644 --- a/libraries/SITL/SITL.h +++ b/libraries/SITL/SITL.h @@ -449,6 +449,8 @@ class SIM { AP_Int8 gnd_behav; + AP_Enum imu_orientation; + struct { AP_Int8 enable; // 0: disabled, 1: roll and pitch, 2: roll, pitch and heave AP_Float length; // m From f2d14f1475141ca27956bf555c6b711a58bfb1a8 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Wed, 2 Apr 2025 10:40:05 +1100 Subject: [PATCH 2/3] AP_InertialSensor: support SIM_IMU_ORIENT --- libraries/AP_InertialSensor/AP_InertialSensor_SITL.cpp | 6 ++++++ 1 file changed, 6 insertions(+) diff --git a/libraries/AP_InertialSensor/AP_InertialSensor_SITL.cpp b/libraries/AP_InertialSensor/AP_InertialSensor_SITL.cpp index d9e91a0df429cf..c52b01d54b3aaa 100644 --- a/libraries/AP_InertialSensor/AP_InertialSensor_SITL.cpp +++ b/libraries/AP_InertialSensor/AP_InertialSensor_SITL.cpp @@ -195,6 +195,9 @@ void AP_InertialSensor_SITL::generate_accel() } accel_accum /= nsamples; + + accel_accum.rotate(sitl->imu_orientation); + _rotate_and_correct_accel(accel_instance, accel_accum); _notify_new_accel_raw_sample(accel_instance, accel_accum, AP_HAL::micros64()); @@ -298,6 +301,9 @@ void AP_InertialSensor_SITL::generate_gyro() #endif } gyro_accum /= nsamples; + + gyro_accum.rotate(sitl->imu_orientation); + _rotate_and_correct_gyro(gyro_instance, gyro_accum); _notify_new_gyro_raw_sample(gyro_instance, gyro_accum, AP_HAL::micros64()); } From 4727b610ba709a0bd64fc8eae9a464071ace24fc Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Wed, 2 Apr 2025 10:40:53 +1100 Subject: [PATCH 3/3] AP_AHRS: fixed initial attitude for non-zero AHRS_ORIENTATION we would update the orientation in the IMUs 1s after startup, leaving a short time when the data was processed incorrectly --- libraries/AP_AHRS/AP_AHRS.cpp | 2 ++ libraries/AP_AHRS/AP_AHRS_Backend.cpp | 3 ++- 2 files changed, 4 insertions(+), 1 deletion(-) diff --git a/libraries/AP_AHRS/AP_AHRS.cpp b/libraries/AP_AHRS/AP_AHRS.cpp index 7c5f4b8eef2b95..4e7ae679c3e5b6 100644 --- a/libraries/AP_AHRS/AP_AHRS.cpp +++ b/libraries/AP_AHRS/AP_AHRS.cpp @@ -229,6 +229,8 @@ AP_AHRS::AP_AHRS(uint8_t flags) : // init sets up INS board orientation void AP_AHRS::init() { + update_orientation(); + // EKF1 is no longer supported - handle case where it is selected if (_ekf_type.get() == 1) { AP_BoardConfig::config_error("EKF1 not available"); diff --git a/libraries/AP_AHRS/AP_AHRS_Backend.cpp b/libraries/AP_AHRS/AP_AHRS_Backend.cpp index aac74a5a23e148..eb76c11a854151 100644 --- a/libraries/AP_AHRS/AP_AHRS_Backend.cpp +++ b/libraries/AP_AHRS/AP_AHRS_Backend.cpp @@ -71,7 +71,8 @@ void AP_AHRS::add_trim(float roll_in_radians, float pitch_in_radians, bool save_ void AP_AHRS::update_orientation() { const uint32_t now_ms = AP_HAL::millis(); - if (now_ms - last_orientation_update_ms < 1000) { + if (last_orientation_update_ms != 0 && + now_ms - last_orientation_update_ms < 1000) { // only update once/second return; }