diff --git a/src/modules/sensors/vehicle_imu/VehicleIMU.cpp b/src/modules/sensors/vehicle_imu/VehicleIMU.cpp index a2cc0ebe5e..43b1e7cd3e 100644 --- a/src/modules/sensors/vehicle_imu/VehicleIMU.cpp +++ b/src/modules/sensors/vehicle_imu/VehicleIMU.cpp @@ -328,15 +328,15 @@ void VehicleIMU::Run() publish_status = true; - // start notifying the user periodically if there's significant continuous clipping - const uint64_t clipping_total = _status.accel_clipping[0] + _status.accel_clipping[1] + _status.accel_clipping[2]; + if (_accel_calibration.enabled() && (hrt_elapsed_time(&_last_clipping_notify_time) > 3_s)) { + // start notifying the user periodically if there's significant continuous clipping + const uint64_t clipping_total = _status.accel_clipping[0] + _status.accel_clipping[1] + _status.accel_clipping[2]; - if ((hrt_elapsed_time(&_last_clipping_notify_time) > 3_s) - && (clipping_total > _last_clipping_notify_total_count + 100)) { - - mavlink_log_critical(&_mavlink_log_pub, "Accel %d clipping, land immediately!", _instance); - _last_clipping_notify_time = accel.timestamp_sample; - _last_clipping_notify_total_count = clipping_total; + if (clipping_total > _last_clipping_notify_total_count + 1000) { + mavlink_log_critical(&_mavlink_log_pub, "Accel %d clipping, not safe to fly!", _instance); + _last_clipping_notify_time = accel.timestamp_sample; + _last_clipping_notify_total_count = clipping_total; + } } }