fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-07-16 08:20:34 +08:00
Code Issues Packages Projects Releases Wiki Activity

commander: remove low priority thread, run tasks in a worker thread on demand

Browse Source 
- removes race conditions
- removes dependencies on static data & methods
- reduces RAM usage by ~3.8KB
This commit is contained in:
Beat Küng
2020-11-27 08:44:38 +01:00
committed by Daniel Agar
parent 419b336a15
commit 05a3492174
7 changed files with 499 additions and 359 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/commander/CMakeLists.txt
+1
View File
@@ -51,6 +51,7 @@ px4_add_module(
mag_calibration.cpp
rc_calibration.cpp
state_machine_helper.cpp
worker_thread.cpp
DEPENDS
circuit_breaker
failure_detector
src/modules/commander/Commander.cpp
+200 -334
View File
@@ -48,16 +48,9 @@
#include "Arming/PreFlightCheck/PreFlightCheck.hpp"
#include "Arming/ArmAuthorization/ArmAuthorization.h"
#include "Arming/HealthFlags/HealthFlags.h"
#include "accelerometer_calibration.h"
#include "airspeed_calibration.h"
#include "calibration_routines.h"
#include "commander_helper.h"
#include "esc_calibration.h"
#include "gyro_calibration.h"
#include "level_calibration.h"
#include "mag_calibration.h"
#include "px4_custom_mode.h"
#include "rc_calibration.h"
#include "state_machine_helper.h"
/* PX4 headers */
@@ -98,18 +91,12 @@ typedef enum VEHICLE_MODE_FLAG {
/* Mavlink log uORB handle */
static orb_advert_t mavlink_log_pub = nullptr;
static volatile bool thread_should_exit = false; /**< daemon exit flag */
static struct vehicle_status_s status = {};
static struct actuator_armed_s armed = {};
static struct vehicle_status_flags_s status_flags = {};
/**
* Loop that runs at a lower rate and priority for calibration and parameter tasks.
*/
void *commander_low_prio_loop(void *arg);
static void answer_command(const vehicle_command_s &cmd, unsigned result,
uORB::Publication<vehicle_command_ack_s> &command_ack_pub);
@@ -1041,15 +1028,196 @@ Commander::handle_command(vehicle_status_s *status_local, const vehicle_command_
break;
case vehicle_command_s::VEHICLE_CMD_PREFLIGHT_CALIBRATION: {
if ((status.arming_state == vehicle_status_s::ARMING_STATE_ARMED)
|| status.arming_state == vehicle_status_s::ARMING_STATE_SHUTDOWN || _worker_thread.isBusy()) {
// reject if armed or shutting down
answer_command(cmd, vehicle_command_s::VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED, command_ack_pub);
} else {
/* try to go to INIT/PREFLIGHT arming state */
if (TRANSITION_DENIED == arming_state_transition(&status, safety_s{}, vehicle_status_s::ARMING_STATE_INIT, &armed,
false /* fRunPreArmChecks */, &mavlink_log_pub, &status_flags,
PreFlightCheck::arm_requirements_t{}, // arming requirements not relevant for switching to ARMING_STATE_INIT
30_s, // time since boot not relevant for switching to ARMING_STATE_INIT
(cmd.from_external ? arm_disarm_reason_t::COMMAND_EXTERNAL : arm_disarm_reason_t::COMMAND_INTERNAL))
) {
answer_command(cmd, vehicle_command_s::VEHICLE_CMD_RESULT_DENIED, command_ack_pub);
break;
}
if ((int)(cmd.param1) == 1) {
/* gyro calibration */
answer_command(cmd, vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED, command_ack_pub);
status_flags.condition_calibration_enabled = true;
_worker_thread.startTask(WorkerThread::Request::GyroCalibration);
} else if ((int)(cmd.param1) == vehicle_command_s::PREFLIGHT_CALIBRATION_TEMPERATURE_CALIBRATION ||
(int)(cmd.param5) == vehicle_command_s::PREFLIGHT_CALIBRATION_TEMPERATURE_CALIBRATION ||
(int)(cmd.param7) == vehicle_command_s::PREFLIGHT_CALIBRATION_TEMPERATURE_CALIBRATION) {
/* temperature calibration: handled in events module */
break;
} else if ((int)(cmd.param2) == 1) {
/* magnetometer calibration */
answer_command(cmd, vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED, command_ack_pub);
status_flags.condition_calibration_enabled = true;
_worker_thread.startTask(WorkerThread::Request::MagCalibration);
} else if ((int)(cmd.param3) == 1) {
/* zero-altitude pressure calibration */
answer_command(cmd, vehicle_command_s::VEHICLE_CMD_RESULT_DENIED, command_ack_pub);
} else if ((int)(cmd.param4) == 1) {
/* RC calibration */
answer_command(cmd, vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED, command_ack_pub);
/* disable RC control input completely */
status_flags.rc_input_blocked = true;
mavlink_log_info(&mavlink_log_pub, "Calibration: Disabling RC input");
} else if ((int)(cmd.param4) == 2) {
/* RC trim calibration */
answer_command(cmd, vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED, command_ack_pub);
status_flags.condition_calibration_enabled = true;
_worker_thread.startTask(WorkerThread::Request::RCTrimCalibration);
} else if ((int)(cmd.param5) == 1) {
/* accelerometer calibration */
answer_command(cmd, vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED, command_ack_pub);
status_flags.condition_calibration_enabled = true;
_worker_thread.startTask(WorkerThread::Request::AccelCalibration);
} else if ((int)(cmd.param5) == 2) {
// board offset calibration
answer_command(cmd, vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED, command_ack_pub);
status_flags.condition_calibration_enabled = true;
_worker_thread.startTask(WorkerThread::Request::LevelCalibration);
} else if ((int)(cmd.param5) == 4) {
// accelerometer quick calibration
answer_command(cmd, vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED, command_ack_pub);
status_flags.condition_calibration_enabled = true;
_worker_thread.startTask(WorkerThread::Request::AccelCalibrationQuick);
} else if ((int)(cmd.param6) == 1 || (int)(cmd.param6) == 2) {
// TODO: param6 == 1 is deprecated, but we still accept it for a while (feb 2017)
/* airspeed calibration */
answer_command(cmd, vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED, command_ack_pub);
status_flags.condition_calibration_enabled = true;
_worker_thread.startTask(WorkerThread::Request::AirspeedCalibration);
} else if ((int)(cmd.param7) == 1) {
/* do esc calibration */
if (check_battery_disconnected(&mavlink_log_pub)) {
answer_command(cmd, vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED, command_ack_pub);
status_flags.condition_calibration_enabled = true;
armed.in_esc_calibration_mode = true;
_worker_thread.startTask(WorkerThread::Request::ESCCalibration);
} else {
answer_command(cmd, vehicle_command_s::VEHICLE_CMD_RESULT_DENIED, command_ack_pub);
}
} else if ((int)(cmd.param4) == 0) {
/* RC calibration ended - have we been in one worth confirming? */
if (status_flags.rc_input_blocked) {
/* enable RC control input */
status_flags.rc_input_blocked = false;
mavlink_log_info(&mavlink_log_pub, "Calibration: Restoring RC input");
}
answer_command(cmd, vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED, command_ack_pub);
} else {
answer_command(cmd, vehicle_command_s::VEHICLE_CMD_RESULT_UNSUPPORTED, command_ack_pub);
}
}
break;
}
case vehicle_command_s::VEHICLE_CMD_FIXED_MAG_CAL_YAW: {
// Magnetometer quick calibration using world magnetic model and known heading
if ((status.arming_state == vehicle_status_s::ARMING_STATE_ARMED)
|| (status.arming_state == vehicle_status_s::ARMING_STATE_SHUTDOWN)
|| _worker_thread.isBusy()) {
// reject if armed or shutting down
answer_command(cmd, vehicle_command_s::VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED, command_ack_pub);
} else {
answer_command(cmd, vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED, command_ack_pub);
// parameter 1: Yaw in degrees
// parameter 3: Latitude
// parameter 4: Longitude
// assume vehicle pointing north (0 degrees) if heading isn't specified
const float heading_radians = PX4_ISFINITE(cmd.param1) ? math::radians(roundf(cmd.param1)) : 0.f;
float latitude = NAN;
float longitude = NAN;
if (PX4_ISFINITE(cmd.param3) && PX4_ISFINITE(cmd.param4)) {
// invalid if both lat & lon are 0 (current mavlink spec)
if ((fabsf(cmd.param3) > 0) && (fabsf(cmd.param4) > 0)) {
latitude = cmd.param3;
longitude = cmd.param4;
}
}
status_flags.condition_calibration_enabled = true;
_worker_thread.setMagQuickData(heading_radians, latitude, longitude);
_worker_thread.startTask(WorkerThread::Request::MagCalibrationQuick);
}
break;
}
case vehicle_command_s::VEHICLE_CMD_PREFLIGHT_STORAGE: {
if ((status.arming_state == vehicle_status_s::ARMING_STATE_ARMED)
|| status.arming_state == vehicle_status_s::ARMING_STATE_SHUTDOWN
|| _worker_thread.isBusy()) {
// reject if armed or shutting down
answer_command(cmd, vehicle_command_s::VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED, command_ack_pub);
} else {
if (((int)(cmd.param1)) == 0) {
answer_command(cmd, vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED, command_ack_pub);
_worker_thread.startTask(WorkerThread::Request::ParamLoadDefault);
} else if (((int)(cmd.param1)) == 1) {
answer_command(cmd, vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED, command_ack_pub);
_worker_thread.startTask(WorkerThread::Request::ParamSaveDefault);
} else if (((int)(cmd.param1)) == 2) {
answer_command(cmd, vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED, command_ack_pub);
_worker_thread.startTask(WorkerThread::Request::ParamResetAll);
}
}
break;
}
case vehicle_command_s::VEHICLE_CMD_START_RX_PAIR:
/* just ack, implementation handled in the IO driver */
answer_command(cmd, vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED, command_ack_pub);
break;
case vehicle_command_s::VEHICLE_CMD_CUSTOM_0:
case vehicle_command_s::VEHICLE_CMD_CUSTOM_1:
case vehicle_command_s::VEHICLE_CMD_CUSTOM_2:
case vehicle_command_s::VEHICLE_CMD_DO_MOUNT_CONTROL:
case vehicle_command_s::VEHICLE_CMD_DO_MOUNT_CONFIGURE:
case vehicle_command_s::VEHICLE_CMD_DO_MOUNT_CONTROL_QUAT:
case vehicle_command_s::VEHICLE_CMD_PREFLIGHT_CALIBRATION:
case vehicle_command_s::VEHICLE_CMD_PREFLIGHT_SET_SENSOR_OFFSETS:
case vehicle_command_s::VEHICLE_CMD_PREFLIGHT_STORAGE:
case vehicle_command_s::VEHICLE_CMD_PREFLIGHT_UAVCAN:
case vehicle_command_s::VEHICLE_CMD_PAYLOAD_PREPARE_DEPLOY:
case vehicle_command_s::VEHICLE_CMD_PAYLOAD_CONTROL_DEPLOY:
@@ -1063,7 +1231,6 @@ Commander::handle_command(vehicle_status_s *status_local, const vehicle_command_
case vehicle_command_s::VEHICLE_CMD_DO_CHANGE_SPEED:
case vehicle_command_s::VEHICLE_CMD_DO_LAND_START:
case vehicle_command_s::VEHICLE_CMD_DO_GO_AROUND:
case vehicle_command_s::VEHICLE_CMD_START_RX_PAIR:
case vehicle_command_s::VEHICLE_CMD_LOGGING_START:
case vehicle_command_s::VEHICLE_CMD_LOGGING_STOP:
case vehicle_command_s::VEHICLE_CMD_NAV_DELAY:
@@ -1072,7 +1239,6 @@ Commander::handle_command(vehicle_status_s *status_local, const vehicle_command_
case vehicle_command_s::VEHICLE_CMD_DO_SET_ROI_LOCATION:
case vehicle_command_s::VEHICLE_CMD_DO_SET_ROI_WPNEXT_OFFSET:
case vehicle_command_s::VEHICLE_CMD_DO_SET_ROI_NONE:
case vehicle_command_s::VEHICLE_CMD_FIXED_MAG_CAL_YAW:
case vehicle_command_s::VEHICLE_CMD_INJECT_FAILURE:
/* ignore commands that are handled by other parts of the system */
break;
@@ -1290,25 +1456,6 @@ Commander::run()
int32_t airmode = 0;
int32_t rc_map_arm_switch = 0;
/* initialize low priority thread */
pthread_attr_t commander_low_prio_attr;
pthread_attr_init(&commander_low_prio_attr);
pthread_attr_setstacksize(&commander_low_prio_attr, PX4_STACK_ADJUSTED(3304));
#ifndef __PX4_QURT
// This is not supported by QURT (yet).
struct sched_param param;
pthread_attr_getschedparam(&commander_low_prio_attr, &param);
/* low priority */
param.sched_priority = SCHED_PRIORITY_DEFAULT - 50;
pthread_attr_setschedparam(&commander_low_prio_attr, &param);
#endif
pthread_t commander_low_prio_thread;
pthread_create(&commander_low_prio_thread, &commander_low_prio_attr, commander_low_prio_loop, nullptr);
pthread_attr_destroy(&commander_low_prio_attr);
status.system_id = _param_mav_sys_id.get();
arm_auth_init(&mavlink_log_pub, &status.system_id);
@@ -2435,6 +2582,23 @@ Commander::run()
control_status_leds(&status, &armed, _status_changed, _battery_warning);
}
// check if the worker has finished
if (_worker_thread.hasResult()) {
int ret = _worker_thread.getResultAndReset();
armed.in_esc_calibration_mode = false;
if (status_flags.condition_calibration_enabled) { // did we do a calibration?
status_flags.condition_calibration_enabled = false;
if (ret == 0) {
tune_positive(true);
} else {
tune_negative(true);
}
}
}
_status_changed = false;
/* store last position lock state */
@@ -2449,15 +2613,6 @@ Commander::run()
px4_usleep(COMMANDER_MONITORING_INTERVAL);
}
thread_should_exit = true;
/* wait for threads to complete */
int ret = pthread_join(commander_low_prio_thread, nullptr);
if (ret) {
warn("join failed: %d", ret);
}
rgbled_set_color_and_mode(led_control_s::COLOR_WHITE, led_control_s::MODE_OFF);
/* close fds */
@@ -3381,295 +3536,6 @@ void answer_command(const vehicle_command_s &cmd, unsigned result,
command_ack_pub.publish(command_ack);
}
void *commander_low_prio_loop(void *arg)
{
/* Set thread name */
px4_prctl(PR_SET_NAME, "commander_low_prio", px4_getpid());
/* Subscribe to command topic */
int cmd_sub = orb_subscribe(ORB_ID(vehicle_command));
/* command ack */
uORB::Publication<vehicle_command_ack_s> command_ack_pub{ORB_ID(vehicle_command_ack)};
/* wakeup source(s) */
px4_pollfd_struct_t fds[1];
fds[0].fd = cmd_sub;
fds[0].events = POLLIN;
while (!thread_should_exit) {
/* wait for up to 1000ms for data */
int pret = px4_poll(&fds[0], (sizeof(fds) / sizeof(fds[0])), 1000);
if (pret < 0) {
/* this is undesirable but not much we can do - might want to flag unhappy status */
warn("commander: poll error %d, %d", pret, errno);
continue;
} else if (pret != 0) {
struct vehicle_command_s cmd {};
/* if we reach here, we have a valid command */
orb_copy(ORB_ID(vehicle_command), cmd_sub, &cmd);
/* only handle low-priority commands here */
switch (cmd.command) {
case vehicle_command_s::VEHICLE_CMD_PREFLIGHT_CALIBRATION: {
if ((status.arming_state == vehicle_status_s::ARMING_STATE_ARMED)
|| status.arming_state == vehicle_status_s::ARMING_STATE_SHUTDOWN) {
// reject if armed or shutting down
answer_command(cmd, vehicle_command_s::VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED, command_ack_pub);
} else {
int calib_ret = PX4_ERROR;
/* try to go to INIT/PREFLIGHT arming state */
if (TRANSITION_DENIED == arming_state_transition(&status, safety_s{}, vehicle_status_s::ARMING_STATE_INIT, &armed,
false /* fRunPreArmChecks */, &mavlink_log_pub, &status_flags,
PreFlightCheck::arm_requirements_t{}, // arming requirements not relevant for switching to ARMING_STATE_INIT
30_s, // time since boot not relevant for switching to ARMING_STATE_INIT
(cmd.from_external ? arm_disarm_reason_t::COMMAND_EXTERNAL : arm_disarm_reason_t::COMMAND_INTERNAL))
) {
answer_command(cmd, vehicle_command_s::VEHICLE_CMD_RESULT_DENIED, command_ack_pub);
break;
} else {
status_flags.condition_calibration_enabled = true;
}
if ((int)(cmd.param1) == 1) {
/* gyro calibration */
answer_command(cmd, vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED, command_ack_pub);
calib_ret = do_gyro_calibration(&mavlink_log_pub);
} else if ((int)(cmd.param1) == vehicle_command_s::PREFLIGHT_CALIBRATION_TEMPERATURE_CALIBRATION ||
(int)(cmd.param5) == vehicle_command_s::PREFLIGHT_CALIBRATION_TEMPERATURE_CALIBRATION ||
(int)(cmd.param7) == vehicle_command_s::PREFLIGHT_CALIBRATION_TEMPERATURE_CALIBRATION) {
/* temperature calibration: handled in events module */
break;
} else if ((int)(cmd.param2) == 1) {
/* magnetometer calibration */
answer_command(cmd, vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED, command_ack_pub);
calib_ret = do_mag_calibration(&mavlink_log_pub);
} else if ((int)(cmd.param3) == 1) {
/* zero-altitude pressure calibration */
answer_command(cmd, vehicle_command_s::VEHICLE_CMD_RESULT_DENIED, command_ack_pub);
} else if ((int)(cmd.param4) == 1) {
/* RC calibration */
answer_command(cmd, vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED, command_ack_pub);
/* disable RC control input completely */
status_flags.rc_input_blocked = true;
calib_ret = OK;
mavlink_log_info(&mavlink_log_pub, "Calibration: Disabling RC input");
} else if ((int)(cmd.param4) == 2) {
/* RC trim calibration */
answer_command(cmd, vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED, command_ack_pub);
calib_ret = do_trim_calibration(&mavlink_log_pub);
} else if ((int)(cmd.param5) == 1) {
/* accelerometer calibration */
answer_command(cmd, vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED, command_ack_pub);
calib_ret = do_accel_calibration(&mavlink_log_pub);
} else if ((int)(cmd.param5) == 2) {
// board offset calibration
answer_command(cmd, vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED, command_ack_pub);
calib_ret = do_level_calibration(&mavlink_log_pub);
} else if ((int)(cmd.param5) == 4) {
// accelerometer quick calibration
answer_command(cmd, vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED, command_ack_pub);
calib_ret = do_accel_calibration_quick(&mavlink_log_pub);
} else if ((int)(cmd.param6) == 1 || (int)(cmd.param6) == 2) {
// TODO: param6 == 1 is deprecated, but we still accept it for a while (feb 2017)
/* airspeed calibration */
answer_command(cmd, vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED, command_ack_pub);
calib_ret = do_airspeed_calibration(&mavlink_log_pub);
} else if ((int)(cmd.param7) == 1) {
/* do esc calibration */
answer_command(cmd, vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED, command_ack_pub);
calib_ret = do_esc_calibration(&mavlink_log_pub, &armed);
} else if ((int)(cmd.param4) == 0) {
/* RC calibration ended - have we been in one worth confirming? */
if (status_flags.rc_input_blocked) {
/* enable RC control input */
status_flags.rc_input_blocked = false;
mavlink_log_info(&mavlink_log_pub, "Calibration: Restoring RC input");
}
answer_command(cmd, vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED, command_ack_pub);
/* this always succeeds */
calib_ret = OK;
} else {
answer_command(cmd, vehicle_command_s::VEHICLE_CMD_RESULT_UNSUPPORTED, command_ack_pub);
}
status_flags.condition_calibration_enabled = false;
if (calib_ret == OK) {
tune_positive(true);
// time since boot not relevant here
if (PreFlightCheck::preflightCheck(&mavlink_log_pub, status, status_flags, false, false, true, 30_s)) {
status_flags.condition_system_sensors_initialized = true;
}
arming_state_transition(&status, safety_s{}, vehicle_status_s::ARMING_STATE_STANDBY, &armed,
false /* fRunPreArmChecks */,
&mavlink_log_pub, &status_flags,
PreFlightCheck::arm_requirements_t{}, // arming requirements not relevant for switching to ARMING_STATE_STANDBY
30_s, // time since boot not relevant for switching to ARMING_STATE_STANDBY
(cmd.from_external ? arm_disarm_reason_t::COMMAND_EXTERNAL : arm_disarm_reason_t::COMMAND_INTERNAL));
} else {
tune_negative(true);
}
}
break;
}
case vehicle_command_s::VEHICLE_CMD_FIXED_MAG_CAL_YAW: {
// Magnetometer quick calibration using world magnetic model and known heading
if ((status.arming_state == vehicle_status_s::ARMING_STATE_ARMED)
|| (status.arming_state == vehicle_status_s::ARMING_STATE_SHUTDOWN)
|| status_flags.condition_calibration_enabled) {
// reject if armed or shutting down
answer_command(cmd, vehicle_command_s::VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED, command_ack_pub);
} else {
// parameter 1: Yaw in degrees
// parameter 3: Latitude
// parameter 4: Longitude
// assume vehicle pointing north (0 degrees) if heading isn't specified
const float heading_radians = PX4_ISFINITE(cmd.param1) ? math::radians(roundf(cmd.param1)) : 0.f;
float latitude = NAN;
float longitude = NAN;
if (PX4_ISFINITE(cmd.param3) && PX4_ISFINITE(cmd.param4)) {
// invalid if both lat & lon are 0 (current mavlink spec)
if ((fabsf(cmd.param3) > 0) && (fabsf(cmd.param4) > 0)) {
latitude = cmd.param3;
longitude = cmd.param4;
}
}
if (do_mag_calibration_quick(&mavlink_log_pub, heading_radians, latitude, longitude) == PX4_OK) {
answer_command(cmd, vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED, command_ack_pub);
tune_positive(true);
} else {
answer_command(cmd, vehicle_command_s::VEHICLE_CMD_RESULT_FAILED, command_ack_pub);
tune_negative(true);
}
}
break;
}
case vehicle_command_s::VEHICLE_CMD_PREFLIGHT_STORAGE: {
if ((status.arming_state == vehicle_status_s::ARMING_STATE_ARMED)
|| status.arming_state == vehicle_status_s::ARMING_STATE_SHUTDOWN) {
// reject if armed or shutting down
answer_command(cmd, vehicle_command_s::VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED, command_ack_pub);
} else {
if (((int)(cmd.param1)) == 0) {
int ret = param_load_default();
if (ret == OK) {
mavlink_log_info(&mavlink_log_pub, "Settings loaded");
answer_command(cmd, vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED, command_ack_pub);
} else {
mavlink_log_critical(&mavlink_log_pub, "Error loading settings");
/* convenience as many parts of NuttX use negative errno */
if (ret < 0) {
ret = -ret;
}
if (ret < 1000) {
mavlink_log_critical(&mavlink_log_pub, "Error: %s", strerror(ret));
}
answer_command(cmd, vehicle_command_s::VEHICLE_CMD_RESULT_FAILED, command_ack_pub);
}
} else if (((int)(cmd.param1)) == 1) {
int ret = param_save_default();
if (ret == OK) {
/* do not spam MAVLink, but provide the answer / green led mechanism */
answer_command(cmd, vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED, command_ack_pub);
} else {
mavlink_log_critical(&mavlink_log_pub, "Error saving settings");
/* convenience as many parts of NuttX use negative errno */
if (ret < 0) {
ret = -ret;
}
if (ret < 1000) {
mavlink_log_critical(&mavlink_log_pub, "Error: %s", strerror(ret));
}
answer_command(cmd, vehicle_command_s::VEHICLE_CMD_RESULT_FAILED, command_ack_pub);
}
} else if (((int)(cmd.param1)) == 2) {
/* reset parameters and save empty file */
param_reset_all();
/* do not spam MAVLink, but provide the answer / green led mechanism */
mavlink_log_critical(&mavlink_log_pub, "Onboard parameters reset");
answer_command(cmd, vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED, command_ack_pub);
}
}
break;
}
case vehicle_command_s::VEHICLE_CMD_START_RX_PAIR:
/* just ack, implementation handled in the IO driver */
answer_command(cmd, vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED, command_ack_pub);
break;
default:
/* don't answer on unsupported commands, it will be done in main loop */
break;
}
}
}
px4_close(cmd_sub);
return nullptr;
}
int Commander::task_spawn(int argc, char *argv[])
{
_task_id = px4_task_spawn_cmd("commander",
src/modules/commander/Commander.hpp
+3
View File
@@ -36,6 +36,7 @@
#include "Arming/PreFlightCheck/PreFlightCheck.hpp"
#include "failure_detector/FailureDetector.hpp"
#include "state_machine_helper.h"
#include "worker_thread.hpp"
#include <lib/controllib/blocks.hpp>
#include <lib/hysteresis/hysteresis.h>
@@ -382,6 +383,8 @@ private:
safety_s _safety{};
vtol_vehicle_status_s _vtol_status{};
WorkerThread _worker_thread;
// Subscriptions
uORB::Subscription _actuator_controls_sub{ORB_ID_VEHICLE_ATTITUDE_CONTROLS};
uORB::Subscription _cmd_sub {ORB_ID(vehicle_command)};
src/modules/commander/esc_calibration.cpp
+18 -24
View File
@@ -54,43 +54,39 @@
using namespace time_literals;
int do_esc_calibration(orb_advert_t *mavlink_log_pub, struct actuator_armed_s *armed)
bool check_battery_disconnected(orb_advert_t *mavlink_log_pub)
{
int return_code = PX4_OK;
int fd = -1;
bool batt_connected = true; // for safety reasons assume battery is connected, will be cleared below if not the case
hrt_abstime timeout_start = 0;
calibration_log_info(mavlink_log_pub, CAL_QGC_STARTED_MSG, "esc");
uORB::SubscriptionData<battery_status_s> batt_sub{ORB_ID(battery_status)};
const battery_status_s &battery = batt_sub.get();
batt_sub.update();
if (battery.timestamp == 0) {
calibration_log_critical(mavlink_log_pub, CAL_QGC_FAILED_MSG, "battery unavailable");
return_code = PX4_ERROR;
goto Out;
return false;
}
// Make sure battery is disconnected
// battery is not connected if the connected flag is not set and we have a recent battery measurement
batt_sub.update();
if (!battery.connected && (hrt_elapsed_time(&battery.timestamp) < 500_ms)) {
batt_connected = false;
return true;
}
if (batt_connected) {
calibration_log_critical(mavlink_log_pub, CAL_QGC_FAILED_MSG, "Disconnect battery and try again");
return_code = PX4_ERROR;
goto Out;
}
calibration_log_critical(mavlink_log_pub, CAL_QGC_FAILED_MSG, "Disconnect battery and try again");
return false;
}
armed->in_esc_calibration_mode = true;
int do_esc_calibration(orb_advert_t *mavlink_log_pub)
{
int return_code = PX4_OK;
hrt_abstime timeout_start = 0;
calibration_log_info(mavlink_log_pub, CAL_QGC_STARTED_MSG, "esc");
fd = px4_open(PWM_OUTPUT0_DEVICE_PATH, 0);
uORB::SubscriptionData<battery_status_s> batt_sub{ORB_ID(battery_status)};
const battery_status_s &battery = batt_sub.get();
batt_sub.update();
bool batt_connected = battery.connected;
int fd = px4_open(PWM_OUTPUT0_DEVICE_PATH, 0);
if (fd < 0) {
calibration_log_critical(mavlink_log_pub, CAL_QGC_FAILED_MSG, "Can't open PWM device");
@@ -152,7 +148,7 @@ int do_esc_calibration(orb_advert_t *mavlink_log_pub, struct actuator_armed_s *a
}
}
px4_usleep(50_ms);
px4_usleep(50000);
}
Out:
@@ -173,8 +169,6 @@ Out:
px4_close(fd);
}
armed->in_esc_calibration_mode = false;
if (return_code == PX4_OK) {
calibration_log_info(mavlink_log_pub, CAL_QGC_DONE_MSG, "esc");
}
src/modules/commander/esc_calibration.h
+3 -1
View File
@@ -44,6 +44,8 @@
#include <uORB/topics/actuator_armed.h>
int do_esc_calibration(orb_advert_t *mavlink_log_pub, struct actuator_armed_s *armed);
int do_esc_calibration(orb_advert_t *mavlink_log_pub);
bool check_battery_disconnected(orb_advert_t *mavlink_log_pub);
#endif
src/modules/commander/worker_thread.cpp
+174
View File
@@ -0,0 +1,174 @@
/****************************************************************************
*
* 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"
#include "airspeed_calibration.h"
#include "calibration_routines.h"
#include "esc_calibration.h"
#include "gyro_calibration.h"
#include "level_calibration.h"
#include "mag_calibration.h"
#include "rc_calibration.h"
#include <px4_platform_common/log.h>
#include <parameters/param.h>
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;
/* 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(3304));
#ifndef __PX4_QURT
// This is not supported by QURT (yet).
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);
#endif
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);
_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;
case Request::MagCalibration:
_ret_value = do_mag_calibration(&_mavlink_log_pub);
break;
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;
case Request::AirspeedCalibration:
_ret_value = do_airspeed_calibration(&_mavlink_log_pub);
break;
case Request::ESCCalibration:
_ret_value = do_esc_calibration(&_mavlink_log_pub);
break;
case Request::MagCalibrationQuick:
_ret_value = do_mag_calibration_quick(&_mavlink_log_pub, _heading_radians, _latitude, _longitude);
break;
case Request::ParamLoadDefault:
_ret_value = param_load_default();
if (_ret_value != 0) {
mavlink_log_critical(&_mavlink_log_pub, "Error loading settings");
}
break;
case Request::ParamSaveDefault:
_ret_value = param_save_default();
if (_ret_value != 0) {
mavlink_log_critical(&_mavlink_log_pub, "Error saving settings");
}
break;
case Request::ParamResetAll:
param_reset_all();
_ret_value = 0;
break;
}
_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;
}
src/modules/commander/worker_thread.hpp
+100
View File
@@ -0,0 +1,100 @@
/****************************************************************************
*
* 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.
*
****************************************************************************/
#pragma once
#include <px4_platform_common/atomic.h>
#include <px4_platform_common/posix.h>
#include <systemlib/mavlink_log.h>
#include <uORB/uORB.h>
/**
* @class WorkerThread
* low priority background thread, started on demand, used for:
* - calibration
* - param saving
*/
class WorkerThread
{
public:
enum class Request {
GyroCalibration,
MagCalibration,
RCTrimCalibration,
AccelCalibration,
LevelCalibration,
AccelCalibrationQuick,
AirspeedCalibration,
ESCCalibration,
MagCalibrationQuick,
ParamLoadDefault,
ParamSaveDefault,
ParamResetAll,
};
WorkerThread() = default;
~WorkerThread();
void setMagQuickData(float heading_rad, float lat, float lon);
void startTask(Request request);
bool isBusy() const { return _state.load() != (int)State::Idle; }
bool hasResult() const { return _state.load() == (int)State::Finished; }
int getResultAndReset() { _state.store((int)State::Idle); return _ret_value; }
private:
enum class State {
Idle,
Running,
Finished
};
static void *threadEntryTrampoline(void *arg);
void threadEntry();
px4::atomic_int _state{(int)State::Idle};
pthread_t _thread_handle{};
int _ret_value{};
Request _request;
orb_advert_t _mavlink_log_pub{nullptr};
// extra arguments
float _heading_radians;
float _latitude;
float _longitude;
};