diff --git a/src/modules/commander/Commander.cpp b/src/modules/commander/Commander.cpp index 38d9086f26..1ace6e22d4 100644 --- a/src/modules/commander/Commander.cpp +++ b/src/modules/commander/Commander.cpp @@ -2032,8 +2032,10 @@ void Commander::run() perf_end(_loop_perf); - // sleep if there are no vehicle_commands or action_requests to process - if (!_vehicle_command_sub.updated() && !_action_request_sub.updated()) { + // Always sleep during calibration to avoid competing with calibration + // worker threads for CPU. Otherwise, sleep only when idle. + if (_vehicle_status.calibration_enabled + || (!_vehicle_command_sub.updated() && !_action_request_sub.updated())) { px4_usleep(COMMANDER_MONITORING_INTERVAL); } } diff --git a/src/modules/commander/accelerometer_calibration.cpp b/src/modules/commander/accelerometer_calibration.cpp index 13990e01c9..d34dc29133 100644 --- a/src/modules/commander/accelerometer_calibration.cpp +++ b/src/modules/commander/accelerometer_calibration.cpp @@ -178,6 +178,10 @@ static calibrate_return read_accelerometer_avg(accel_worker_data_s *worker_data, /* use the first sensor to pace the readout, but do per-sensor counts */ while (counts[0] < samples_num) { + // Yield CPU — updatedBlocking() returns immediately when data is + // already available, so with high-rate sensors the loop never blocks. + px4_usleep(1000); + if (accel_sub[0].updatedBlocking(100000)) { for (unsigned accel_index = 0; accel_index < MAX_ACCEL_SENS; accel_index++) { sensor_accel_s arp; diff --git a/src/modules/commander/calibration_routines.cpp b/src/modules/commander/calibration_routines.cpp index 3c4d5ee2cb..9bedb92cad 100644 --- a/src/modules/commander/calibration_routines.cpp +++ b/src/modules/commander/calibration_routines.cpp @@ -90,6 +90,10 @@ enum detect_orientation_return detect_orientation(orb_advert_t *mavlink_log_pub, uORB::SubscriptionBlocking vehicle_acceleration_sub{ORB_ID(vehicle_acceleration)}; while (true) { + // Yield CPU — updateBlocking() returns immediately when data is + // already available, so with high-rate sensors the loop never blocks. + px4_usleep(1000); + vehicle_acceleration_s accel; if (vehicle_acceleration_sub.updateBlocking(accel, 100000)) { diff --git a/src/modules/commander/gyro_calibration.cpp b/src/modules/commander/gyro_calibration.cpp index 676b05297b..df3c07e3ea 100644 --- a/src/modules/commander/gyro_calibration.cpp +++ b/src/modules/commander/gyro_calibration.cpp @@ -59,6 +59,8 @@ #include #include +using namespace time_literals; + static constexpr char sensor_name[] {"gyro"}; static constexpr unsigned MAX_GYROS = 4; @@ -91,12 +93,24 @@ static calibrate_return gyro_calibration_worker(gyro_worker_data_t &worker_data) /* use slowest gyro to pace, but count correctly per-gyro for statistics */ unsigned slow_count = 0; + hrt_abstime last_cancel_check = hrt_absolute_time(); while (slow_count < CALIBRATION_COUNT) { - if (calibrate_cancel_check(worker_data.mavlink_log_pub, calibration_started)) { - return calibrate_return_cancelled; + // Throttle cancel check — calibrate_cancel_check() creates a new + // uORB::Subscription each call, triggering an O(n) topic lookup. + // At high sensor rates this dominates CPU. Check at most every 200ms. + if (hrt_elapsed_time(&last_cancel_check) > 200_ms) { + last_cancel_check = hrt_absolute_time(); + + if (calibrate_cancel_check(worker_data.mavlink_log_pub, calibration_started)) { + return calibrate_return_cancelled; + } } + // Yield CPU — updatedBlocking() returns immediately when data is + // already available, so with high-rate sensors the loop never blocks. + px4_usleep(1000); + if (gyro_sub[0].updatedBlocking(100000)) { unsigned update_count = CALIBRATION_COUNT; diff --git a/src/modules/commander/mag_calibration.cpp b/src/modules/commander/mag_calibration.cpp index 0cecec7139..a3b9c9aa0c 100644 --- a/src/modules/commander/mag_calibration.cpp +++ b/src/modules/commander/mag_calibration.cpp @@ -278,14 +278,22 @@ static calibrate_return mag_calibration_worker(detect_orientation_return orienta uORB::SubscriptionBlocking gyro_sub{ORB_ID(sensor_gyro)}; + hrt_abstime last_cancel_check = hrt_absolute_time(); + while (fabsf(gyro_x_integral) < gyro_int_thresh_rad && fabsf(gyro_y_integral) < gyro_int_thresh_rad && fabsf(gyro_z_integral) < gyro_int_thresh_rad) { - /* abort on request */ - if (calibrate_cancel_check(worker_data->mavlink_log_pub, calibration_started)) { - result = calibrate_return_cancelled; - return result; + // Throttle cancel check — calibrate_cancel_check() creates a new + // uORB::Subscription each call, triggering an O(n) topic lookup. + // At high sensor rates this dominates CPU. Check at most every 200ms. + if (hrt_elapsed_time(&last_cancel_check) > 200_ms) { + last_cancel_check = hrt_absolute_time(); + + if (calibrate_cancel_check(worker_data->mavlink_log_pub, calibration_started)) { + result = calibrate_return_cancelled; + return result; + } } /* abort with timeout */ @@ -297,6 +305,10 @@ static calibrate_return mag_calibration_worker(detect_orientation_return orienta } /* Wait clocking for new data on all gyro */ + // Yield CPU — updateBlocking() returns immediately when data is + // already available, so with high-rate sensors the loop never blocks. + px4_usleep(1000); + sensor_gyro_s gyro; if (gyro_sub.updateBlocking(gyro, 1000_ms)) { @@ -326,14 +338,27 @@ static calibrate_return mag_calibration_worker(detect_orientation_return orienta unsigned poll_errcount = 0; unsigned calibration_counter_side = 0; + last_cancel_check = hrt_absolute_time(); + while (hrt_absolute_time() < calibration_deadline && calibration_counter_side < worker_data->calibration_points_perside) { - if (calibrate_cancel_check(worker_data->mavlink_log_pub, calibration_started)) { - result = calibrate_return_cancelled; - break; + // Throttle cancel check — calibrate_cancel_check() creates a new + // uORB::Subscription each call, triggering an O(n) topic lookup. + // At high sensor rates this dominates CPU. Check at most every 200ms. + if (hrt_elapsed_time(&last_cancel_check) > 200_ms) { + last_cancel_check = hrt_absolute_time(); + + if (calibrate_cancel_check(worker_data->mavlink_log_pub, calibration_started)) { + result = calibrate_return_cancelled; + break; + } } + // Yield CPU — updatedBlocking() returns immediately when data is + // already available, so with high-rate sensors the loop never blocks. + px4_usleep(1000); + if (mag_sub[0].updatedBlocking(1000_ms)) { bool rejected = false; Vector3f new_samples[MAX_MAGS] {};