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
This commit is contained in:
Jacob Dahl authored and GitHub committed 2026-11-11 14:38:01 -07:00
1 parent ff8f44e8e1
commit e192036345
18 files changed
+605 -92

No files matched your search

+14
View File
@@ -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 <side>` | Still, with `<side>` facing down: `down`, `up`, `front`, `back`, `left`, `right`. |
| `simulator_sih orient <side> rotate` | Rotating about the vertical with `<side>` 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.
+9 -1
View File
@@ -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);
@@ -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;
@@ -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;
}
@@ -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;
+78 -41
View File
@@ -38,8 +38,8 @@
* @author Lorenz Meier <lm@inf.ethz.ch>
*/
#include <px4_platform_common/atomic.h>
#include <px4_platform_common/defines.h>
#include <px4_platform_common/events.h>
#include <px4_platform_common/posix.h>
#include <px4_platform_common/time.h>
@@ -54,7 +54,6 @@
#include <uORB/Publication.hpp>
#include <uORB/SubscriptionBlocking.hpp>
#include <uORB/topics/vehicle_acceleration.h>
#include <uORB/topics/vehicle_command.h>
#include <uORB/topics/vehicle_command_ack.h>
#include "calibration_routines.h"
@@ -90,6 +89,10 @@ enum detect_orientation_return detect_orientation(orb_advert_t *mavlink_log_pub,
uORB::SubscriptionBlocking<vehicle_acceleration_s> 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<vehicle_command_ack_s> 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<uint32_t>(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<vehicle_command_ack_s> 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;
}
+12 -1
View File
@@ -38,6 +38,7 @@
#include <drivers/drv_hrt.h>
#include <uORB/Publication.hpp>
#include <uORB/topics/vehicle_command.h>
// 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
+4 -16
View File
@@ -59,8 +59,6 @@
#include <uORB/SubscriptionBlocking.hpp>
#include <uORB/topics/sensor_gyro.h>
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;
@@ -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)) {
+5 -25
View File
@@ -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<sensor_gyro_s> 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
+18
View File
@@ -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
}
+217 -1
View File
@@ -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<Sih>(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;
@@ -56,6 +56,7 @@
#pragma once
#include <mathlib/math/WhiteGaussianNoise.hpp>
#include <px4_platform_common/atomic.h>
#include <px4_platform_common/module.h>
#include <px4_platform_common/module_params.h>
#include <px4_platform_common/posix.h>
@@ -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};
@@ -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)
+5
View File
@@ -31,6 +31,11 @@
"PX4_PARAM_ATT_EN": 1
}
},
{
"model": "quadx",
"test_filter": "calibration",
"timeout_min": 10
},
{
"model": "quadx",
"test_filter": "sih_home_yaw",
+9 -2
View File
@@ -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."""
+1
View File
@@ -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
+62
View File
@@ -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")