mirror of
https://github.com/PX4/PX4-Autopilot.git
synced 2026-11-22 14:19:57 +08:00
* 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
252 lines
7.2 KiB
C++
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, ¶m);
|
|
|
|
/* low priority */
|
|
param.sched_priority = SCHED_PRIORITY_DEFAULT - 50;
|
|
pthread_attr_setschedparam(&low_prio_attr, ¶m);
|
|
|
|
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;
|
|
}
|