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")