Files
PX4-Autopilot/src/modules/commander/worker_thread.cpp
T
Jacob Dahl e192036345 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
2026-11-11 14:38:01 -07:00

252 lines
7.2 KiB
C++

/****************************************************************************
*
* Copyright (c) 2020 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name PX4 nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
#include "worker_thread.hpp"
#include "accelerometer_calibration.h"
#if defined(CONFIG_SENSORS_VEHICLE_AIRSPEED)
#include "airspeed_calibration.h"
#endif
#include "calibration_routines.h"
#include "baro_calibration.h"
#include "esc_calibration.h"
#include "gyro_calibration.h"
#include "level_calibration.h"
#if defined(CONFIG_SENSORS_VEHICLE_MAGNETOMETER)
#include "mag_calibration.h"
#endif
#include "rc_calibration.h"
#include <px4_platform_common/events.h>
#include <px4_platform_common/log.h>
#include <px4_platform_common/shutdown.h>
#include <parameters/param.h>
using namespace time_literals;
WorkerThread::~WorkerThread()
{
if (_state.load() == (int)State::Running) {
/* wait for thread to complete */
int ret = pthread_join(_thread_handle, nullptr);
if (ret) {
PX4_ERR("join failed: %d", ret);
}
}
}
void WorkerThread::startTask(Request request)
{
if (isBusy()) {
return;
}
_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);
pthread_attr_setstacksize(&low_prio_attr, PX4_STACK_ADJUSTED(4804));
struct sched_param param;
pthread_attr_getschedparam(&low_prio_attr, &param);
/* low priority */
param.sched_priority = SCHED_PRIORITY_DEFAULT - 50;
pthread_attr_setschedparam(&low_prio_attr, &param);
int ret = pthread_create(&_thread_handle, &low_prio_attr, &threadEntryTrampoline, this);
pthread_attr_destroy(&low_prio_attr);
if (ret == 0) {
_state.store((int)State::Running);
} else {
PX4_ERR("Failed to start thread (%i)", ret);
calibrate_cancel_close();
_state.store((int)State::Finished);
_ret_value = ret;
}
}
void *WorkerThread::threadEntryTrampoline(void *arg)
{
WorkerThread *worker_thread = (WorkerThread *)arg;
worker_thread->threadEntry();
return nullptr;
}
void WorkerThread::threadEntry()
{
px4_prctl(PR_SET_NAME, "commander_low_prio", px4_getpid());
switch (_request) {
case Request::GyroCalibration:
_ret_value = do_gyro_calibration(&_mavlink_log_pub);
break;
#if defined(CONFIG_SENSORS_VEHICLE_MAGNETOMETER)
case Request::MagCalibration:
_ret_value = do_mag_calibration(&_mavlink_log_pub);
break;
case Request::MagCalibrationQuick:
_ret_value = do_mag_calibration_quick(&_mavlink_log_pub, _heading_radians, _latitude, _longitude);
break;
#else
case Request::MagCalibration:
case Request::MagCalibrationQuick:
_ret_value = -1;
break;
#endif
case Request::RCTrimCalibration:
_ret_value = do_trim_calibration(&_mavlink_log_pub);
break;
case Request::AccelCalibration:
_ret_value = do_accel_calibration(&_mavlink_log_pub);
break;
case Request::LevelCalibration:
_ret_value = do_level_calibration(&_mavlink_log_pub);
break;
case Request::AccelCalibrationQuick:
_ret_value = do_accel_calibration_quick(&_mavlink_log_pub);
break;
#if defined(CONFIG_SENSORS_VEHICLE_AIRSPEED)
case Request::AirspeedCalibration:
_ret_value = do_airspeed_calibration(&_mavlink_log_pub);
break;
#else
case Request::AirspeedCalibration:
_ret_value = -1;
break;
#endif
case Request::ESCCalibration:
_ret_value = do_esc_calibration(&_mavlink_log_pub);
break;
case Request::BaroCalibration:
_ret_value = do_baro_calibration(&_mavlink_log_pub);
break;
case Request::ParamLoadDefault:
_ret_value = param_load_default();
// blank storage (1) is not an error: all parameters were reset to defaults
if (_ret_value == 1) {
mavlink_log_warning(&_mavlink_log_pub, "Blank storage, parameters reset to default\t");
events::send(events::ID("commander_load_param_blank"), events::Log::Warning, "Blank storage, parameters reset to default");
_ret_value = 0;
}
if (_ret_value != 0) {
mavlink_log_critical(&_mavlink_log_pub, "Error loading settings\t");
events::send(events::ID("commander_load_param_failed"), events::Log::Critical, "Error loading settings");
}
break;
case Request::ParamSaveDefault:
_ret_value = param_save_default(true);
if (_ret_value != 0) {
mavlink_log_critical(&_mavlink_log_pub, "Error saving settings\t");
events::send(events::ID("commander_save_param_failed"), events::Log::Critical, "Error saving settings");
}
break;
case Request::ParamResetAll:
param_reset_all();
_ret_value = param_save_default(true);
break;
case Request::ParamResetSensorFactory: {
const char *reset_cal[] = { "CAL_ACC*", "CAL_GYRO*", "CAL_MAG*" };
param_reset_specific(reset_cal, sizeof(reset_cal) / sizeof(reset_cal[0]));
_ret_value = param_save_default(true);
#if defined(CONFIG_BOARDCTL_RESET)
px4_reboot_request(REBOOT_REQUEST, 400_ms);
#endif // CONFIG_BOARDCTL_RESET
break;
}
case Request::ParamResetAllConfig: {
const char *exclude_list[] = {
"LND_FLIGHT_T_HI",
"LND_FLIGHT_T_LO",
"COM_FLIGHT_UUID"
};
param_reset_excludes(exclude_list, sizeof(exclude_list) / sizeof(exclude_list[0]));
_ret_value = param_save_default(true);
break;
}
}
calibrate_cancel_close();
_state.store((int)State::Finished); // set this last to signal the main thread we're done
}
void WorkerThread::setMagQuickData(float heading_rad, float lat, float lon)
{
_heading_radians = heading_rad;
_latitude = lat;
_longitude = lon;
}