diff --git a/libraries/AP_InertialSensor/AP_InertialSensor.cpp b/libraries/AP_InertialSensor/AP_InertialSensor.cpp index 4d4480e43f..37504d8897 100644 --- a/libraries/AP_InertialSensor/AP_InertialSensor.cpp +++ b/libraries/AP_InertialSensor/AP_InertialSensor.cpp @@ -1460,7 +1460,7 @@ bool AP_InertialSensor::get_accel_health_all(void) const return (get_accel_count() > 0); } -#if HAL_GCS_ENABLED +#if HAL_GCS_ENABLED && AP_AHRS_ENABLED /* calculate the trim_roll and trim_pitch. This is used for redoing the trim without needing a full accel cal @@ -1521,6 +1521,7 @@ MAV_RESULT AP_InertialSensor::calibrate_trim() // reset ahrs's trim to suggested values from calibration routine ahrs.set_trim(trim_rad); + last_accel_cal_ms = AP_HAL::millis(); _trimming_accel = false; return MAV_RESULT_ACCEPTED; @@ -1530,7 +1531,7 @@ failed: _trimming_accel = false; return MAV_RESULT_FAILED; } -#endif // HAL_GCS_ENABLED +#endif // HAL_GCS_ENABLED && AP_AHRS_ENABLED /* check if the accelerometers are calibrated in 3D and that current number of accels matched number when calibrated @@ -2408,7 +2409,9 @@ bool AP_InertialSensor::calibrate_gyros() if (!gyro_calibrated_ok_all()) { return false; } +#if AP_AHRS_ENABLED AP::ahrs().reset_gyro_drift(); +#endif return true; } @@ -2550,8 +2553,10 @@ MAV_RESULT AP_InertialSensor::simple_accel_cal() #endif } +#if AP_AHRS_ENABLED // force trim to zero AP::ahrs().set_trim(Vector3f(0, 0, 0)); +#endif } else { DEV_PRINTF("\nFAILED\n"); // restore old values @@ -2572,8 +2577,10 @@ MAV_RESULT AP_InertialSensor::simple_accel_cal() update(); } +#if AP_AHRS_ENABLED // and reset state estimators AP::ahrs().reset(); +#endif // stop flashing leds AP_Notify::flags.initialising = false; diff --git a/libraries/AP_InertialSensor/AP_InertialSensor_Backend.cpp b/libraries/AP_InertialSensor/AP_InertialSensor_Backend.cpp index 9ceb54c12d..3c45c611b1 100644 --- a/libraries/AP_InertialSensor/AP_InertialSensor_Backend.cpp +++ b/libraries/AP_InertialSensor/AP_InertialSensor_Backend.cpp @@ -218,7 +218,7 @@ void AP_InertialSensor_Backend::apply_gyro_filters(const uint8_t instance, const continue; } bool inactive = notch.is_inactive(); -#ifndef HAL_BUILD_AP_PERIPH +#if AP_AHRS_ENABLED // by default we only run the expensive notch filters on the // currently active IMU we reset the inactive notch filters so // that if we switch IMUs we're not left with old data @@ -442,8 +442,14 @@ void AP_InertialSensor_Backend::log_gyro_raw(uint8_t instance, const uint64_t sa return; } +#if AP_AHRS_ENABLED + const bool log_because_primary_gyro = _imu.raw_logging_option_set(AP_InertialSensor::RAW_LOGGING_OPTION::PRIMARY_GYRO_ONLY) && (instance == AP::ahrs().get_primary_gyro_index()); +#else + const bool log_because_primary_gyro = false; +#endif + if (_imu.raw_logging_option_set(AP_InertialSensor::RAW_LOGGING_OPTION::ALL_GYROS) || - (_imu.raw_logging_option_set(AP_InertialSensor::RAW_LOGGING_OPTION::PRIMARY_GYRO_ONLY) && (instance == AP::ahrs().get_primary_gyro_index())) || + log_because_primary_gyro || should_log_imu_raw()) { if (_imu.raw_logging_option_set(AP_InertialSensor::RAW_LOGGING_OPTION::PRE_AND_POST_FILTER)) {