From e1920363455383d976efb596af1fd3d897d8af08 Mon Sep 17 00:00:00 2001 From: Jacob Dahl <37091262+dakejahl@users.noreply.github.com> Date: Wed, 11 Nov 2026 12:38:01 -0900 Subject: [PATCH] fix(commander): ack calibration cancels once and stop promptly (#29514) * feat(simulator_sih): add orient command to pose the vehicle for calibration Accel and mag calibration need the vehicle held still on each side and rotated, which SIH could not do, so neither could be tested in simulation. `simulator_sih orient` sets the attitude kinematically, keeping the IMU and the simulated mag consistent with it, and `orient all` runs every side in turn so a calibration started from a GCS completes on its own. Assisted-by: Claude:claude-opus-5-5 * fix(commander): hand a calibration cancel to the running calibration A cancel got TEMPORARILY_REJECTED from Commander and ACCEPTED from the calibration, in either order. The calibration polled for it with a fresh vehicle_command subscription that only saw the newest command, filtered it by a per-side start time, and did not poll while waiting for the vehicle to be still or while measuring a side. Baro and level calibration never polled. A cancel could be lost or take until the vehicle was held still. Commander now hands the cancel to the running calibration, which acks it once when it stops, or rejects it if it ends without stopping. Every calibration loop checks for it, and accel calibration no longer reports a failure after a cancel. Fixes #29424 Assisted-by: Claude:claude-opus-5-5 * test(mavsdk_tests): run accel and mag calibration and cancels on SIH Covers a full accel and mag calibration, and a cancel in each place it used to be missed or delayed, checking it gets exactly one ack. Assisted-by: Claude:claude-opus-5-5 * perf(simulator_sih): build the orient command for SITL only Commander denies sensor calibration when SIH runs on flight controller hardware, so orient was about 1.8 KB of unusable code on every NuttX board that builds SIH. Assisted-by: Claude:claude-opus-5-5 * fix(commander): open calibration cancels before the worker thread starts Commander handles one command per loop and the worker thread runs at low priority, so a cancel queued right behind the start could be rejected before the worker opened it, and the calibration kept running. Assisted-by: Claude:claude-opus-5-5 * fix(commander): stop reporting a cancelled gyro calibration as failed A cancel broke out of the retry loop into the failure report, as accel calibration did before. Assisted-by: Claude:claude-opus-5-5 * test(mavsdk_tests): cover gyro cancel and remove two calibration races A retried start is rejected once the calibration runs, and that late ack could be counted against the cancel, so the start is sent once. The baro cancel landed 1 s into a 3 s calibration, about 200 ms of host time at speed factor 10; it is now sent right after the start. Assisted-by: Claude:claude-opus-5-5 * style(mavsdk_tests): wrap the calibration start assert for flake8 The 104-character line failed the Python CI checks. Assisted-by: Claude:claude-opus-5-5 * fix(commander): require param7 zero for a calibration cancel An ESC calibration (param7 = 1) or baro temperature calibration (param7 = 3) sent while a calibration ran matched the cancel test. It stopped the running calibration and was acked ACCEPTED, though the requested calibration never started. `commander calibrate esc` did the same. Assisted-by: Claude:claude-fable-5-2 --- docs/en/sim_sih/index.md | 14 ++ src/modules/commander/Commander.cpp | 10 +- .../commander/accelerometer_calibration.cpp | 21 +- .../commander/airspeed_calibration.cpp | 4 +- src/modules/commander/baro_calibration.cpp | 4 + .../commander/calibration_routines.cpp | 119 ++++++---- src/modules/commander/calibration_routines.h | 13 +- src/modules/commander/gyro_calibration.cpp | 20 +- src/modules/commander/level_calibration.cpp | 4 + src/modules/commander/mag_calibration.cpp | 30 +-- src/modules/commander/worker_thread.cpp | 18 ++ src/modules/simulation/simulator_sih/sih.cpp | 218 +++++++++++++++++- src/modules/simulation/simulator_sih/sih.hpp | 22 ++ .../autopilot_tester_calibration.py | 121 ++++++++++ test/mavsdk_tests/configs/sih-sitl.json | 5 + test/mavsdk_tests/conftest.py | 11 +- test/mavsdk_tests/pytest.ini | 1 + test/mavsdk_tests/test_calibration.py | 62 +++++ 18 files changed, 605 insertions(+), 92 deletions(-) create mode 100644 test/mavsdk_tests/autopilot_tester_calibration.py create mode 100644 test/mavsdk_tests/test_calibration.py diff --git a/docs/en/sim_sih/index.md b/docs/en/sim_sih/index.md index f4a961f8808..48a3aeca50c 100644 --- a/docs/en/sim_sih/index.md +++ b/docs/en/sim_sih/index.md @@ -149,6 +149,20 @@ export PX4_HOME_ALT=28.5 make px4_sitl_sih sihsim_quadx ``` +#### Sensor Calibration + +`simulator_sih orient` poses the vehicle on the ground the way accelerometer and magnetometer calibration ask for, so either calibration can be run from QGroundControl or a script. +Run it from the PX4 shell, or the [MAVLink shell](../debug/mavlink_shell.md), after starting the calibration: + +| Command | Pose | +| ------------------------------------ | --------------------------------------------------------------------------------- | +| `simulator_sih orient all` | Each side in turn: 10 s still, then 10 s rotating. Completes either calibration. | +| `simulator_sih orient ` | Still, with `` facing down: `down`, `up`, `front`, `back`, `left`, `right`. | +| `simulator_sih orient rotate` | Rotating about the vertical with `` down, with a tilt swing. | +| `simulator_sih orient off` | Level, then back to the flight dynamics. | + +`orient` is only built for SITL: sensor calibration is rejected when SIH runs on flight controller hardware. + ### ROS 2 Integration SIH works with ROS 2 via the [uXRCE-DDS](../middleware/uxrce_dds.md) client, which auto-starts in SITL mode. diff --git a/src/modules/commander/Commander.cpp b/src/modules/commander/Commander.cpp index 67bcb95f457..a092e555611 100644 --- a/src/modules/commander/Commander.cpp +++ b/src/modules/commander/Commander.cpp @@ -42,6 +42,7 @@ /* commander module headers */ #include "Arming/ArmAuthorization/ArmAuthorization.h" +#include "calibration_routines.h" #include "commander_helper.h" #include "esc_calibration.h" #define DEFINE_GET_PX4_CUSTOM_MODE @@ -1444,7 +1445,14 @@ Commander::handle_command(const vehicle_command_s &cmd) case vehicle_command_s::VEHICLE_CMD_PREFLIGHT_CALIBRATION: { - if (isArmed() || _worker_thread.isBusy()) { + if (_worker_thread.isBusy() + && (int)(cmd.param1) == 0 && (int)(cmd.param2) == 0 && (int)(cmd.param3) == 0 + && (int)(cmd.param4) == 0 && (int)(cmd.param5) == 0 && (int)(cmd.param6) == 0 + && (int)(cmd.param7) == 0) { + // cancel: the running calibration acks it once it has stopped + calibrate_cancel_request(cmd); + + } else if (isArmed() || _worker_thread.isBusy()) { // reject if armed or shutting down answer_command(cmd, vehicle_command_ack_s::VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED); diff --git a/src/modules/commander/accelerometer_calibration.cpp b/src/modules/commander/accelerometer_calibration.cpp index 0a71f2c0fb1..b7b578060ef 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) { + if (calibrate_cancel_check(worker_data->mavlink_log_pub)) { + 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); @@ -221,7 +225,11 @@ static calibrate_return accel_calibration_worker(detect_orientation_return orien calibration_log_info(worker_data->mavlink_log_pub, "[cal] Hold still, measuring %s side", detect_orientation_str(orientation)); - read_accelerometer_avg(worker_data, orientation, samples_num); + const calibrate_return result = read_accelerometer_avg(worker_data, orientation, samples_num); + + if (result != calibrate_return_ok) { + return result; + } // check accel for (unsigned accel_index = 0; accel_index < MAX_ACCEL_SENS; accel_index++) { @@ -334,8 +342,15 @@ int do_accel_calibration(orb_advert_t *mavlink_log_pub) /* measure and calculate offsets & scales */ bool data_collected[detect_orientation_side_count] {}; - if (calibrate_from_orientation(mavlink_log_pub, data_collected, accel_calibration_worker, &worker_data, - false) == calibrate_return_ok) { + const calibrate_return result = calibrate_from_orientation(mavlink_log_pub, data_collected, accel_calibration_worker, + &worker_data, false); + + if (result == calibrate_return_cancelled) { + // already reported + return PX4_ERROR; + } + + if (result == calibrate_return_ok) { bool param_save = false; bool failed = true; diff --git a/src/modules/commander/airspeed_calibration.cpp b/src/modules/commander/airspeed_calibration.cpp index a63598cc89c..d2f9c635817 100644 --- a/src/modules/commander/airspeed_calibration.cpp +++ b/src/modules/commander/airspeed_calibration.cpp @@ -90,7 +90,7 @@ static int collect_zero_offsets(orb_advert_t *mavlink_log_pub, const hrt_abstime while (calibration_counter < calibration_count) { - if (calibrate_cancel_check(mavlink_log_pub, calibration_started)) { + if (calibrate_cancel_check(mavlink_log_pub)) { return PX4_ERROR; } @@ -185,7 +185,7 @@ static int check_pitot_direction(orb_advert_t *mavlink_log_pub, const hrt_abstim while (calibration_counter < maxcount) { - if (calibrate_cancel_check(mavlink_log_pub, calibration_started)) { + if (calibrate_cancel_check(mavlink_log_pub)) { return PX4_ERROR; } diff --git a/src/modules/commander/baro_calibration.cpp b/src/modules/commander/baro_calibration.cpp index 93c188855f2..17e40ead0ee 100644 --- a/src/modules/commander/baro_calibration.cpp +++ b/src/modules/commander/baro_calibration.cpp @@ -87,6 +87,10 @@ int do_baro_calibration(orb_advert_t *mavlink_log_pub) while (hrt_elapsed_time(&time_start_us) < 3_s) { + if (calibrate_cancel_check(mavlink_log_pub)) { + return PX4_ERROR; + } + for (int instance = 0; instance < MAX_SENSOR_COUNT; instance++) { sensor_baro_s sensor_baro; diff --git a/src/modules/commander/calibration_routines.cpp b/src/modules/commander/calibration_routines.cpp index 9bedb92cadd..d88a90dea5a 100644 --- a/src/modules/commander/calibration_routines.cpp +++ b/src/modules/commander/calibration_routines.cpp @@ -38,8 +38,8 @@ * @author Lorenz Meier */ +#include #include -#include #include #include @@ -54,7 +54,6 @@ #include #include #include -#include #include #include "calibration_routines.h" @@ -90,6 +89,10 @@ enum detect_orientation_return detect_orientation(orb_advert_t *mavlink_log_pub, uORB::SubscriptionBlocking vehicle_acceleration_sub{ORB_ID(vehicle_acceleration)}; while (true) { + if (calibrate_cancel_check(mavlink_log_pub)) { + return ORIENTATION_ERROR; + } + // Yield CPU — updateBlocking() returns immediately when data is // already available, so with high-rate sensors the loop never blocks. px4_usleep(1000); @@ -225,14 +228,13 @@ calibrate_return calibrate_from_orientation(orb_advert_t *mavlink_log_pub, bool side_data_collected[detect_orientation_side_count], calibration_from_orientation_worker_t calibration_worker, void *worker_data, bool lenient_still_position) { - const hrt_abstime calibration_started = hrt_absolute_time(); calibrate_return result = calibrate_return_ok; unsigned orientation_failures = 0; // Rotate through all requested orientation while (true) { - if (calibrate_cancel_check(mavlink_log_pub, calibration_started)) { + if (calibrate_cancel_check(mavlink_log_pub)) { result = calibrate_return_cancelled; break; } @@ -274,6 +276,11 @@ calibrate_return calibrate_from_orientation(orb_advert_t *mavlink_log_pub, px4_usleep(20000); enum detect_orientation_return orient = detect_orientation(mavlink_log_pub, lenient_still_position); + if (calibrate_cancel_check(mavlink_log_pub)) { + result = calibrate_return_cancelled; + break; + } + if (orient == ORIENTATION_ERROR) { orientation_failures++; calibration_log_info(mavlink_log_pub, "[cal] detected motion, hold still..."); @@ -322,53 +329,83 @@ calibrate_return calibrate_from_orientation(orb_advert_t *mavlink_log_pub, return result; } -bool calibrate_cancel_check(orb_advert_t *mavlink_log_pub, const hrt_abstime &calibration_started) +// Commander hands a GCS cancel to the calibration running on the worker thread, which sends the only ack for it +enum class CancelState : int { + Closed, ///< nothing running that can be cancelled + Open, ///< a calibration that checks for a cancel is running + Requested, ///< a cancel was handed over and not acted on yet + Cancelled ///< the calibration stopped and acked the cancel +}; + +static px4::atomic_int cancel_state{(int)CancelState::Closed}; + +// Only written by Commander while Open, only read by the calibration once Requested +static vehicle_command_s cancel_cmd{}; + +static void send_cancel_ack(const vehicle_command_s &cmd, uint8_t result) { - bool ret = false; + vehicle_command_ack_s command_ack{}; + command_ack.command = cmd.command; + command_ack.result = result; + command_ack.target_system = cmd.source_system; + command_ack.target_component = cmd.source_component; + command_ack.timestamp = hrt_absolute_time(); - uORB::Subscription vehicle_command_sub{ORB_ID(vehicle_command)}; - vehicle_command_s cmd; + uORB::Publication command_ack_pub{ORB_ID(vehicle_command_ack)}; + command_ack_pub.publish(command_ack); +} - while (vehicle_command_sub.update(&cmd)) { - if (cmd.command == vehicle_command_s::VEHICLE_CMD_PREFLIGHT_CALIBRATION) { - // only handle commands sent after calibration started from external sources - if ((cmd.timestamp > calibration_started) && cmd.from_external) { +void calibrate_cancel_open() +{ + cancel_state.store((int)CancelState::Open); +} - vehicle_command_ack_s command_ack{}; +void calibrate_cancel_close() +{ + int state = cancel_state.load(); - if ((int)cmd.param1 == 0 && - (int)cmd.param2 == 0 && - (int)cmd.param3 == 0 && - (int)cmd.param4 == 0 && - (int)cmd.param5 == 0 && - (int)cmd.param6 == 0) { + while (!cancel_state.compare_exchange(&state, (int)CancelState::Closed)) {} - command_ack.result = vehicle_command_ack_s::VEHICLE_CMD_RESULT_ACCEPTED; - mavlink_log_critical(mavlink_log_pub, CAL_QGC_CANCELLED_MSG); - tune_positive(true); - ret = true; + if (state == (int)CancelState::Requested) { + // the calibration finished before it got to the cancel + send_cancel_ack(cancel_cmd, vehicle_command_ack_s::VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED); + } +} - } else { - command_ack.result = vehicle_command_ack_s::VEHICLE_CMD_RESULT_DENIED; - mavlink_log_critical(mavlink_log_pub, "command denied during calibration: %" PRIu32 "\t", cmd.command); - events::send(events::ID("commander_cal_cmd_denied"), {events::Log::Error, events::LogInternal::Info}, - "Command denied during calibration: {1}", cmd.command); - tune_negative(true); - ret = false; - } +void calibrate_cancel_request(const vehicle_command_s &cmd) +{ + int state = cancel_state.load(); - command_ack.command = cmd.command; - command_ack.target_system = cmd.source_system; - command_ack.target_component = cmd.source_component; - command_ack.timestamp = hrt_absolute_time(); + if (state == (int)CancelState::Open) { + cancel_cmd = cmd; - uORB::Publication command_ack_pub{ORB_ID(vehicle_command_ack)}; - command_ack_pub.publish(command_ack); - - return ret; - } + if (cancel_state.compare_exchange(&state, (int)CancelState::Requested)) { + return; } } - return ret; + if (state == (int)CancelState::Closed) { + tune_negative(true); + send_cancel_ack(cmd, vehicle_command_ack_s::VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED); + + } else if (state == (int)CancelState::Cancelled) { + // a retry of a cancel already acted on + send_cancel_ack(cmd, vehicle_command_ack_s::VEHICLE_CMD_RESULT_ACCEPTED); + } + + // a retry while Requested is answered by the ack still to come for the first cancel +} + +bool calibrate_cancel_check(orb_advert_t *mavlink_log_pub) +{ + int state = (int)CancelState::Requested; + + if (cancel_state.compare_exchange(&state, (int)CancelState::Cancelled)) { + mavlink_log_critical(mavlink_log_pub, CAL_QGC_CANCELLED_MSG); + tune_positive(true); + send_cancel_ack(cancel_cmd, vehicle_command_ack_s::VEHICLE_CMD_RESULT_ACCEPTED); + return true; + } + + return state == (int)CancelState::Cancelled; } diff --git a/src/modules/commander/calibration_routines.h b/src/modules/commander/calibration_routines.h index a18fa1d4d64..ab4c5c1eb6b 100644 --- a/src/modules/commander/calibration_routines.h +++ b/src/modules/commander/calibration_routines.h @@ -38,6 +38,7 @@ #include #include +#include // The order of these cannot change since the calibration calculations depend on them in this order enum detect_orientation_return { @@ -79,8 +80,18 @@ calibrate_return calibrate_from_orientation(orb_advert_t *mavlink_log_pub, ///< void *worker_data, ///< Opaque data passed to worker routine bool lenient_still_detection); ///< true: Use more lenient still position detection +/// Lets a GCS cancel the calibration about to start. It must call calibrate_cancel_check() often. +void calibrate_cancel_open(); + +/// Ends cancelling when the calibration returns. A cancel it never acted on is rejected. +void calibrate_cancel_close(); + +/// Hands a GCS cancel to the running calibration, which acks it once it has stopped +void calibrate_cancel_request(const vehicle_command_s &cmd); + /// Used to periodically check for a cancel command -bool calibrate_cancel_check(orb_advert_t *mavlink_log_pub, const hrt_abstime &calibration_started); +/// @return true once the calibration has been cancelled +bool calibrate_cancel_check(orb_advert_t *mavlink_log_pub); // TODO FIXME: below are workarounds for QGC. The issue is that sometimes diff --git a/src/modules/commander/gyro_calibration.cpp b/src/modules/commander/gyro_calibration.cpp index 35163b9de6d..56d27e64946 100644 --- a/src/modules/commander/gyro_calibration.cpp +++ b/src/modules/commander/gyro_calibration.cpp @@ -59,8 +59,6 @@ #include #include -using namespace time_literals; - static constexpr char sensor_name[] {"gyro"}; static constexpr unsigned MAX_GYROS = 4; @@ -79,7 +77,6 @@ struct gyro_worker_data_t { static calibrate_return gyro_calibration_worker(gyro_worker_data_t &worker_data) { - const hrt_abstime calibration_started = hrt_absolute_time(); unsigned calibration_counter[MAX_GYROS] {}; static constexpr unsigned CALIBRATION_COUNT = 250; unsigned poll_errcount = 0; @@ -93,18 +90,10 @@ 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) { - // 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; - } + if (calibrate_cancel_check(worker_data.mavlink_log_pub)) { + return calibrate_return_cancelled; } // Yield CPU — updatedBlocking() returns immediately when data is @@ -217,9 +206,8 @@ int do_gyro_calibration(orb_advert_t *mavlink_log_pub) calibrate_return cal_return = gyro_calibration_worker(worker_data); if (cal_return == calibrate_return_cancelled) { - // Cancel message already sent, we are done here - res = PX4_ERROR; - break; + // already reported + return PX4_ERROR; } else if (cal_return == calibrate_return_error) { res = PX4_ERROR; diff --git a/src/modules/commander/level_calibration.cpp b/src/modules/commander/level_calibration.cpp index bab21bd6a28..e07f4ad0778 100644 --- a/src/modules/commander/level_calibration.cpp +++ b/src/modules/commander/level_calibration.cpp @@ -93,6 +93,10 @@ int do_level_calibration(orb_advert_t *mavlink_log_pub) while (hrt_elapsed_time(&start) < calibration_duration) { + if (calibrate_cancel_check(mavlink_log_pub)) { + return 1; + } + vehicle_attitude_s att{}; if (!att_sub.updateBlocking(att, 100000)) { diff --git a/src/modules/commander/mag_calibration.cpp b/src/modules/commander/mag_calibration.cpp index c8633d45109..e28044b76f3 100644 --- a/src/modules/commander/mag_calibration.cpp +++ b/src/modules/commander/mag_calibration.cpp @@ -191,7 +191,6 @@ static float get_sphere_radius() static calibrate_return mag_calibration_worker(detect_orientation_return orientation, void *data) { - const hrt_abstime calibration_started = hrt_absolute_time(); calibrate_return result = calibrate_return_ok; mag_worker_data_t *worker_data = (mag_worker_data_t *)(data); @@ -221,22 +220,12 @@ 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) { - // 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; - } + if (calibrate_cancel_check(worker_data->mavlink_log_pub)) { + return calibrate_return_cancelled; } /* abort with timeout */ @@ -281,21 +270,12 @@ 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) { - // 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; - } + if (calibrate_cancel_check(worker_data->mavlink_log_pub)) { + result = calibrate_return_cancelled; + break; } // Yield CPU — updatedBlocking() returns immediately when data is diff --git a/src/modules/commander/worker_thread.cpp b/src/modules/commander/worker_thread.cpp index 23a36a1a5af..70dc2ab15a1 100644 --- a/src/modules/commander/worker_thread.cpp +++ b/src/modules/commander/worker_thread.cpp @@ -74,6 +74,21 @@ void WorkerThread::startTask(Request request) _request = request; + // Open before the thread starts, so a cancel Commander handles before the low priority thread runs reaches it + switch (_request) { + case Request::GyroCalibration: + case Request::MagCalibration: + case Request::AccelCalibration: + case Request::LevelCalibration: + case Request::AirspeedCalibration: + case Request::BaroCalibration: + calibrate_cancel_open(); + break; + + default: + break; + } + /* initialize low priority thread */ pthread_attr_t low_prio_attr; pthread_attr_init(&low_prio_attr); @@ -94,6 +109,7 @@ void WorkerThread::startTask(Request request) } else { PX4_ERR("Failed to start thread (%i)", ret); + calibrate_cancel_close(); _state.store((int)State::Finished); _ret_value = ret; } @@ -222,6 +238,8 @@ void WorkerThread::threadEntry() } } + calibrate_cancel_close(); + _state.store((int)State::Finished); // set this last to signal the main thread we're done } diff --git a/src/modules/simulation/simulator_sih/sih.cpp b/src/modules/simulation/simulator_sih/sih.cpp index 11b00218da0..e007fea4f28 100644 --- a/src/modules/simulation/simulator_sih/sih.cpp +++ b/src/modules/simulation/simulator_sih/sih.cpp @@ -56,6 +56,29 @@ using namespace time_literals; ModuleBase::Descriptor Sih::desc{task_spawn, custom_command, print_usage}; +#if defined(__PX4_POSIX) +// Poses for the orient command, named after the face pointing down as the calibration names them +static constexpr struct { + const char *name; + float roll; + float pitch; +} orient_sides[] = { + {"down", 0.f, 0.f}, + {"up", M_PI_F, 0.f}, + {"front", 0.f, -M_PI_F / 2.f}, + {"back", 0.f, M_PI_F / 2.f}, + {"left", -M_PI_F / 2.f, 0.f}, + {"right", M_PI_F / 2.f, 0.f}, +}; + +static constexpr float ORIENT_RATE_MAX = M_PI_F; // [rad/s] turn rate between poses +static constexpr float ORIENT_YAW_RATE = 1.5f; // [rad/s] rotation about the vertical +static constexpr float ORIENT_WOBBLE = 0.4f; // [rad] tilt swing while rotating, so the accel never reads still +static constexpr float ORIENT_WOBBLE_FREQ = 0.5f; // [Hz] +static constexpr hrt_abstime ORIENT_ALL_HOLD = 10_s; // still on each side for the accel calibration, +static constexpr hrt_abstime ORIENT_ALL_ROTATE = 10_s; // then rotating for the mag calibration +#endif // __PX4_POSIX + Sih::Sih() : ModuleParams(nullptr) { @@ -280,9 +303,21 @@ void Sih::sensor_step() read_motors(dt); - generate_force_and_torques(dt); +#if defined(__PX4_POSIX) + orient_update(now); + if (_orient_mode == OrientMode::Off) { + generate_force_and_torques(dt); + equations_of_motion(dt); + + } else { + orient_step(now, dt); + } + +#else + generate_force_and_torques(dt); equations_of_motion(dt); +#endif // __PX4_POSIX reconstruct_sensors_signals(now); @@ -784,6 +819,120 @@ void Sih::equations_of_motion(const float dt) _lpos(2) = -(_lla.altitude() - _lpos_ref_alt); } +#if defined(__PX4_POSIX) +void Sih::orient_update(const hrt_abstime &now) +{ + static_assert(sizeof(orient_sides) / sizeof(orient_sides[0]) == ORIENT_SIDE_COUNT, "orient side table"); + + int request = _orient_request.load(); + + if (request == ORIENT_NONE || !_orient_request.compare_exchange(&request, ORIENT_NONE)) { + return; + } + + if (request == ORIENT_OFF) { + if (_orient_mode != OrientMode::Off) { + _orient_mode = OrientMode::Leveling; + } + + return; + } + + if (_orient_mode == OrientMode::Off) { + if (!_grounded) { + PX4_WARN("orient: vehicle not on the ground"); + return; + } + + _orient_yaw = Eulerf(_q).psi(); + } + + _orient_start = now; + + if (request == ORIENT_ALL) { + _orient_mode = OrientMode::AllSides; + _orient_side = -1; + + } else { + _orient_mode = (request >= ORIENT_ROTATE) ? OrientMode::Rotate : OrientMode::Hold; + _orient_side = request % ORIENT_ROTATE; + } +} + +void Sih::orient_step(const hrt_abstime &now, const float dt) +{ + bool rotate = (_orient_mode == OrientMode::Rotate); + hrt_abstime rotate_start = _orient_start; + + if (_orient_mode == OrientMode::AllSides) { + static constexpr hrt_abstime side_time = ORIENT_ALL_HOLD + ORIENT_ALL_ROTATE; + const hrt_abstime elapsed = now - _orient_start; + const int side = elapsed / side_time; + + if (side >= ORIENT_SIDE_COUNT) { + PX4_INFO("orient: all sides done"); + _orient_mode = OrientMode::Leveling; + + } else { + if (side != _orient_side) { + PX4_INFO("orient: %s side", orient_sides[side].name); + _orient_side = side; + } + + rotate = (elapsed % side_time) >= ORIENT_ALL_HOLD; + rotate_start = _orient_start + side * side_time + ORIENT_ALL_HOLD; + } + } + + const Quatf q_yaw{AxisAnglef(Vector3f(0.f, 0.f, 1.f), _orient_yaw)}; + Quatf q_target = q_yaw; + + if (_orient_mode != OrientMode::Leveling) { + const Quatf q_side{Eulerf(orient_sides[_orient_side].roll, orient_sides[_orient_side].pitch, 0.f)}; + + if (rotate) { + const float t = (now - rotate_start) * 1e-6f; + const float tilt = ORIENT_WOBBLE * sinf(2.f * M_PI_F * ORIENT_WOBBLE_FREQ * t); + const Quatf q_spin = Quatf(AxisAnglef(Vector3f(0.f, 0.f, 1.f), ORIENT_YAW_RATE * t)); + const Quatf q_wobble = Quatf(AxisAnglef(Vector3f(1.f, 0.f, 0.f), tilt)); + q_target = q_yaw * q_spin * q_wobble * q_side; + + } else { + q_target = q_yaw * q_side; + } + } + + // turn towards the target at a bounded rate, with the gyro reading the turn + Quatf q_error = _q.inversed() * q_target; + q_error.canonicalize(); + Vector3f rotation = AxisAnglef(q_error); + const float max_angle = ORIENT_RATE_MAX * dt; + + if (rotation.norm() > max_angle) { + rotation = rotation.normalized() * max_angle; + } + + if (_orient_mode == OrientMode::Leveling && rotation.norm() < 1e-4f) { + _orient_mode = OrientMode::Off; + _w_B.zero(); + return; + } + + _w_B = (dt > FLT_EPSILON) ? rotation / dt : Vector3f{}; + + // at rest in place + _specific_force_E = -Vector3f(_R_N2E.col(2)) * LatLonAlt::Wgs84::gravity(_lla.latitude_rad()); + _v_E.zero(); + _v_E_dot.zero(); + _v_N_dot.zero(); + + _q_E = Quatf(_R_N2E) * _q * Quatf(AxisAnglef(rotation)); + _q_E.normalize(); + + ecefToNed(); +} +#endif // __PX4_POSIX + void Sih::ecefToNed() { _lla = LatLonAlt::fromEcef(_p_E); @@ -1165,6 +1314,16 @@ int Sih::print_status() } PX4_INFO("vehicle landed: %d", _grounded); + +#if defined(__PX4_POSIX) + + if (_orient_mode != OrientMode::Off) { + PX4_INFO("orient: %s", (_orient_mode == OrientMode::Leveling || _orient_side < 0) ? "moving" : + orient_sides[_orient_side].name); + } + +#endif // __PX4_POSIX + PX4_INFO("local position NED (m)"); _lpos.print(); PX4_INFO("local velocity NED (m/s)"); @@ -1224,6 +1383,48 @@ Sih *Sih::instantiate(int argc, char *argv[]) int Sih::custom_command(int argc, char *argv[]) { +#if defined(__PX4_POSIX) + Sih *sih = get_instance(desc); + + if (!is_running(desc) || !sih) { + return print_usage("not running"); + } + + if (argc >= 2 && !strcmp(argv[0], "orient")) { + int request = ORIENT_NONE; + + if (!strcmp(argv[1], "off")) { + request = ORIENT_OFF; + + } else if (!strcmp(argv[1], "all")) { + request = ORIENT_ALL; + + } else { + for (int side = 0; side < ORIENT_SIDE_COUNT; side++) { + if (!strcmp(argv[1], orient_sides[side].name)) { + request = side; + } + } + + if (request != ORIENT_NONE && argc >= 3) { + if (strcmp(argv[2], "rotate") != 0) { + return print_usage("unknown orient option"); + } + + request += ORIENT_ROTATE; + } + } + + if (request == ORIENT_NONE) { + return print_usage("unknown side"); + } + + sih->_orient_request.store(request); + return 0; + } + +#endif // __PX4_POSIX + return print_usage("unknown command"); } @@ -1251,11 +1452,26 @@ Quaternion representation is used for the attitude. Forward Euler is used for integration. Most of the variables are declared global in the .hpp file to avoid stack overflow. +### Examples +Pose the vehicle on the ground for accel and mag calibration (SITL only), sides named as the calibration names them. +With `rotate` it turns about the vertical with a tilt swing, as the mag calibration asks for: +$ simulator_sih orient left rotate + +Each side in turn: 10 s still, then 10 s rotating, then level. This completes either calibration. +$ simulator_sih orient all + +Level the vehicle and resume the flight dynamics: +$ simulator_sih orient off )DESCR_STR"); PRINT_MODULE_USAGE_NAME("simulator_sih", "simulation"); PRINT_MODULE_USAGE_COMMAND("start"); +#if defined(__PX4_POSIX) + PRINT_MODULE_USAGE_COMMAND_DESCR("orient", "Pose the vehicle on the ground for calibration"); + PRINT_MODULE_USAGE_ARG("down|up|front|back|left|right|all|off", "Side facing down, every side in turn, or level", false); + PRINT_MODULE_USAGE_ARG("rotate", "Rotate about the vertical", true); +#endif // __PX4_POSIX PRINT_MODULE_USAGE_DEFAULT_COMMANDS(); return 0; diff --git a/src/modules/simulation/simulator_sih/sih.hpp b/src/modules/simulation/simulator_sih/sih.hpp index e33cbc67737..ca7a2e87a18 100644 --- a/src/modules/simulation/simulator_sih/sih.hpp +++ b/src/modules/simulation/simulator_sih/sih.hpp @@ -56,6 +56,7 @@ #pragma once #include +#include #include #include #include @@ -231,6 +232,27 @@ private: void ecefToNed(); +#if defined(__PX4_POSIX) + // orient command: poses the vehicle on the ground for accel and mag calibration. + // SITL only: Commander denies sensor calibration when SIH runs on flight controller hardware. + void orient_update(const hrt_abstime &now); + void orient_step(const hrt_abstime &now, const float dt); + + static constexpr int ORIENT_SIDE_COUNT = 6; + static constexpr int ORIENT_NONE = -1; // no request + static constexpr int ORIENT_OFF = ORIENT_SIDE_COUNT; + static constexpr int ORIENT_ALL = ORIENT_SIDE_COUNT + 1; + static constexpr int ORIENT_ROTATE = 8; // added to a side + + enum class OrientMode {Off, Hold, Rotate, AllSides, Leveling}; + + px4::atomic_int _orient_request{ORIENT_NONE}; // set by the CLI, taken by the simulation loop + OrientMode _orient_mode{OrientMode::Off}; + int _orient_side{0}; + hrt_abstime _orient_start{0}; + float _orient_yaw{0.f}; // heading kept while posing +#endif // __PX4_POSIX + #if defined(ENABLE_LOCKSTEP_SCHEDULER) void lockstep_loop(); int _lockstep_component{-1}; diff --git a/test/mavsdk_tests/autopilot_tester_calibration.py b/test/mavsdk_tests/autopilot_tester_calibration.py new file mode 100644 index 00000000000..aea9c565e3d --- /dev/null +++ b/test/mavsdk_tests/autopilot_tester_calibration.py @@ -0,0 +1,121 @@ +"""AutopilotTester for sensor calibration on SIH. + +`simulator_sih orient`, sent over the MAVLink shell, poses the vehicle the way a person would +during accelerometer and magnetometer calibration. The tester follows the calibration like a +GCS, through the `[cal]` status texts and the command acks. +""" + +import json +import threading +from typing import Any, List, Optional + +from mavsdk.plugins.shell import Device, Shell # type: ignore + +from autopilot_tester import MAV_COMP_ID_AUTOPILOT1, MAV_RESULT_ACCEPTED, AutopilotTester, log + +MAV_CMD_PREFLIGHT_CALIBRATION = 241 +MAV_TYPE_GCS = 6 +MAV_AUTOPILOT_INVALID = 8 + +# PX4 only sends STATUSTEXT while it hears a GCS. MAVSDK's 1 Hz heartbeat is in host time, which at +# a high speed factor is longer than PX4's GCS timeout in simulation time. +HEARTBEAT_INTERVAL_S = 0.05 + +START_ACK_TIMEOUT_S = 5.0 + + +class AutopilotTesterCalibration(AutopilotTester): + + def __init__(self, speed_factor: Optional[float] = None) -> None: + super().__init__(speed_factor) + self._shell: Any = None + self._lock = threading.Lock() + self._acks: List[int] = [] + self._texts: List[str] = [] + self._heartbeat_thread = threading.Thread(target=self._send_heartbeats, daemon=True) + + def connect(self, url: str) -> None: + super().connect(url) + self._heartbeat_thread.start() + self._shell = Shell(self._system) + # Without a subscriber MAVSDK does not ask for a response, and PX4 closes the shell + # before it has run the command. + self._shell.subscribe_receive(self._on_shell_output) + self._mavlink_direct.subscribe_message("COMMAND_ACK", self._on_ack) + self._telemetry.subscribe_status_text(self._on_status_text) + + def close(self) -> List[str]: + self._should_exit.set() + if self._heartbeat_thread.is_alive(): + self._heartbeat_thread.join() + return super().close() + + def _send_heartbeats(self) -> None: + while not self._should_exit.wait(HEARTBEAT_INTERVAL_S): + self.send_mavlink_message("HEARTBEAT", { + "type": MAV_TYPE_GCS, "autopilot": MAV_AUTOPILOT_INVALID, "base_mode": 0, + "custom_mode": 0, "system_status": 0, "mavlink_version": 3}) + + def _on_shell_output(self, receive: Any, _user_data: Any) -> None: + for line in receive.data.splitlines(): + if line.strip(): + log("Shell: {}".format(line.strip())) + + def _on_ack(self, message: Any, _user_data: Any) -> None: + ack = json.loads(message.fields_json) + if ack["command"] == MAV_CMD_PREFLIGHT_CALIBRATION: + with self._lock: + self._acks.append(int(ack["result"])) + + def _on_status_text(self, status_text: Any, _user_data: Any) -> None: + text = str(status_text.text) + if text.startswith("[cal]"): + log(text) + with self._lock: + self._texts.append(text) + + def shell(self, command: str) -> None: + log("Shell: {}".format(command)) + self._shell.send(command + "\n", Device.SHELL) + + def start_calibration(self, param1: float = 0.0, param2: float = 0.0, param3: float = 0.0, + param4: float = 0.0, param5: float = 0.0, param6: float = 0.0, + param7: float = 0.0) -> None: + """Send one start, without retries: PX4 rejects a retry once the calibration runs, and + that late ack would be counted against the cancel.""" + self._send_calibration_command(param1, param2, param3, param4, param5, param6, param7) + assert self.poll_condition_with_timeout(lambda: bool(self.calibration_acks()), + START_ACK_TIMEOUT_S), \ + "No ack for the calibration start" + assert self.calibration_acks() == [MAV_RESULT_ACCEPTED] + + def cancel_calibration(self) -> None: + """Send one cancel, without retries, so its acks can be counted.""" + self._send_calibration_command() + + def _send_calibration_command(self, param1: float = 0.0, param2: float = 0.0, + param3: float = 0.0, param4: float = 0.0, param5: float = 0.0, + param6: float = 0.0, param7: float = 0.0) -> None: + """Forget earlier acks and send the command once.""" + with self._lock: + self._acks.clear() + + self.send_mavlink_message("COMMAND_LONG", { + "target_system": self._target_system_id(), + "target_component": MAV_COMP_ID_AUTOPILOT1, + "command": MAV_CMD_PREFLIGHT_CALIBRATION, "confirmation": 0, + "param1": param1, "param2": param2, "param3": param3, "param4": param4, + "param5": param5, "param6": param6, "param7": param7, + }) + + def calibration_acks(self) -> List[int]: + with self._lock: + return list(self._acks) + + def has_text(self, text: str) -> bool: + with self._lock: + return any(text in t for t in self._texts) + + def wait_for_text(self, text: str, timeout_s: float) -> None: + assert self.poll_condition_with_timeout(lambda: self.has_text(text), timeout_s), \ + "No '{}' within {} s".format(text, timeout_s) diff --git a/test/mavsdk_tests/configs/sih-sitl.json b/test/mavsdk_tests/configs/sih-sitl.json index 3658095898c..4b6ed4ddf9a 100644 --- a/test/mavsdk_tests/configs/sih-sitl.json +++ b/test/mavsdk_tests/configs/sih-sitl.json @@ -31,6 +31,11 @@ "PX4_PARAM_ATT_EN": 1 } }, + { + "model": "quadx", + "test_filter": "calibration", + "timeout_min": 10 + }, { "model": "quadx", "test_filter": "sih_home_yaw", diff --git a/test/mavsdk_tests/conftest.py b/test/mavsdk_tests/conftest.py index ec7646a29cc..68b82f1e290 100644 --- a/test/mavsdk_tests/conftest.py +++ b/test/mavsdk_tests/conftest.py @@ -3,12 +3,13 @@ from typing import Iterator, Optional, Type, TypeVar import pytest # Show the values in failed asserts inside the testers, not just in tests. -pytest.register_assert_rewrite("autopilot_tester", "autopilot_tester_flow", - "autopilot_tester_follow_me", +pytest.register_assert_rewrite("autopilot_tester", "autopilot_tester_calibration", + "autopilot_tester_flow", "autopilot_tester_follow_me", "autopilot_tester_loiter", "autopilot_tester_rtl", "autopilot_tester_vision_target") from autopilot_tester import AutopilotTester # noqa: E402 +from autopilot_tester_calibration import AutopilotTesterCalibration # noqa: E402 from autopilot_tester_flow import AutopilotTesterFlow # noqa: E402 from autopilot_tester_follow_me import AutopilotTesterFollowMe # noqa: E402 from autopilot_tester_loiter import AutopilotTesterLoiter # noqa: E402 @@ -47,6 +48,12 @@ def tester(request: pytest.FixtureRequest) -> Iterator[AutopilotTester]: yield from connected_tester(request, AutopilotTester) +@pytest.fixture +def tester_calibration(request: pytest.FixtureRequest) -> Iterator[AutopilotTesterCalibration]: + """A tester posing the SIH vehicle for sensor calibration, connected to the vehicle.""" + yield from connected_tester(request, AutopilotTesterCalibration) + + @pytest.fixture def tester_rtl(request: pytest.FixtureRequest) -> Iterator[AutopilotTesterRtl]: """A tester with rally point and geofence support, connected to the vehicle.""" diff --git a/test/mavsdk_tests/pytest.ini b/test/mavsdk_tests/pytest.ini index 53b6f32d9c4..4f142767576 100644 --- a/test/mavsdk_tests/pytest.ini +++ b/test/mavsdk_tests/pytest.ini @@ -4,6 +4,7 @@ addopts = --strict-markers -p no:cacheprovider norecursedirs = integration_test_runner catch2 configs markers = + calibration: sensor calibration tests, posing the SIH vehicle multicopter: multicopter tests vtol: VTOL tests offboard: offboard position/velocity control diff --git a/test/mavsdk_tests/test_calibration.py b/test/mavsdk_tests/test_calibration.py new file mode 100644 index 00000000000..7f069c6a2bd --- /dev/null +++ b/test/mavsdk_tests/test_calibration.py @@ -0,0 +1,62 @@ +from typing import Dict, Optional + +import pytest + +from autopilot_tester_calibration import AutopilotTesterCalibration + +MAV_RESULT_ACCEPTED = 0 + +# Before a cancel was handed to the running calibration, these took from tens of seconds to never. +CANCEL_TIMEOUT_S = 2.0 + + +@pytest.mark.calibration +def test_mag_calibration(tester_calibration: AutopilotTesterCalibration) -> None: + tester = tester_calibration + tester.wait_until_ready() + tester.start_calibration(param2=1) + tester.shell("simulator_sih orient all") + tester.wait_for_text("[cal] calibration done: mag", 150) + + +@pytest.mark.calibration +def test_accel_calibration(tester_calibration: AutopilotTesterCalibration) -> None: + tester = tester_calibration + tester.wait_until_ready() + tester.start_calibration(param5=1) + tester.shell("simulator_sih orient all") + tester.wait_for_text("[cal] calibration done: accel", 150) + + +@pytest.mark.calibration +@pytest.mark.parametrize("params, orient, cancel_after_text, cancel_after_s", [ + pytest.param({"param5": 1}, "down rotate", None, 3.0, id="accel_never_still"), + pytest.param({"param2": 1}, "all", "[cal] down side done", 2.0, id="mag_between_sides"), + pytest.param({"param2": 1}, "all", "[cal] down side calibration: progress", 0.0, + id="mag_collecting"), + pytest.param({"param5": 2}, "down rotate", None, 2.0, id="level_moving"), + pytest.param({"param1": 1}, "down rotate", None, 2.0, id="gyro_moving"), + pytest.param({"param3": 1}, None, None, 0.0, id="baro"), +]) +def test_calibration_cancel(tester_calibration: AutopilotTesterCalibration, + params: Dict[str, float], orient: Optional[str], + cancel_after_text: Optional[str], cancel_after_s: float) -> None: + tester = tester_calibration + tester.wait_until_ready() + + if orient: + tester.shell("simulator_sih orient " + orient) + + tester.start_calibration(**params) + + if cancel_after_text: + tester.wait_for_text(cancel_after_text, 60) + + tester.sleep_for(cancel_after_s) + tester.cancel_calibration() + tester.wait_for_text("[cal] calibration cancelled", CANCEL_TIMEOUT_S) + + # Late acks would arrive in this time. + tester.sleep_for(2.0) + assert tester.calibration_acks() == [MAV_RESULT_ACCEPTED] + assert not tester.has_text("[cal] calibration failed")