Files
PX4-Autopilot/src/modules/commander/Commander.cpp
T
ac97d5263a feat(commander)!: Replace CBRK_IO_SAFETY with COM_SAFETY_MODE for more options (#28744)
* feat(commander): only report safety button as available when it is available and configured to be used via COM_PREARM_MODE

* feat(commander): add a new option to COM_PREARM_MODE to explicitly and exclusively allow prearming via VEHICLE_CMD_DO_SET_SAFETY_SWITCH_STATE

* feat(commander): explicit dedicated safety-button-only and mavlink-only prearm modes

* fix(commander): also accept VEHICLE_CMD_DO_SET_SAFETY_SWITCH_STATE on Always and Disarmed pre-arm profiles to match documentation

* docs(prearm): update docs with new prearm mode behaviour

* fix(commander): fix syntax for bool

* docs(prearm): revert last param table, as COM_FORCE_SAFETY is already mentioned in a table above.

* docs(prearm): fix MAVLink spelling, grammar and parameter table

Make the pre-arm docs consistent with the COM_PREARM_MODE parameter and
the docs style guide: MAVLink is spelled consistently (page and option
labels), selectable option names are shown in code style, the parameter
table lists all five COM_PREARM_MODE options with the correct default,
and sentences are fixed and put one per line.

Question for the author: the COM_PREARM_MODE option labels say "Safety
button", while the docs say "safety switch". Are these synonyms? If so,
which term should we use in both places? If not, what is the difference?

Assisted-by: Claude:claude-opus-5-5
Signed-off-by: Hamish Willee <hamishwillee@gmail.com>

* fix(commander): clean up _button_disabled logic: button still needs to be enabled in Disabled and Always prearmed modes to switch safety off.

* docs(prearm): adjust COM_PREARM_MODE description and clarify/fix/improve prearm docs

* chore(VehicleCommand.msg): improved comments

* chore(msg): v0->v1 translation for VehicleCommand.msg

* Revert "chore(msg): v0->v1 translation for VehicleCommand.msg"

This reverts commit 50ea14fc76310ba7b53ebcd1c6ee8cf4e1c0776b.

* feat(commander): deny VEHICLE_CMD_DO_SET_SAFETY_SWITCH_STATE when not accepted

* feat(commander): COM_SAFETY_MODE replaces CBRK_IO_SAFETY

* docs(prearm): update docs with new COM_SAFETY_MODE behaviour

* docs(prearm): minor cleanup

* fix(commander): migrate CBRK_IO_SAFETY and correct safety ACKs

A stored CBRK_IO_SAFETY=0 was dropped on import, so upgraded vehicles booted with COM_SAFETY_MODE=0 and safety off. The CAN GPS nodes have no commander, so setting COM_SAFETY_MODE there only logged an error at boot. A SAFETY_SAFE command in mode 0 and ESC calibration with safety on were ACKed ACCEPTED while doing nothing.

Assisted-by: Claude:claude-opus-5-5

---------

Signed-off-by: Hamish Willee <hamishwillee@gmail.com>
Co-authored-by: Hamish Willee <hamishwillee@gmail.com>
Co-authored-by: Jacob Dahl <dahl.jakejacob@gmail.com>
2026-10-06 17:35:07 -06:00

3293 lines
116 KiB
C++

/****************************************************************************
*
* Copyright (c) 2013-2023 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.
*
****************************************************************************/
/**
* @file Commander.cpp
*
* Main state machine / business logic
*
*/
#include "Commander.hpp"
/* commander module headers */
#include "Arming/ArmAuthorization/ArmAuthorization.h"
#include "commander_helper.h"
#include "esc_calibration.h"
#define DEFINE_GET_PX4_CUSTOM_MODE
#include "px4_custom_mode.h"
#include "ModeUtil/control_mode.hpp"
#include "ModeUtil/conversions.hpp"
#include <lib/modes/ui.hpp>
#include <lib/modes/standard_modes.hpp>
/* PX4 headers */
#include <drivers/drv_hrt.h>
#include <drivers/drv_tone_alarm.h>
#include <lib/geo/geo.h>
#include <mathlib/mathlib.h>
#include <px4_platform_common/events.h>
#include <px4_platform_common/px4_config.h>
#include <px4_platform_common/defines.h>
#include <px4_platform_common/external_reset_lockout.h>
#include <px4_platform_common/posix.h>
#include <px4_platform_common/shutdown.h>
#include <px4_platform_common/tasks.h>
#include <px4_platform_common/time.h>
#include <systemlib/mavlink_log.h>
#include <math.h>
#include <float.h>
#include <cstring>
#include <matrix/math.hpp>
#include <uORB/topics/mavlink_log.h>
#include <uORB/topics/tune_control.h>
ModuleBase::Descriptor Commander::desc{task_spawn, custom_command, print_usage};
typedef enum VEHICLE_MODE_FLAG {
VEHICLE_MODE_FLAG_CUSTOM_MODE_ENABLED = 1, /* 0b00000001 Reserved for future use. | */
VEHICLE_MODE_FLAG_TEST_ENABLED = 2, /* 0b00000010 system has a test mode enabled. This flag is intended for temporary system tests and should not be used for stable implementations. | */
VEHICLE_MODE_FLAG_AUTO_ENABLED = 4, /* 0b00000100 autonomous mode enabled, system finds its own goal positions. Guided flag can be set or not, depends on the actual implementation. | */
VEHICLE_MODE_FLAG_GUIDED_ENABLED = 8, /* 0b00001000 guided mode enabled, system flies MISSIONs / mission items. | */
VEHICLE_MODE_FLAG_STABILIZE_ENABLED = 16, /* 0b00010000 system stabilizes electronically its attitude (and optionally position). It needs however further control inputs to move around. | */
VEHICLE_MODE_FLAG_HIL_ENABLED = 32, /* 0b00100000 hardware in the loop simulation. All motors / actuators are blocked, but internal software is full operational. | */
VEHICLE_MODE_FLAG_MANUAL_INPUT_ENABLED = 64, /* 0b01000000 remote control input is enabled. | */
VEHICLE_MODE_FLAG_SAFETY_ARMED = 128, /* 0b10000000 MAV safety set to armed. Motors are enabled / running / can start. Ready to fly. Additional note: this flag is to be ignore when sent in the command MAV_CMD_DO_SET_MODE and MAV_CMD_COMPONENT_ARM_DISARM shall be used instead. The flag can still be used to report the armed state. | */
VEHICLE_MODE_FLAG_ENUM_END = 129, /* | */
} VEHICLE_MODE_FLAG;
// TODO: generate
static constexpr bool operator ==(const actuator_armed_s &a, const actuator_armed_s &b)
{
return (a.armed == b.armed &&
a.prearmed == b.prearmed &&
a.ready_to_arm == b.ready_to_arm &&
a.lockdown == b.lockdown &&
a.kill == b.kill &&
a.termination == b.termination &&
a.in_esc_calibration_mode == b.in_esc_calibration_mode);
}
static_assert(sizeof(actuator_armed_s) == 16, "actuator_armed equality operator review");
#if defined(BOARD_HAS_POWER_CONTROL)
static orb_advert_t tune_control_pub = nullptr;
static void play_power_button_down_tune()
{
// Override any other tunes because power-off sound should have the priority
set_tune_override(tune_control_s::TUNE_ID_POWER_OFF);
}
static void stop_tune()
{
tune_control_s tune_control{};
tune_control.tune_override = true;
tune_control.timestamp = hrt_absolute_time();
orb_publish(ORB_ID(tune_control), tune_control_pub, &tune_control);
}
static orb_advert_t power_button_state_pub = nullptr;
static int power_button_state_notification_cb(board_power_button_state_notification_e request)
{
// Note: this can be called from IRQ handlers, so we publish a message that will be handled
// on the main thread of commander.
power_button_state_s button_state{};
button_state.timestamp = hrt_absolute_time();
const int ret = PWR_BUTTON_RESPONSE_SHUT_DOWN_PENDING;
switch (request) {
case PWR_BUTTON_IDEL:
button_state.event = power_button_state_s::PWR_BUTTON_STATE_IDEL;
break;
case PWR_BUTTON_DOWN:
button_state.event = power_button_state_s::PWR_BUTTON_STATE_DOWN;
play_power_button_down_tune();
break;
case PWR_BUTTON_UP:
button_state.event = power_button_state_s::PWR_BUTTON_STATE_UP;
stop_tune();
break;
case PWR_BUTTON_REQUEST_SHUT_DOWN:
button_state.event = power_button_state_s::PWR_BUTTON_STATE_REQUEST_SHUTDOWN;
break;
default:
PX4_ERR("unhandled power button state: %i", (int)request);
return ret;
}
if (power_button_state_pub != nullptr) {
orb_publish(ORB_ID(power_button_state), power_button_state_pub, &button_state);
} else {
PX4_ERR("power_button_state_pub not properly initialized");
}
return ret;
}
#endif // BOARD_HAS_POWER_CONTROL
#ifndef CONSTRAINED_FLASH
static bool send_vehicle_command(const uint32_t cmd, const float param1 = NAN, const float param2 = NAN,
const float param3 = NAN, const float param4 = NAN, const double param5 = static_cast<double>(NAN),
const double param6 = static_cast<double>(NAN), const float param7 = NAN)
{
vehicle_command_s vcmd{};
vcmd.command = cmd;
vcmd.param1 = param1;
vcmd.param2 = param2;
vcmd.param3 = param3;
vcmd.param4 = param4;
vcmd.param5 = param5;
vcmd.param6 = param6;
vcmd.param7 = param7;
uORB::SubscriptionData<vehicle_status_s> vehicle_status_sub{ORB_ID(vehicle_status)};
vcmd.source_system = vehicle_status_sub.get().system_id;
vcmd.target_system = vehicle_status_sub.get().system_id;
vcmd.source_component = vehicle_status_sub.get().component_id;
vcmd.target_component = vehicle_status_sub.get().component_id;
uORB::Publication<vehicle_command_s> vcmd_pub{ORB_ID(vehicle_command)};
vcmd.timestamp = hrt_absolute_time();
return vcmd_pub.publish(vcmd);
}
static bool wait_for_vehicle_command_reply(const uint32_t cmd,
uORB::SubscriptionData<vehicle_command_ack_s> &vehicle_command_ack_sub)
{
hrt_abstime start = hrt_absolute_time();
while (hrt_absolute_time() - start < 100_ms) {
if (vehicle_command_ack_sub.update()) {
if (vehicle_command_ack_sub.get().command == cmd) {
return vehicle_command_ack_sub.get().result == vehicle_command_ack_s::VEHICLE_CMD_RESULT_ACCEPTED;
}
}
px4_usleep(10000);
}
return false;
}
static bool broadcast_vehicle_command(const uint32_t cmd, const float param1 = NAN, const float param2 = NAN,
const float param3 = NAN, const float param4 = NAN, const double param5 = static_cast<double>(NAN),
const double param6 = static_cast<double>(NAN), const float param7 = NAN)
{
vehicle_command_s vcmd{};
vcmd.command = cmd;
vcmd.param1 = param1;
vcmd.param2 = param2;
vcmd.param3 = param3;
vcmd.param4 = param4;
vcmd.param5 = param5;
vcmd.param6 = param6;
vcmd.param7 = param7;
uORB::SubscriptionData<vehicle_status_s> vehicle_status_sub{ORB_ID(vehicle_status)};
vcmd.source_system = vehicle_status_sub.get().system_id;
vcmd.target_system = 0;
vcmd.source_component = vehicle_status_sub.get().component_id;
vcmd.target_component = 0;
uORB::Publication<vehicle_command_s> vcmd_pub{ORB_ID(vehicle_command)};
vcmd.timestamp = hrt_absolute_time();
return vcmd_pub.publish(vcmd);
}
#endif
int Commander::custom_command(int argc, char *argv[])
{
if (!is_running(desc)) {
print_usage("not running");
return 1;
}
#ifndef CONSTRAINED_FLASH
if (!strcmp(argv[0], "calibrate")) {
if (argc > 1) {
if (!strcmp(argv[1], "gyro")) {
// gyro calibration: param1 = 1
send_vehicle_command(vehicle_command_s::VEHICLE_CMD_PREFLIGHT_CALIBRATION, 1.f, 0.f, 0.f, 0.f, 0.0, 0.0, 0.f);
} else if (!strcmp(argv[1], "mag")) {
if (argc > 2 && (strcmp(argv[2], "quick") == 0)) {
// Magnetometer quick calibration with optional heading
float heading_deg = 0.0f;
if (argc > 3) {
char *end;
heading_deg = strtof(argv[3], &end);
if (*end != '\0') { // Check if parsing failed
PX4_ERR("Invalid heading value: %s", argv[3]);
return 1;
}
heading_deg = matrix::wrap(roundf(heading_deg), 0.f, 360.f);
}
// Send command with heading (param1), other params zeroed (use GPS)
send_vehicle_command(vehicle_command_s::VEHICLE_CMD_FIXED_MAG_CAL_YAW,
heading_deg, // param1: Yaw of vehicle in earth frame (deg)
0.0f, // param2: CompassMask, 0 for all
0.0f, // param3: latitude (deg) If Latitude and longitude are both zero then use the current vehicle location
0.0f, // param4: longitude (deg)
0.0, // param5: unused
0.0, // param6: unused
0.0f); // param7: unused
} else {
// magnetometer calibration: param2 = 1
send_vehicle_command(vehicle_command_s::VEHICLE_CMD_PREFLIGHT_CALIBRATION, 0.f, 1.f, 0.f, 0.f, 0.0, 0.0, 0.f);
}
} else if (!strcmp(argv[1], "baro")) {
// baro calibration: param3 = 1
send_vehicle_command(vehicle_command_s::VEHICLE_CMD_PREFLIGHT_CALIBRATION, 0.f, 0.f, 1.f, 0.f, 0.0, 0.0, 0.f);
} else if (!strcmp(argv[1], "accel")) {
if (argc > 2 && (strcmp(argv[2], "quick") == 0)) {
// accelerometer quick calibration: param5 = 3
send_vehicle_command(vehicle_command_s::VEHICLE_CMD_PREFLIGHT_CALIBRATION, 0.f, 0.f, 0.f, 0.f, 4.0, 0.0, 0.f);
} else {
// accelerometer calibration: param5 = 1
send_vehicle_command(vehicle_command_s::VEHICLE_CMD_PREFLIGHT_CALIBRATION, 0.f, 0.f, 0.f, 0.f, 1.0, 0.0, 0.f);
}
} else if (!strcmp(argv[1], "level")) {
// board level calibration: param5 = 2
send_vehicle_command(vehicle_command_s::VEHICLE_CMD_PREFLIGHT_CALIBRATION, 0.f, 0.f, 0.f, 0.f, 2.0, 0.0, 0.f);
} else if (!strcmp(argv[1], "airspeed")) {
// airspeed calibration: param6 = 2
send_vehicle_command(vehicle_command_s::VEHICLE_CMD_PREFLIGHT_CALIBRATION, 0.f, 0.f, 0.f, 0.f, 0.0, 2.0, 0.f);
} else if (!strcmp(argv[1], "esc")) {
// ESC calibration: param7 = 1
send_vehicle_command(vehicle_command_s::VEHICLE_CMD_PREFLIGHT_CALIBRATION, 0.f, 0.f, 0.f, 0.f, 0.0, 0.0, 1.f);
} else {
PX4_ERR("argument %s unsupported.", argv[1]);
return 1;
}
return 0;
} else {
PX4_ERR("missing argument");
}
}
if (!strcmp(argv[0], "check")) {
send_vehicle_command(vehicle_command_s::VEHICLE_CMD_RUN_PREARM_CHECKS);
uORB::SubscriptionData<vehicle_status_s> vehicle_status_sub{ORB_ID(vehicle_status)};
PX4_INFO("Preflight check: %s", vehicle_status_sub.get().pre_flight_checks_pass ? "OK" : "FAILED");
return 0;
}
if (!strcmp(argv[0], "safety")) {
if (argc < 2) {
PX4_ERR("missing argument");
return 1;
}
if (!strcmp(argv[1], "on")) {
send_vehicle_command(vehicle_command_s::VEHICLE_CMD_DO_SET_SAFETY_SWITCH_STATE, vehicle_command_s::SAFETY_SAFE);
} else if (!strcmp(argv[1], "off")) {
send_vehicle_command(vehicle_command_s::VEHICLE_CMD_DO_SET_SAFETY_SWITCH_STATE, vehicle_command_s::SAFETY_DANGEROUS);
} else {
PX4_ERR("invlaid argument, use [on|off]");
return 1;
}
return 0;
}
if (!strcmp(argv[0], "actuator_group_test")) {
if (argc < 2) {
PX4_ERR("missing argument");
return 1;
}
uint8_t group;
float value = 1.f;
if (!strcmp(argv[1], "roll")) {
group = vehicle_command_s::ACTUATOR_TEST_GROUP_ROLL_TORQUE;
} else if (!strcmp(argv[1], "pitch")) {
group = vehicle_command_s::ACTUATOR_TEST_GROUP_PITCH_TORQUE;
} else if (!strcmp(argv[1], "yaw")) {
group = vehicle_command_s::ACTUATOR_TEST_GROUP_YAW_TORQUE;
} else if (!strcmp(argv[1], "tilt")) {
group = vehicle_command_s::ACTUATOR_TEST_GROUP_COLLECTIVE_TILT;
} else if (!strcmp(argv[1], "xthrust")) {
group = vehicle_command_s::ACTUATOR_TEST_GROUP_X_THRUST;
value = 0.1f; // For thrust use a lower default to avoid unintended takeoffs
} else if (!strcmp(argv[1], "ythrust")) {
group = vehicle_command_s::ACTUATOR_TEST_GROUP_Y_THRUST;
value = 0.1f;
} else if (!strcmp(argv[1], "zthrust")) {
group = vehicle_command_s::ACTUATOR_TEST_GROUP_Z_THRUST;
value = -0.1f;
} else {
PX4_ERR("argument %s unsupported.", argv[1]);
return 1;
}
if (argc > 2) {
char *end;
const float user_value = strtof(argv[2], &end);
if (*end == '\0' && PX4_ISFINITE(user_value)) {
value = user_value;
} else {
PX4_WARN("actuator_group_test: value \"%s\" is invalid. Using default %.1f", argv[2], (double) value);
}
}
send_vehicle_command(vehicle_command_s::VEHICLE_CMD_ACTUATOR_GROUP_TEST, group, value);
return 0;
}
if (!strcmp(argv[0], "arm")) {
float param2 = 0.f;
// 21196: force arming/disarming (e.g. allow arming to override preflight checks and disarming in flight)
if (argc > 1 && !strcmp(argv[1], "-f")) {
param2 = 21196.f;
}
send_vehicle_command(vehicle_command_s::VEHICLE_CMD_COMPONENT_ARM_DISARM,
static_cast<float>(vehicle_command_s::ARMING_ACTION_ARM),
param2);
return 0;
}
if (!strcmp(argv[0], "disarm")) {
float param2 = 0.f;
// 21196: force arming/disarming (e.g. allow arming to override preflight checks and disarming in flight)
if (argc > 1 && !strcmp(argv[1], "-f")) {
param2 = 21196.f;
}
send_vehicle_command(vehicle_command_s::VEHICLE_CMD_COMPONENT_ARM_DISARM,
static_cast<float>(vehicle_command_s::ARMING_ACTION_DISARM),
param2);
return 0;
}
if (!strcmp(argv[0], "takeoff")) {
// switch to takeoff mode and arm
uORB::SubscriptionData<vehicle_command_ack_s> vehicle_command_ack_sub{ORB_ID(vehicle_command_ack)};
send_vehicle_command(vehicle_command_s::VEHICLE_CMD_NAV_TAKEOFF);
if (wait_for_vehicle_command_reply(vehicle_command_s::VEHICLE_CMD_NAV_TAKEOFF, vehicle_command_ack_sub)) {
send_vehicle_command(vehicle_command_s::VEHICLE_CMD_COMPONENT_ARM_DISARM,
static_cast<float>(vehicle_command_s::ARMING_ACTION_ARM),
0.f);
}
return 0;
}
if (!strcmp(argv[0], "land")) {
send_vehicle_command(vehicle_command_s::VEHICLE_CMD_NAV_LAND);
return 0;
}
if (!strcmp(argv[0], "transition")) {
uORB::Subscription vehicle_status_sub{ORB_ID(vehicle_status)};
vehicle_status_s vehicle_status{};
vehicle_status_sub.copy(&vehicle_status);
send_vehicle_command(vehicle_command_s::VEHICLE_CMD_DO_VTOL_TRANSITION,
(float)(vehicle_status.vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING ?
vtol_vehicle_status_s::VEHICLE_VTOL_STATE_FW :
vtol_vehicle_status_s::VEHICLE_VTOL_STATE_MC), 0.0f);
return 0;
}
if (!strcmp(argv[0], "mode")) {
if (argc > 1) {
if (!strcmp(argv[1], "manual")) {
send_vehicle_command(vehicle_command_s::VEHICLE_CMD_DO_SET_MODE, 1, PX4_CUSTOM_MAIN_MODE_MANUAL);
} else if (!strcmp(argv[1], "altctl")) {
send_vehicle_command(vehicle_command_s::VEHICLE_CMD_DO_SET_MODE, 1, PX4_CUSTOM_MAIN_MODE_ALTCTL);
} else if (!strcmp(argv[1], "posctl")) {
send_vehicle_command(vehicle_command_s::VEHICLE_CMD_DO_SET_MODE, 1, PX4_CUSTOM_MAIN_MODE_POSCTL);
} else if (!strcmp(argv[1], "position:slow")) {
send_vehicle_command(vehicle_command_s::VEHICLE_CMD_DO_SET_MODE, 1, PX4_CUSTOM_MAIN_MODE_POSCTL,
PX4_CUSTOM_SUB_MODE_POSCTL_SLOW);
} else if (!strcmp(argv[1], "altitude_cruise")) {
send_vehicle_command(vehicle_command_s::VEHICLE_CMD_DO_SET_MODE, 1, PX4_CUSTOM_MAIN_MODE_ALTITUDE_CRUISE);
} else if (!strcmp(argv[1], "manual_parking")) {
send_vehicle_command(vehicle_command_s::VEHICLE_CMD_DO_SET_MODE, 1, PX4_CUSTOM_MAIN_MODE_MANUAL_PARKING);
} else if (!strcmp(argv[1], "auto:mission")) {
send_vehicle_command(vehicle_command_s::VEHICLE_CMD_DO_SET_MODE, 1, PX4_CUSTOM_MAIN_MODE_AUTO,
PX4_CUSTOM_SUB_MODE_AUTO_MISSION);
} else if (!strcmp(argv[1], "auto:loiter")) {
send_vehicle_command(vehicle_command_s::VEHICLE_CMD_DO_SET_MODE, 1, PX4_CUSTOM_MAIN_MODE_AUTO,
PX4_CUSTOM_SUB_MODE_AUTO_LOITER);
} else if (!strcmp(argv[1], "auto:course")) {
send_vehicle_command(vehicle_command_s::VEHICLE_CMD_DO_SET_MODE, 1, PX4_CUSTOM_MAIN_MODE_AUTO,
PX4_CUSTOM_SUB_MODE_GUIDED_COURSE);
} else if (!strcmp(argv[1], "auto:rtl")) {
send_vehicle_command(vehicle_command_s::VEHICLE_CMD_DO_SET_MODE, 1, PX4_CUSTOM_MAIN_MODE_AUTO,
PX4_CUSTOM_SUB_MODE_AUTO_RTL);
} else if (!strcmp(argv[1], "acro")) {
send_vehicle_command(vehicle_command_s::VEHICLE_CMD_DO_SET_MODE, 1, PX4_CUSTOM_MAIN_MODE_ACRO);
} else if (!strcmp(argv[1], "offboard")) {
send_vehicle_command(vehicle_command_s::VEHICLE_CMD_DO_SET_MODE, 1, PX4_CUSTOM_MAIN_MODE_OFFBOARD);
} else if (!strcmp(argv[1], "stabilized")) {
send_vehicle_command(vehicle_command_s::VEHICLE_CMD_DO_SET_MODE, 1, PX4_CUSTOM_MAIN_MODE_STABILIZED);
} else if (!strcmp(argv[1], "auto:takeoff")) {
send_vehicle_command(vehicle_command_s::VEHICLE_CMD_DO_SET_MODE, 1, PX4_CUSTOM_MAIN_MODE_AUTO,
PX4_CUSTOM_SUB_MODE_AUTO_TAKEOFF);
} else if (!strcmp(argv[1], "auto:land")) {
send_vehicle_command(vehicle_command_s::VEHICLE_CMD_DO_SET_MODE, 1, PX4_CUSTOM_MAIN_MODE_AUTO,
PX4_CUSTOM_SUB_MODE_AUTO_LAND);
} else if (!strcmp(argv[1], "auto:precland")) {
send_vehicle_command(vehicle_command_s::VEHICLE_CMD_DO_SET_MODE, 1, PX4_CUSTOM_MAIN_MODE_AUTO,
PX4_CUSTOM_SUB_MODE_AUTO_PRECLAND);
} else if (!strcmp(argv[1], "ext1")) {
send_vehicle_command(vehicle_command_s::VEHICLE_CMD_DO_SET_MODE, 1, PX4_CUSTOM_MAIN_MODE_AUTO,
PX4_CUSTOM_SUB_MODE_EXTERNAL1);
} else {
PX4_ERR("argument %s unsupported.", argv[1]);
}
return 0;
} else {
PX4_ERR("missing argument");
}
}
if (!strcmp(argv[0], "termination")) {
bool ret = send_vehicle_command(vehicle_command_s::VEHICLE_CMD_DO_FLIGHTTERMINATION, 1.0f, 0.0f);
return (ret ? 0 : 1);
}
if (!strcmp(argv[0], "pair")) {
// GCS pairing request handled by a companion
bool ret = broadcast_vehicle_command(vehicle_command_s::VEHICLE_CMD_START_RX_PAIR, 10.f);
return (ret ? 0 : 1);
}
if (!strcmp(argv[0], "set_ekf_origin")) {
if (argc > 3) {
double latitude = atof(argv[1]);
double longitude = atof(argv[2]);
float altitude = atof(argv[3]);
// Set the ekf NED origin global coordinates.
bool ret = send_vehicle_command(vehicle_command_s::VEHICLE_CMD_SET_GPS_GLOBAL_ORIGIN,
0.f, 0.f, 0.0, 0.0, latitude, longitude, altitude);
return (ret ? 0 : 1);
} else {
PX4_ERR("missing argument");
return 0;
}
}
if (!strcmp(argv[0], "set_heading")) {
if (argc > 1) {
const float heading = atof(argv[1]);
const float heading_accuracy = NAN;
bool ret = send_vehicle_command(vehicle_command_s::VEHICLE_CMD_EXTERNAL_ATTITUDE_ESTIMATE,
0.f, 0.f, heading, 0.f, 0.0, 0.0, heading_accuracy);
return (ret ? 0 : 1);
} else {
PX4_ERR("missing argument");
return 0;
}
}
if (!strcmp(argv[0], "poweroff")) {
bool ret = send_vehicle_command(vehicle_command_s::VEHICLE_CMD_PREFLIGHT_REBOOT_SHUTDOWN,
2.0f);
return (ret ? 0 : 1);
}
#endif
return print_usage("unknown command");
}
int Commander::print_status()
{
PX4_INFO("%s", isArmed() ? "Armed" : "Disarmed");
PX4_INFO("prearm safety: %s", _safety.isSafetyOff() ? "Off" : "On");
PX4_INFO("navigation mode: %s", mode_util::nav_state_names[_vehicle_status.nav_state]);
PX4_INFO("user intended navigation mode: %s", mode_util::nav_state_names[_vehicle_status.nav_state_user_intention]);
PX4_INFO("in failsafe: %s", _failsafe.inFailsafe() ? "yes" : "no");
_mode_management.printStatus();
perf_print_counter(_loop_perf);
perf_print_counter(_preflight_check_perf);
return 0;
}
extern "C" __EXPORT int commander_main(int argc, char *argv[])
{
return ModuleBase::main(Commander::desc, argc, argv);
}
static constexpr const char *arm_disarm_reason_str(arm_disarm_reason_t calling_reason)
{
static_assert((uint8_t)arm_disarm_reason_t::stick_gesture == vehicle_status_s::ARM_DISARM_REASON_STICK_GESTURE,
"(dis)arm enum mismatch");
static_assert((uint8_t)arm_disarm_reason_t::rc_switch == vehicle_status_s::ARM_DISARM_REASON_RC_SWITCH,
"(dis)arm enum mismatch");
static_assert((uint8_t)arm_disarm_reason_t::command_internal == vehicle_status_s::ARM_DISARM_REASON_COMMAND_INTERNAL,
"(dis)arm enum mismatch");
static_assert((uint8_t)arm_disarm_reason_t::command_external == vehicle_status_s::ARM_DISARM_REASON_COMMAND_EXTERNAL,
"(dis)arm enum mismatch");
static_assert((uint8_t)arm_disarm_reason_t::mission_start == vehicle_status_s::ARM_DISARM_REASON_MISSION_START,
"(dis)arm enum mismatch");
static_assert((uint8_t)arm_disarm_reason_t::landing == vehicle_status_s::ARM_DISARM_REASON_LANDING,
"(dis)arm enum mismatch");
static_assert(
(uint8_t)arm_disarm_reason_t::preflight_inaction == vehicle_status_s::ARM_DISARM_REASON_PREFLIGHT_INACTION,
"(dis)arm enum mismatch");
static_assert((uint8_t)arm_disarm_reason_t::kill_switch == vehicle_status_s::ARM_DISARM_REASON_KILL_SWITCH,
"(dis)arm enum mismatch");
static_assert((uint8_t)arm_disarm_reason_t::rc_button == vehicle_status_s::ARM_DISARM_REASON_RC_BUTTON,
"(dis)arm enum mismatch");
static_assert((uint8_t)arm_disarm_reason_t::failsafe == vehicle_status_s::ARM_DISARM_REASON_FAILSAFE,
"(dis)arm enum mismatch");
switch (calling_reason) {
case arm_disarm_reason_t::stick_gesture: return "stick gesture";
case arm_disarm_reason_t::rc_switch: return "RC switch";
case arm_disarm_reason_t::command_internal: return "internal command";
case arm_disarm_reason_t::command_external: return "external command";
case arm_disarm_reason_t::mission_start: return "mission start";
case arm_disarm_reason_t::landing: return "landing";
case arm_disarm_reason_t::preflight_inaction: return "auto preflight disarming";
case arm_disarm_reason_t::kill_switch: return "kill-switch";
case arm_disarm_reason_t::rc_button: return "RC button";
case arm_disarm_reason_t::failsafe: return "failsafe";
}
return "";
};
transition_result_t Commander::arm(arm_disarm_reason_t calling_reason, bool run_preflight_checks)
{
if (isArmed()) {
return TRANSITION_NOT_CHANGED;
}
if (_vehicle_status.calibration_enabled
|| _vehicle_status.rc_calibration_in_progress
|| _actuator_armed.in_esc_calibration_mode) {
mavlink_log_critical(&_mavlink_log_pub, "Arming denied: calibrating\t");
events::send(events::ID("commander_arm_denied_calibrating"), {events::Log::Critical, events::LogInternal::Info},
"Arming denied: calibrating");
tune_negative(true);
return TRANSITION_DENIED;
}
// allow a grace period for re-arming: preflight checks don't need to pass during that time, for example for accidental in-air disarming
if (calling_reason == arm_disarm_reason_t::rc_switch
&& ((_last_disarmed_timestamp != 0) && (hrt_elapsed_time(&_last_disarmed_timestamp) < 5_s))) {
run_preflight_checks = false;
}
if (run_preflight_checks) {
if (_vehicle_control_mode.flag_control_manual_enabled) {
if (!_failsafe_flags.manual_control_signal_lost) {
bool throttle_safe;
if (_vehicle_status.vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROVER) {
// Rovers: center stick = stop, safe to arm near center
throttle_safe = (fabsf(_last_manual_throttle) < 0.2f);
} else if (_vehicle_control_mode.flag_control_climb_rate_enabled) {
// Climb-rate modes (AltCtl, PosCtl, etc.): center = hover, safe at or below center
throttle_safe = (_last_manual_throttle <= 0.2f);
} else {
// Manual/Stab/Acro on MC/FW: bottom = no thrust, safe at bottom
throttle_safe = (_last_manual_throttle < -0.8f);
}
if (!throttle_safe) {
mavlink_log_critical(&_mavlink_log_pub, "Arming denied: throttle not in safe position\t");
events::send(events::ID("commander_arm_denied_throttle_unsafe"), {events::Log::Critical, events::LogInternal::Info},
"Arming denied: throttle not in safe position");
tune_negative(true);
return TRANSITION_DENIED;
}
}
}
_health_and_arming_checks.update(false, true);
if (!_health_and_arming_checks.canArm(_vehicle_status.nav_state)) {
tune_negative(true);
mavlink_log_critical(&_mavlink_log_pub, "Arming denied: Resolve system health failures first\t");
events::send(events::ID("commander_arm_denied_resolve_failures"), {events::Log::Critical, events::LogInternal::Info},
"Arming denied: Resolve system health failures first");
return TRANSITION_DENIED;
}
}
// Capture a fresh home position on the ground before the motors spin (prop wash perturbs the
// baro), but only at a real mission start: skip a mid-mission re-arm (seq_current > 0) and any
// in-air re-arm.
if (_param_com_home_en.get() && !_config_overrides.disable_auto_set_home
&& _vehicle_land_detected.landed
&& (!_mission_in_progress || _mission_result_sub.get().seq_current == 0)) {
_home_position.setHomePosition();
}
_vehicle_status.armed_time = hrt_absolute_time();
_vehicle_status.arming_state = vehicle_status_s::ARMING_STATE_ARMED;
_vehicle_status.latest_arming_reason = (uint8_t)calling_reason;
mavlink_log_info(&_mavlink_log_pub, "Armed by %s\t", arm_disarm_reason_str(calling_reason));
events::send<events::px4::enums::arm_disarm_reason_t>(events::ID("commander_armed_by"), events::Log::Info,
"Armed by {1}", calling_reason);
_status_changed = true;
return TRANSITION_CHANGED;
}
transition_result_t Commander::disarm(arm_disarm_reason_t calling_reason, bool forced)
{
if (!isArmed()) {
return TRANSITION_NOT_CHANGED;
}
if (!forced) {
const bool landed = (_vehicle_land_detected.landed || _vehicle_land_detected.maybe_landed
|| is_ground_vehicle(_vehicle_status));
const bool mc_manual_thrust_mode = _vehicle_status.vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING
&& _vehicle_control_mode.flag_control_manual_enabled
&& !_vehicle_control_mode.flag_control_climb_rate_enabled;
const bool commanded_by_rc = (calling_reason == arm_disarm_reason_t::stick_gesture)
|| (calling_reason == arm_disarm_reason_t::rc_switch)
|| (calling_reason == arm_disarm_reason_t::rc_button);
if (!landed && !(mc_manual_thrust_mode && commanded_by_rc)) {
if (calling_reason != arm_disarm_reason_t::stick_gesture) {
mavlink_log_critical(&_mavlink_log_pub, "Disarming denied: not landed\t");
events::send(events::ID("commander_disarm_denied_not_landed"),
{events::Log::Critical, events::LogInternal::Info},
"Disarming denied: not landed");
}
return TRANSITION_DENIED;
}
}
_vehicle_status.armed_time = 0;
_vehicle_status.arming_state = vehicle_status_s::ARMING_STATE_DISARMED;
_vehicle_status.latest_disarming_reason = (uint8_t)calling_reason;
_vehicle_status.takeoff_time = 0;
_have_taken_off_since_arming = false;
_last_disarmed_timestamp = hrt_absolute_time();
_user_mode_intention.onDisarm();
mavlink_log_info(&_mavlink_log_pub, "Disarmed by %s\t", arm_disarm_reason_str(calling_reason));
events::send<events::px4::enums::arm_disarm_reason_t>(events::ID("commander_disarmed_by"), events::Log::Info,
"Disarmed by {1}", calling_reason);
if (_param_com_force_safety.get()) {
_safety.activateSafety();
}
// update flight uuid
const int32_t flight_uuid = _param_com_flight_uuid.get() + 1;
_param_com_flight_uuid.set(flight_uuid);
_param_com_flight_uuid.commit_no_notification();
_status_changed = true;
return TRANSITION_CHANGED;
}
Commander::Commander() :
ModuleParams(nullptr)
{
_vehicle_land_detected.landed = true;
_vehicle_status.arming_state = vehicle_status_s::ARMING_STATE_DISARMED;
_vehicle_status.system_id = 1;
_vehicle_status.component_id = 1;
_vehicle_status.system_type = 0;
_vehicle_status.vehicle_type = vehicle_status_s::VEHICLE_TYPE_UNSPECIFIED;
_vehicle_status.nav_state = _user_mode_intention.get();
_vehicle_status.nav_state_user_intention = _user_mode_intention.get();
_vehicle_status.nav_state_timestamp = hrt_absolute_time();
_vehicle_status.gcs_connection_lost = true;
_vehicle_status.power_input_valid = true;
// default for vtol is rotary wing
_vtol_vehicle_status.vehicle_vtol_state = vtol_vehicle_status_s::VEHICLE_VTOL_STATE_MC;
param_t param_mav_comp_id = param_find("MAV_COMP_ID");
param_t param_mav_sys_id = param_find("MAV_SYS_ID");
_param_mav_type = param_find("MAV_TYPE");
_param_rc_map_fltmode = param_find("RC_MAP_FLTMODE");
int32_t value_int32 = 0;
// MAV_SYS_ID => vehicle_status.system_id
if ((param_mav_sys_id != PARAM_INVALID) && (param_get(param_mav_sys_id, &value_int32) == PX4_OK)) {
_vehicle_status.system_id = value_int32;
}
// MAV_COMP_ID => vehicle_status.component_id
if ((param_mav_comp_id != PARAM_INVALID) && (param_get(param_mav_comp_id, &value_int32) == PX4_OK)) {
_vehicle_status.component_id = value_int32;
}
updateParameters();
// Apply the configured boot flight mode before the first run
_user_mode_intention.change((uint8_t)_param_com_fltmode_boot.get(), ModeChangeSource::User, false, true);
_failsafe.setOnNotifyUserCallback(&Commander::onFailsafeNotifyUserTrampoline, this);
_auto_disarm_killed.set_hysteresis_time_from(false, 5_s);
}
Commander::~Commander()
{
perf_free(_loop_perf);
perf_free(_preflight_check_perf);
}
bool
Commander::handle_command(const vehicle_command_s &cmd)
{
/* only handle commands that are meant to be handled by this system and component, or broadcast */
if (((cmd.target_system != _vehicle_status.system_id) && (cmd.target_system != 0))
|| ((cmd.target_component != _vehicle_status.component_id) && (cmd.target_component != 0))) {
return false;
}
/* result of the command, sent via answer_command() once after the switch below.
* Cases that answer_command() themselves (e.g. because they must reply before an
* irreversible action, or because another module owns the reply) must `return true;`
* instead of `break;`, so they are not answered a second time here. */
unsigned cmd_result = vehicle_command_ack_s::VEHICLE_CMD_RESULT_UNSUPPORTED;
/* request to set different system mode */
switch (cmd.command) {
case vehicle_command_s::VEHICLE_CMD_DO_REPOSITION: {
// Just switch the flight mode here, the navigator takes care of
// doing something sensible with the coordinates. Its designed
// to not require navigator and command to receive / process
// the data at the exact same time.
const bool change_mode_requested = (uint32_t(cmd.param2) & 1) != 0;
if (change_mode_requested) {
// If already in course mode, stay in course mode (navigator handles any altitude update)
// Only switch to loiter if a specific lat/lon target is given, or we are not in course mode.
const bool has_position_target = PX4_ISFINITE(cmd.param5) && PX4_ISFINITE(cmd.param6);
const bool in_course_mode = _vehicle_status.nav_state == vehicle_status_s::NAVIGATION_STATE_GUIDED_COURSE;
const uint8_t target_state = (in_course_mode && !has_position_target)
? vehicle_status_s::NAVIGATION_STATE_GUIDED_COURSE
: vehicle_status_s::NAVIGATION_STATE_AUTO_LOITER;
if (_user_mode_intention.change(target_state, getSourceFromCommand(cmd))) {
cmd_result = vehicle_command_ack_s::VEHICLE_CMD_RESULT_ACCEPTED;
} else {
printRejectMode(target_state);
cmd_result = vehicle_command_ack_s::VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED;
}
} else if (_vehicle_status.nav_state == vehicle_status_s::NAVIGATION_STATE_AUTO_LOITER) {
// No mode switch requested: reposition the current hold setpoint in place.
cmd_result = vehicle_command_ack_s::VEHICLE_CMD_RESULT_ACCEPTED;
} else {
// Supported, but no mode switch requested and not in hold: nothing to act on.
cmd_result = vehicle_command_ack_s::VEHICLE_CMD_RESULT_DENIED;
}
}
break;
case vehicle_command_s::VEHICLE_CMD_DO_CHANGE_ALTITUDE: {
// Accept only in modes where the navigator handles altitude changes in-place.
// No mode switching: if the current mode doesn't support it, temporarily reject.
const uint8_t nav_state = _vehicle_status.nav_state;
if (nav_state == vehicle_status_s::NAVIGATION_STATE_GUIDED_COURSE
|| nav_state == vehicle_status_s::NAVIGATION_STATE_AUTO_LOITER) {
cmd_result = vehicle_command_ack_s::VEHICLE_CMD_RESULT_ACCEPTED;
} else {
cmd_result = vehicle_command_ack_s::VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED;
}
}
break;
case vehicle_command_s::VEHICLE_CMD_DO_SET_MODE: {
uint8_t base_mode = (uint8_t)cmd.param1;
uint8_t custom_main_mode = (uint8_t)cmd.param2;
uint8_t custom_sub_mode = (uint8_t)cmd.param3;
uint8_t desired_nav_state = vehicle_status_s::NAVIGATION_STATE_MAX;
transition_result_t main_ret = TRANSITION_NOT_CHANGED;
if (base_mode & VEHICLE_MODE_FLAG_CUSTOM_MODE_ENABLED) {
/* use autopilot-specific mode */
if (custom_main_mode == PX4_CUSTOM_MAIN_MODE_MANUAL) {
desired_nav_state = vehicle_status_s::NAVIGATION_STATE_MANUAL;
} else if (custom_main_mode == PX4_CUSTOM_MAIN_MODE_ALTCTL) {
desired_nav_state = vehicle_status_s::NAVIGATION_STATE_ALTCTL;
} else if (custom_main_mode == PX4_CUSTOM_MAIN_MODE_ALTITUDE_CRUISE) {
desired_nav_state = vehicle_status_s::NAVIGATION_STATE_ALTITUDE_CRUISE;
} else if (custom_main_mode == PX4_CUSTOM_MAIN_MODE_MANUAL_PARKING) {
desired_nav_state = vehicle_status_s::NAVIGATION_STATE_MANUAL_PARKING;
} else if (custom_main_mode == PX4_CUSTOM_MAIN_MODE_POSCTL) {
switch (custom_sub_mode) {
default:
case PX4_CUSTOM_SUB_MODE_POSCTL_POSCTL:
desired_nav_state = vehicle_status_s::NAVIGATION_STATE_POSCTL;
break;
case PX4_CUSTOM_SUB_MODE_POSCTL_SLOW:
desired_nav_state = vehicle_status_s::NAVIGATION_STATE_POSITION_SLOW;
break;
}
} else if (custom_main_mode == PX4_CUSTOM_MAIN_MODE_AUTO) {
if (custom_sub_mode > 0) {
switch (custom_sub_mode) {
case PX4_CUSTOM_SUB_MODE_AUTO_LOITER:
desired_nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_LOITER;
break;
case PX4_CUSTOM_SUB_MODE_AUTO_MISSION:
desired_nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_MISSION;
break;
case PX4_CUSTOM_SUB_MODE_AUTO_RTL:
desired_nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_RTL;
break;
case PX4_CUSTOM_SUB_MODE_AUTO_TAKEOFF:
desired_nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_TAKEOFF;
break;
case PX4_CUSTOM_SUB_MODE_AUTO_LAND:
desired_nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_LAND;
break;
case PX4_CUSTOM_SUB_MODE_AUTO_FOLLOW_TARGET:
desired_nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_FOLLOW_TARGET;
break;
case PX4_CUSTOM_SUB_MODE_AUTO_PRECLAND:
desired_nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_PRECLAND;
break;
case PX4_CUSTOM_SUB_MODE_GUIDED_COURSE:
desired_nav_state = vehicle_status_s::NAVIGATION_STATE_GUIDED_COURSE;
break;
case PX4_CUSTOM_SUB_MODE_EXTERNAL1...PX4_CUSTOM_SUB_MODE_EXTERNAL8:
desired_nav_state = vehicle_status_s::NAVIGATION_STATE_EXTERNAL1 + (custom_sub_mode - PX4_CUSTOM_SUB_MODE_EXTERNAL1);
break;
default:
main_ret = TRANSITION_DENIED;
mavlink_log_critical(&_mavlink_log_pub, "Unsupported auto mode\t");
events::send(events::ID("commander_unsupported_auto_mode"), events::Log::Error,
"Unsupported auto mode");
break;
}
} else {
desired_nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_MISSION;
}
} else if (custom_main_mode == PX4_CUSTOM_MAIN_MODE_ACRO) {
desired_nav_state = vehicle_status_s::NAVIGATION_STATE_ACRO;
} else if (custom_main_mode == PX4_CUSTOM_MAIN_MODE_STABILIZED) {
desired_nav_state = vehicle_status_s::NAVIGATION_STATE_STAB;
} else if (custom_main_mode == PX4_CUSTOM_MAIN_MODE_OFFBOARD) {
desired_nav_state = vehicle_status_s::NAVIGATION_STATE_OFFBOARD;
} else {
main_ret = TRANSITION_DENIED;
mavlink_log_critical(&_mavlink_log_pub, "Unsupported main mode\t");
events::send(events::ID("commander_unsupported_main_mode"), events::Log::Error,
"Unsupported main mode");
}
} else {
/* use base mode */
if (base_mode & VEHICLE_MODE_FLAG_AUTO_ENABLED) {
desired_nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_MISSION;
} else if (base_mode & VEHICLE_MODE_FLAG_MANUAL_INPUT_ENABLED) {
if (base_mode & VEHICLE_MODE_FLAG_GUIDED_ENABLED) {
desired_nav_state = vehicle_status_s::NAVIGATION_STATE_POSCTL;
} else if (base_mode & VEHICLE_MODE_FLAG_STABILIZE_ENABLED) {
desired_nav_state = vehicle_status_s::NAVIGATION_STATE_STAB;
} else {
desired_nav_state = vehicle_status_s::NAVIGATION_STATE_MANUAL;
}
} else {
main_ret = TRANSITION_DENIED;
mavlink_log_critical(&_mavlink_log_pub, "Unsupported base mode\t");
events::send(events::ID("commander_unsupported_base_mode"), events::Log::Error,
"Unsupported base mode");
}
}
if (desired_nav_state != vehicle_status_s::NAVIGATION_STATE_MAX) {
// Special handling for LAND mode: always allow to switch into it such that if used
// as emergency mode it is always available. When triggering it the user generally wants
// the vehicle to descend immediately, and if that means to switch to DESCEND it is fine.
const bool force = desired_nav_state == vehicle_status_s::NAVIGATION_STATE_AUTO_LAND;
if (_user_mode_intention.change(desired_nav_state, getSourceFromCommand(cmd), false, force)) {
main_ret = TRANSITION_CHANGED;
} else {
if (cmd.from_external && cmd.source_component == 190) { // MAV_COMP_ID_MISSIONPLANNER
printRejectMode(desired_nav_state);
}
main_ret = TRANSITION_DENIED;
}
}
if (main_ret != TRANSITION_DENIED) {
cmd_result = vehicle_command_ack_s::VEHICLE_CMD_RESULT_ACCEPTED;
} else {
cmd_result = vehicle_command_ack_s::VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED;
}
}
break;
case vehicle_command_s::VEHICLE_CMD_SET_NAV_STATE: { // Used from ROS
uint8_t desired_nav_state = static_cast<uint8_t>(lroundf(cmd.param1));
if (_user_mode_intention.change(desired_nav_state, getSourceFromCommand(cmd))) {
cmd_result = vehicle_command_ack_s::VEHICLE_CMD_RESULT_ACCEPTED;
} else {
cmd_result = vehicle_command_ack_s::VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED;
}
}
break;
case vehicle_command_s::VEHICLE_CMD_COMPONENT_ARM_DISARM: {
// Adhere to MAVLink specs, but base on knowledge that these fundamentally encode ints
// for logic state parameters
const int8_t arming_action = static_cast<int8_t>(lroundf(cmd.param1));
if (arming_action != vehicle_command_s::ARMING_ACTION_ARM
&& arming_action != vehicle_command_s::ARMING_ACTION_DISARM) {
mavlink_log_critical(&_mavlink_log_pub, "Unsupported ARM_DISARM param: %.3f\t", (double)cmd.param1);
events::send<float>(events::ID("commander_unsupported_arm_disarm_param"), events::Log::Error,
"Unsupported ARM_DISARM param: {1:.3}", cmd.param1);
} else {
// Arm is forced (checks skipped) when param2 is set to a magic number.
const bool forced = (static_cast<int>(lroundf(cmd.param2)) == 21196);
transition_result_t arming_res = TRANSITION_DENIED;
arm_disarm_reason_t arm_disarm_reason = cmd.from_external ? arm_disarm_reason_t::command_external :
arm_disarm_reason_t::command_internal;
if (arming_action == vehicle_command_s::ARMING_ACTION_ARM) {
arming_res = arm(arm_disarm_reason, cmd.from_external || !forced);
} else if (arming_action == vehicle_command_s::ARMING_ACTION_DISARM) {
arming_res = disarm(arm_disarm_reason, forced);
}
if (arming_res == TRANSITION_DENIED) {
cmd_result = vehicle_command_ack_s::VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED;
} else {
cmd_result = vehicle_command_ack_s::VEHICLE_CMD_RESULT_ACCEPTED;
}
}
}
break;
case vehicle_command_s::VEHICLE_CMD_DO_FLIGHTTERMINATION: {
cmd_result = vehicle_command_ack_s::VEHICLE_CMD_RESULT_DENIED;
if (cmd.param1 > 0.5f) {
// Trigger real termination.
if (!isArmed()) {
cmd_result = vehicle_command_ack_s::VEHICLE_CMD_RESULT_DENIED;
} else if (_user_mode_intention.change(vehicle_status_s::NAVIGATION_STATE_TERMINATION)) {
cmd_result = vehicle_command_ack_s::VEHICLE_CMD_RESULT_ACCEPTED;
} else {
cmd_result = vehicle_command_ack_s::VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED;
}
}
}
break;
case vehicle_command_s::VEHICLE_CMD_DO_SET_HOME: {
if (_param_com_home_en.get()) {
bool use_current = cmd.param1 > 0.5f;
if (use_current) {
/* use current position */
if (_home_position.setHomePosition(true)) {
cmd_result = vehicle_command_ack_s::VEHICLE_CMD_RESULT_ACCEPTED;
} else {
cmd_result = vehicle_command_ack_s::VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED;
}
} else {
float roll = matrix::wrap_2pi(math::radians(cmd.param2));
roll = PX4_ISFINITE(roll) ? roll : 0.0f;
float pitch = matrix::wrap_2pi(math::radians(cmd.param3));
pitch = PX4_ISFINITE(pitch) ? pitch : 0.0f;
float yaw = matrix::wrap_2pi(math::radians(cmd.param4));
yaw = PX4_ISFINITE(yaw) ? yaw : (float)NAN;
const double lat = cmd.param5;
const double lon = cmd.param6;
const float alt = cmd.param7;
if (PX4_ISFINITE(lat) && PX4_ISFINITE(lon) && PX4_ISFINITE(alt)) {
if (_home_position.setManually(lat, lon, alt, roll, pitch, yaw)) {
cmd_result = vehicle_command_ack_s::VEHICLE_CMD_RESULT_ACCEPTED;
} else {
cmd_result = vehicle_command_ack_s::VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED;
}
} else {
cmd_result = vehicle_command_ack_s::VEHICLE_CMD_RESULT_DENIED;
}
}
} else {
// COM_HOME_EN disabled
cmd_result = vehicle_command_ack_s::VEHICLE_CMD_RESULT_DENIED;
}
}
break;
case vehicle_command_s::VEHICLE_CMD_NAV_RETURN_TO_LAUNCH: {
/* switch to Return which ends the mission */
if (_user_mode_intention.change(vehicle_status_s::NAVIGATION_STATE_AUTO_RTL, getSourceFromCommand(cmd))) {
mavlink_log_info(&_mavlink_log_pub, "Switching to Return\t");
events::send(events::ID("commander_rtl"), events::Log::Info, "Switching to Return");
cmd_result = vehicle_command_ack_s::VEHICLE_CMD_RESULT_ACCEPTED;
} else {
printRejectMode(vehicle_status_s::NAVIGATION_STATE_AUTO_RTL);
cmd_result = vehicle_command_ack_s::VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED;
}
}
break;
case vehicle_command_s::VEHICLE_CMD_NAV_TAKEOFF: {
/* ok, home set, use it to take off */
if (_user_mode_intention.change(vehicle_status_s::NAVIGATION_STATE_AUTO_TAKEOFF, getSourceFromCommand(cmd))) {
cmd_result = vehicle_command_ack_s::VEHICLE_CMD_RESULT_ACCEPTED;
} else {
printRejectMode(vehicle_status_s::NAVIGATION_STATE_AUTO_TAKEOFF);
cmd_result = vehicle_command_ack_s::VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED;
}
}
break;
case vehicle_command_s::VEHICLE_CMD_NAV_VTOL_TAKEOFF:
#if CONFIG_MODE_NAVIGATOR_VTOL_TAKEOFF
/* ok, home set, use it to take off */
if (_user_mode_intention.change(vehicle_status_s::NAVIGATION_STATE_AUTO_VTOL_TAKEOFF, getSourceFromCommand(cmd))) {
cmd_result = vehicle_command_ack_s::VEHICLE_CMD_RESULT_ACCEPTED;
} else {
printRejectMode(vehicle_status_s::NAVIGATION_STATE_AUTO_VTOL_TAKEOFF);
cmd_result = vehicle_command_ack_s::VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED;
}
#else
cmd_result = vehicle_command_ack_s::VEHICLE_CMD_RESULT_UNSUPPORTED;
#endif // CONFIG_MODE_NAVIGATOR_VTOL_TAKEOFF
break;
case vehicle_command_s::VEHICLE_CMD_NAV_LAND: {
// Special handling for LAND mode: always allow to switch into it such that if used
// as emergency mode it is always available. When triggering it the user generally wants
// the vehicle to descend immediately, and if that means to switch to DESCEND it is fine.
const bool force = true;
if (_user_mode_intention.change(vehicle_status_s::NAVIGATION_STATE_AUTO_LAND, getSourceFromCommand(cmd), false,
force)) {
mavlink_log_info(&_mavlink_log_pub, "Landing at current position\t");
events::send(events::ID("commander_landing_current_pos"), events::Log::Info,
"Landing at current position");
cmd_result = vehicle_command_ack_s::VEHICLE_CMD_RESULT_ACCEPTED;
} else {
printRejectMode(vehicle_status_s::NAVIGATION_STATE_AUTO_LAND);
cmd_result = vehicle_command_ack_s::VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED;
}
}
break;
case vehicle_command_s::VEHICLE_CMD_NAV_PRECLAND: {
if (_user_mode_intention.change(vehicle_status_s::NAVIGATION_STATE_AUTO_PRECLAND, getSourceFromCommand(cmd))) {
mavlink_log_info(&_mavlink_log_pub, "Precision landing\t");
events::send(events::ID("commander_landing_prec_land"), events::Log::Info,
"Landing using precision landing");
cmd_result = vehicle_command_ack_s::VEHICLE_CMD_RESULT_ACCEPTED;
} else {
printRejectMode(vehicle_status_s::NAVIGATION_STATE_AUTO_PRECLAND);
cmd_result = vehicle_command_ack_s::VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED;
}
}
break;
case vehicle_command_s::VEHICLE_CMD_MISSION_START: {
cmd_result = vehicle_command_ack_s::VEHICLE_CMD_RESULT_DENIED;
// check if current mission and first item are valid
if (!_failsafe_flags.auto_mission_missing) {
// requested first mission item valid
if (PX4_ISFINITE(cmd.param1) && (cmd.param1 >= -1) && (cmd.param1 < _mission_result_sub.get().seq_total)) {
// switch to AUTO_MISSION and ARM
if (_user_mode_intention.change(vehicle_status_s::NAVIGATION_STATE_AUTO_MISSION, getSourceFromCommand(cmd))
&& (TRANSITION_DENIED != arm(arm_disarm_reason_t::mission_start))) {
cmd_result = vehicle_command_ack_s::VEHICLE_CMD_RESULT_ACCEPTED;
} else {
printRejectMode(vehicle_status_s::NAVIGATION_STATE_AUTO_MISSION);
cmd_result = vehicle_command_ack_s::VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED;
}
}
} else {
mavlink_log_critical(&_mavlink_log_pub, "Mission start denied! No valid mission\t");
events::send(events::ID("commander_mission_start_denied_no_mission"), {events::Log::Critical, events::LogInternal::Info},
"Mission start denied! No valid mission");
}
}
break;
case vehicle_command_s::VEHICLE_CMD_CONTROL_HIGH_LATENCY: {
// if no high latency telemetry exists send a failed acknowledge
if (_high_latency_datalink_timestamp < _boot_timestamp) {
cmd_result = vehicle_command_ack_s::VEHICLE_CMD_RESULT_FAILED;
mavlink_log_critical(&_mavlink_log_pub, "Control high latency failed! Telemetry unavailable\t");
events::send(events::ID("commander_ctrl_high_latency_failed"), {events::Log::Critical, events::LogInternal::Info},
"Control high latency failed! Telemetry unavailable");
}
}
break;
case vehicle_command_s::VEHICLE_CMD_DO_ORBIT: {
transition_result_t main_ret;
if (_vehicle_status.in_transition_mode) {
main_ret = TRANSITION_DENIED;
} else if (_vehicle_status.vehicle_type == vehicle_status_s::VEHICLE_TYPE_FIXED_WING) {
// for fixed wings the behavior of orbit is the same as loiter
if (_user_mode_intention.change(vehicle_status_s::NAVIGATION_STATE_AUTO_LOITER, getSourceFromCommand(cmd))) {
main_ret = TRANSITION_CHANGED;
} else {
main_ret = TRANSITION_DENIED;
}
} else {
// Switch to orbit state and let the orbit task handle the command further
if (_user_mode_intention.change(vehicle_status_s::NAVIGATION_STATE_ORBIT, getSourceFromCommand(cmd))) {
main_ret = TRANSITION_CHANGED;
} else {
main_ret = TRANSITION_DENIED;
}
}
if (main_ret != TRANSITION_DENIED) {
cmd_result = vehicle_command_ack_s::VEHICLE_CMD_RESULT_ACCEPTED;
} else {
cmd_result = vehicle_command_ack_s::VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED;
mavlink_log_critical(&_mavlink_log_pub, "Orbit command rejected");
}
}
break;
case vehicle_command_s::VEHICLE_CMD_DO_FIGUREEIGHT: {
#ifdef CONFIG_FIGURE_OF_EIGHT
if (!((_vehicle_status.vehicle_type == vehicle_status_s::VEHICLE_TYPE_FIXED_WING) || (_vehicle_status.is_vtol))) {
cmd_result = vehicle_command_ack_s::VEHICLE_CMD_RESULT_UNSUPPORTED;
mavlink_log_critical(&_mavlink_log_pub, "Figure 8 command only available for fixed wing and vtol vehicles.");
break;
}
transition_result_t main_ret = TRANSITION_DENIED;
if ((_vehicle_status.vehicle_type == vehicle_status_s::VEHICLE_TYPE_FIXED_WING) &&
(!_vehicle_status.in_transition_mode)) {
if (_user_mode_intention.change(vehicle_status_s::NAVIGATION_STATE_AUTO_LOITER)) {
main_ret = TRANSITION_CHANGED;
} else {
main_ret = TRANSITION_DENIED;
}
}
if (main_ret != TRANSITION_DENIED) {
cmd_result = vehicle_command_ack_s::VEHICLE_CMD_RESULT_ACCEPTED;
} else {
cmd_result = vehicle_command_ack_s::VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED;
mavlink_log_critical(&_mavlink_log_pub, "Figure 8 command rejected, Only available in fixed wing mode.");
}
#else
cmd_result = vehicle_command_ack_s::VEHICLE_CMD_RESULT_UNSUPPORTED;
mavlink_log_critical(&_mavlink_log_pub, "Figure 8 command not supported.");
#endif // CONFIG_FIGURE_OF_EIGHT
}
break;
case vehicle_command_s::VEHICLE_CMD_ACTUATOR_TEST:
cmd_result = handleCommandActuatorTest(cmd);
break;
case vehicle_command_s::VEHICLE_CMD_PREFLIGHT_REBOOT_SHUTDOWN: {
const int param1 = cmd.param1;
if (param1 == 0) {
// 0: Do nothing for autopilot
answer_command(cmd, vehicle_command_ack_s::VEHICLE_CMD_RESULT_ACCEPTED);
#if defined(CONFIG_BOARDCTL_RESET)
} else if ((param1 == 1) && (!isArmed() || isTerminated()) && (px4_reboot_request(REBOOT_REQUEST, 400_ms) == 0)) {
// 1: Reboot autopilot
answer_command(cmd, vehicle_command_ack_s::VEHICLE_CMD_RESULT_ACCEPTED);
while (1) { px4_usleep(1); }
#endif // CONFIG_BOARDCTL_RESET
#if defined(BOARD_HAS_POWER_CONTROL)
} else if ((param1 == 2) && !isArmed() && (px4_shutdown_request(400_ms) == 0)) {
// 2: Shutdown autopilot
answer_command(cmd, vehicle_command_ack_s::VEHICLE_CMD_RESULT_ACCEPTED);
while (1) { px4_usleep(1); }
#endif // BOARD_HAS_POWER_CONTROL
#if defined(CONFIG_BOARDCTL_RESET)
} else if ((param1 == 3) && !isArmed() && (px4_reboot_request(REBOOT_TO_BOOTLOADER, 400_ms) == 0)) {
// 3: Reboot autopilot and keep it in the bootloader until upgraded.
answer_command(cmd, vehicle_command_ack_s::VEHICLE_CMD_RESULT_ACCEPTED);
while (1) { px4_usleep(1); }
#endif // CONFIG_BOARDCTL_RESET
} else {
answer_command(cmd, vehicle_command_ack_s::VEHICLE_CMD_RESULT_DENIED);
}
}
return true;
case vehicle_command_s::VEHICLE_CMD_PREFLIGHT_CALIBRATION: {
if (isArmed() || _worker_thread.isBusy()) {
// reject if armed or shutting down
answer_command(cmd, vehicle_command_ack_s::VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED);
} else if (_vehicle_status.hil_state == vehicle_status_s::HIL_STATE_ON
&& ((int)(cmd.param1) != 0 || (int)(cmd.param2) != 0 || (int)(cmd.param3) != 0
|| (int)(cmd.param5) != 0 || (int)(cmd.param6) != 0 || (int)(cmd.param7) != 0)) {
// reject sensor calibration in SIH mode — simulated sensors cannot be calibrated,
// RC (param4) is a real receiver and stays allowed
answer_command(cmd, vehicle_command_ack_s::VEHICLE_CMD_RESULT_DENIED);
mavlink_log_critical(&_mavlink_log_pub, "Sensor calibration denied: not supported in SIH mode\t");
events::send(events::ID("commander_calib_denied_sih"), events::Log::Critical,
"Sensor calibration denied: not supported in SIH mode");
} else {
if ((int)(cmd.param1) == 1) {
/* gyro calibration */
answer_command(cmd, vehicle_command_ack_s::VEHICLE_CMD_RESULT_ACCEPTED);
_vehicle_status.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 */
return true;
} else if ((int)(cmd.param2) == 1) {
/* magnetometer calibration */
#if defined(CONFIG_SENSORS_VEHICLE_MAGNETOMETER)
answer_command(cmd, vehicle_command_ack_s::VEHICLE_CMD_RESULT_ACCEPTED);
_vehicle_status.calibration_enabled = true;
_worker_thread.startTask(WorkerThread::Request::MagCalibration);
#else
answer_command(cmd, vehicle_command_ack_s::VEHICLE_CMD_RESULT_UNSUPPORTED);
#endif
} else if ((int)(cmd.param3) == 1) {
/* baro calibration */
answer_command(cmd, vehicle_command_ack_s::VEHICLE_CMD_RESULT_ACCEPTED);
_vehicle_status.calibration_enabled = true;
_worker_thread.startTask(WorkerThread::Request::BaroCalibration);
} else if ((int)(cmd.param4) == 1) {
/* RC calibration */
answer_command(cmd, vehicle_command_ack_s::VEHICLE_CMD_RESULT_ACCEPTED);
/* disable RC control input completely */
_vehicle_status.rc_calibration_in_progress = true;
mavlink_log_info(&_mavlink_log_pub, "Calibration: Disabling RC input\t");
events::send(events::ID("commander_calib_rc_off"), events::Log::Info,
"Calibration: Disabling RC input");
} else if ((int)(cmd.param4) == 2) {
/* RC trim calibration */
answer_command(cmd, vehicle_command_ack_s::VEHICLE_CMD_RESULT_ACCEPTED);
_vehicle_status.calibration_enabled = true;
_worker_thread.startTask(WorkerThread::Request::RCTrimCalibration);
} else if ((int)(cmd.param5) == 1) {
/* accelerometer calibration */
answer_command(cmd, vehicle_command_ack_s::VEHICLE_CMD_RESULT_ACCEPTED);
_vehicle_status.calibration_enabled = true;
_worker_thread.startTask(WorkerThread::Request::AccelCalibration);
} else if ((int)(cmd.param5) == 2) {
// board offset calibration
answer_command(cmd, vehicle_command_ack_s::VEHICLE_CMD_RESULT_ACCEPTED);
_vehicle_status.calibration_enabled = true;
_worker_thread.startTask(WorkerThread::Request::LevelCalibration);
} else if ((int)(cmd.param5) == 4) {
// accelerometer quick calibration
answer_command(cmd, vehicle_command_ack_s::VEHICLE_CMD_RESULT_ACCEPTED);
_vehicle_status.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_ack_s::VEHICLE_CMD_RESULT_ACCEPTED);
_vehicle_status.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)) {
if (!_safety.isSafetyOff()) {
answer_command(cmd, vehicle_command_ack_s::VEHICLE_CMD_RESULT_DENIED);
const char *safety_message = _safety.isButtonAvailable() ? "Press safety button" : "Turn safety off";
mavlink_log_critical(&_mavlink_log_pub, "ESC calibration denied! %s first\t", safety_message);
events::send(events::ID("commander_esc_calibration_denied"), events::Log::Critical,
"ESCs calibration denied");
} else {
answer_command(cmd, vehicle_command_ack_s::VEHICLE_CMD_RESULT_ACCEPTED);
_vehicle_status.calibration_enabled = true;
_actuator_armed.in_esc_calibration_mode = true;
_worker_thread.startTask(WorkerThread::Request::ESCCalibration);
}
} else {
answer_command(cmd, vehicle_command_ack_s::VEHICLE_CMD_RESULT_DENIED);
}
} else if ((int)(cmd.param4) == 0) {
/* RC calibration ended - have we been in one worth confirming? */
if (_vehicle_status.rc_calibration_in_progress) {
/* enable RC control input */
_vehicle_status.rc_calibration_in_progress = false;
mavlink_log_info(&_mavlink_log_pub, "Calibration: Restoring RC input\t");
events::send(events::ID("commander_calib_rc_on"), events::Log::Info,
"Calibration: Restoring RC input");
}
answer_command(cmd, vehicle_command_ack_s::VEHICLE_CMD_RESULT_ACCEPTED);
} else {
answer_command(cmd, vehicle_command_ack_s::VEHICLE_CMD_RESULT_UNSUPPORTED);
}
}
return true;
}
case vehicle_command_s::VEHICLE_CMD_FIXED_MAG_CAL_YAW: {
// Magnetometer quick calibration using world magnetic model and known heading
if (isArmed() || _worker_thread.isBusy()) {
// reject if armed or shutting down
answer_command(cmd, vehicle_command_ack_s::VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED);
} else if (_vehicle_status.hil_state == vehicle_status_s::HIL_STATE_ON) {
// reject calibration in SIH mode — simulated sensors cannot be calibrated
answer_command(cmd, vehicle_command_ack_s::VEHICLE_CMD_RESULT_DENIED);
mavlink_log_critical(&_mavlink_log_pub, "Calibration denied: not supported in SIH mode\t");
events::send(events::ID("commander_mag_yaw_calib_denied_sih"), events::Log::Critical,
"Calibration denied: not supported in SIH mode");
} else {
#if defined(CONFIG_SENSORS_VEHICLE_MAGNETOMETER)
answer_command(cmd, vehicle_command_ack_s::VEHICLE_CMD_RESULT_ACCEPTED);
// parameter 1: Heading (degrees)
// parameter 3: Latitude (degrees)
// parameter 4: Longitude (degrees)
// 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;
}
}
_vehicle_status.calibration_enabled = true;
_worker_thread.setMagQuickData(heading_radians, latitude, longitude);
_worker_thread.startTask(WorkerThread::Request::MagCalibrationQuick);
#else
answer_command(cmd, vehicle_command_ack_s::VEHICLE_CMD_RESULT_UNSUPPORTED);
#endif
}
return true;
}
case vehicle_command_s::VEHICLE_CMD_PREFLIGHT_STORAGE: {
if (isArmed() || _worker_thread.isBusy()) {
// reject if armed or shutting down
answer_command(cmd, vehicle_command_ack_s::VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED);
} else {
if (((int)(cmd.param1)) == 0) {
answer_command(cmd, vehicle_command_ack_s::VEHICLE_CMD_RESULT_ACCEPTED);
_worker_thread.startTask(WorkerThread::Request::ParamLoadDefault);
} else if (((int)(cmd.param1)) == 1) {
answer_command(cmd, vehicle_command_ack_s::VEHICLE_CMD_RESULT_ACCEPTED);
_worker_thread.startTask(WorkerThread::Request::ParamSaveDefault);
} else if (((int)(cmd.param1)) == 2) {
answer_command(cmd, vehicle_command_ack_s::VEHICLE_CMD_RESULT_ACCEPTED);
_worker_thread.startTask(WorkerThread::Request::ParamResetAllConfig);
} else if (((int)(cmd.param1)) == 3) {
answer_command(cmd, vehicle_command_ack_s::VEHICLE_CMD_RESULT_ACCEPTED);
_worker_thread.startTask(WorkerThread::Request::ParamResetSensorFactory);
} else if (((int)(cmd.param1)) == 4) {
answer_command(cmd, vehicle_command_ack_s::VEHICLE_CMD_RESULT_ACCEPTED);
_worker_thread.startTask(WorkerThread::Request::ParamResetAll);
}
}
return true;
}
case vehicle_command_s::VEHICLE_CMD_DO_SET_STANDARD_MODE: {
mode_util::StandardMode standard_mode = (mode_util::StandardMode) roundf(cmd.param1);
uint8_t nav_state = mode_util::getNavStateFromStandardMode(standard_mode, _vehicle_status.vehicle_type,
_vehicle_status.is_vtol);
if (nav_state == vehicle_status_s::NAVIGATION_STATE_MAX) {
answer_command(cmd, vehicle_command_ack_s::VEHICLE_CMD_RESULT_FAILED);
} else {
if (_user_mode_intention.change(nav_state, getSourceFromCommand(cmd))) {
answer_command(cmd, vehicle_command_ack_s::VEHICLE_CMD_RESULT_ACCEPTED);
} else {
answer_command(cmd, vehicle_command_ack_s::VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED);
}
}
}
return true;
case vehicle_command_s::VEHICLE_CMD_RUN_PREARM_CHECKS:
_health_and_arming_checks.update(true);
answer_command(cmd, vehicle_command_ack_s::VEHICLE_CMD_RESULT_ACCEPTED);
return true;
case vehicle_command_s::VEHICLE_CMD_DO_SET_ACTUATOR:
answer_command(cmd, vehicle_command_ack_s::VEHICLE_CMD_RESULT_ACCEPTED);
return true;
case vehicle_command_s::VEHICLE_CMD_DO_SET_SAFETY_SWITCH_STATE: {
// reject if armed, only allow pre or post flight for safety
// or if COM_SAFETY_MODE is not set to accept mavlink commands
if (isArmed()) {
answer_command(cmd, vehicle_command_ack_s::VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED);
} else if ((SafetyMode)_param_com_safety_mode.get() == SafetyMode::BUTTON_ONLY) {
answer_command(cmd, vehicle_command_ack_s::VEHICLE_CMD_RESULT_DENIED);
} else {
int commanded_state = (int)cmd.param1;
if (commanded_state == vehicle_command_s::SAFETY_DANGEROUS) {
_safety.deactivateSafety();
answer_command(cmd, vehicle_command_ack_s::VEHICLE_CMD_RESULT_ACCEPTED);
} else if (commanded_state == vehicle_command_s::SAFETY_SAFE) {
if (_safety.isSafetyDisabled()) {
// COM_SAFETY_MODE 0 keeps safety off, so turning it on would be a no-op
answer_command(cmd, vehicle_command_ack_s::VEHICLE_CMD_RESULT_DENIED);
} else {
_safety.activateSafety();
answer_command(cmd, vehicle_command_ack_s::VEHICLE_CMD_RESULT_ACCEPTED);
}
} else {
answer_command(cmd, vehicle_command_ack_s::VEHICLE_CMD_RESULT_FAILED);
}
}
}
return true;
case vehicle_command_s::VEHICLE_CMD_GUIDED_CHANGE_HEADING: // Handled by navigator
case vehicle_command_s::VEHICLE_CMD_START_RX_PAIR:
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_SET_SENSOR_OFFSETS:
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:
case vehicle_command_s::VEHICLE_CMD_DO_VTOL_TRANSITION:
case vehicle_command_s::VEHICLE_CMD_DO_TRIGGER_CONTROL:
case vehicle_command_s::VEHICLE_CMD_DO_DIGICAM_CONTROL:
case vehicle_command_s::VEHICLE_CMD_DO_SET_CAM_TRIGG_DIST:
case vehicle_command_s::VEHICLE_CMD_OBLIQUE_SURVEY:
case vehicle_command_s::VEHICLE_CMD_DO_SET_CAM_TRIGG_INTERVAL:
case vehicle_command_s::VEHICLE_CMD_SET_CAMERA_MODE:
case vehicle_command_s::VEHICLE_CMD_SET_CAMERA_ZOOM:
case vehicle_command_s::VEHICLE_CMD_SET_CAMERA_FOCUS:
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_DO_SET_MISSION_CURRENT:
case vehicle_command_s::VEHICLE_CMD_LOGGING_START:
case vehicle_command_s::VEHICLE_CMD_LOGGING_STOP:
case vehicle_command_s::VEHICLE_CMD_NAV_DELAY:
case vehicle_command_s::VEHICLE_CMD_DO_SET_ROI:
case vehicle_command_s::VEHICLE_CMD_NAV_ROI:
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_INJECT_FAILURE:
case vehicle_command_s::VEHICLE_CMD_SET_GPS_GLOBAL_ORIGIN:
case vehicle_command_s::VEHICLE_CMD_DO_SET_GLOBAL_ORIGIN:
case vehicle_command_s::VEHICLE_CMD_DO_GIMBAL_MANAGER_PITCHYAW:
case vehicle_command_s::VEHICLE_CMD_DO_GIMBAL_MANAGER_CONFIGURE:
case vehicle_command_s::VEHICLE_CMD_CONFIGURE_ACTUATOR:
case vehicle_command_s::VEHICLE_CMD_ESC_REQUEST_EEPROM:
case vehicle_command_s::VEHICLE_CMD_REQUEST_MESSAGE:
case vehicle_command_s::VEHICLE_CMD_DO_WINCH:
case vehicle_command_s::VEHICLE_CMD_DO_GRIPPER:
case vehicle_command_s::VEHICLE_CMD_EXTERNAL_POSITION_ESTIMATE:
case vehicle_command_s::VEHICLE_CMD_REQUEST_CAMERA_INFORMATION:
case vehicle_command_s::VEHICLE_CMD_EXTERNAL_ATTITUDE_ESTIMATE:
case vehicle_command_s::VEHICLE_CMD_DO_AUTOTUNE_ENABLE:
case vehicle_command_s::VEHICLE_CMD_ESTIMATOR_SENSOR_ENABLE:
case vehicle_command_s::VEHICLE_CMD_ACTUATOR_GROUP_TEST:
/* ignore commands that are handled by other parts of the system: no reply from here */
return true;
default:
/* Command not handled above: reply UNSUPPORTED. This makes sense because only commands
* to this component ID (or all) are passed by mavlink. */
cmd_result = vehicle_command_ack_s::VEHICLE_CMD_RESULT_UNSUPPORTED;
break;
}
answer_command(cmd, cmd_result);
return true;
}
ModeChangeSource Commander::getSourceFromCommand(const vehicle_command_s &cmd)
{
return cmd.source_component >= vehicle_command_s::COMPONENT_MODE_EXECUTOR_START ? ModeChangeSource::ModeExecutor :
ModeChangeSource::User;
}
void Commander::handleCommandsFromModeExecutors()
{
if (_vehicle_command_mode_executor_sub.updated()) {
const unsigned last_generation = _vehicle_command_mode_executor_sub.get_last_generation();
vehicle_command_s cmd;
if (_vehicle_command_mode_executor_sub.copy(&cmd)) {
if (_vehicle_command_mode_executor_sub.get_last_generation() != last_generation + 1) {
PX4_ERR("vehicle_command from executor lost, generation %u -> %u", last_generation,
_vehicle_command_mode_executor_sub.get_last_generation());
}
// For commands from mode executors, we check if it is in charge and then publish it on the official
// command topic
const int mode_executor_in_charge = _mode_management.modeExecutorInCharge();
// source_system is set to the mode executor
if (cmd.source_component == vehicle_command_s::COMPONENT_MODE_EXECUTOR_START + mode_executor_in_charge) {
cmd.source_system = _vehicle_status.system_id;
cmd.timestamp = hrt_absolute_time();
_vehicle_command_pub.publish(cmd);
} else {
cmd.source_system = _vehicle_status.system_id;
answer_command(cmd, vehicle_command_ack_s::VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED);
PX4_WARN("Got cmd from executor %i not in charge (in charge: %i)", cmd.source_system, mode_executor_in_charge);
}
}
}
}
unsigned Commander::handleCommandActuatorTest(const vehicle_command_s &cmd)
{
if (isArmed() || !_safety.isSafetyOff()) {
return vehicle_command_ack_s::VEHICLE_CMD_RESULT_DENIED;
}
actuator_test_s actuator_test{};
actuator_test.timestamp = hrt_absolute_time();
actuator_test.function = static_cast<int>(lroundf(cmd.param5));
if (actuator_test.function < 1000) {
const int first_motor_function = 1; // from MAVLink ACTUATOR_OUTPUT_FUNCTION
const int first_servo_function = 33;
if (actuator_test.function >= first_motor_function
&& actuator_test.function < first_motor_function + actuator_test_s::MAX_NUM_MOTORS) {
actuator_test.function = actuator_test.function - first_motor_function + actuator_test_s::FUNCTION_MOTOR1;
} else if (actuator_test.function >= first_servo_function
&& actuator_test.function < first_servo_function + actuator_test_s::MAX_NUM_SERVOS) {
actuator_test.function = actuator_test.function - first_servo_function + actuator_test_s::FUNCTION_SERVO1;
} else {
return vehicle_command_ack_s::VEHICLE_CMD_RESULT_UNSUPPORTED;
}
} else {
actuator_test.function -= 1000;
}
actuator_test.value = cmd.param1;
actuator_test.action = actuator_test_s::ACTION_DO_CONTROL;
int timeout_ms = static_cast<int>(lroundf(cmd.param2 * 1000.f));
if (timeout_ms <= 0) {
actuator_test.action = actuator_test_s::ACTION_RELEASE_CONTROL;
} else {
actuator_test.timeout_ms = timeout_ms;
}
// enforce a timeout and a maximum limit
if (actuator_test.timeout_ms == 0 || actuator_test.timeout_ms > 3000) {
actuator_test.timeout_ms = 3000;
}
_actuator_test_pub.publish(actuator_test);
return vehicle_command_ack_s::VEHICLE_CMD_RESULT_ACCEPTED;
}
void Commander::executeActionRequest(const action_request_s &action_request)
{
arm_disarm_reason_t arm_disarm_reason{};
// Silently ignore RC actions during RC calibration
if (_vehicle_status.rc_calibration_in_progress
&& (action_request.source == action_request_s::SOURCE_STICK_GESTURE
|| action_request.source == action_request_s::SOURCE_RC_SWITCH
|| action_request.source == action_request_s::SOURCE_RC_BUTTON
|| action_request.source == action_request_s::SOURCE_RC_MODE_SLOT)) {
return;
}
switch (action_request.source) {
case action_request_s::SOURCE_STICK_GESTURE: arm_disarm_reason = arm_disarm_reason_t::stick_gesture; break;
case action_request_s::SOURCE_RC_SWITCH: arm_disarm_reason = arm_disarm_reason_t::rc_switch; break;
case action_request_s::SOURCE_RC_BUTTON: arm_disarm_reason = arm_disarm_reason_t::rc_button; break;
}
switch (action_request.action) {
case action_request_s::ACTION_DISARM: disarm(arm_disarm_reason); break;
case action_request_s::ACTION_ARM: arm(arm_disarm_reason); break;
case action_request_s::ACTION_TOGGLE_ARMING:
if (isArmed()) {
disarm(arm_disarm_reason);
} else {
arm(arm_disarm_reason);
}
break;
case action_request_s::ACTION_UNKILL:
if (_actuator_armed.kill) {
mavlink_log_info(&_mavlink_log_pub, "Kill disengaged\t");
events::send(events::ID("commander_kill_sw_disengaged"), events::Log::Info, "Kill disengaged");
_status_changed = true;
_actuator_armed.kill = false;
}
break;
case action_request_s::ACTION_KILL:
if (!_actuator_armed.kill) {
const char kill_switch_string[] = "Kill engaged\t";
events::LogLevels log_levels{events::Log::Info};
if (_vehicle_land_detected.landed) {
mavlink_log_info(&_mavlink_log_pub, kill_switch_string);
} else {
mavlink_log_critical(&_mavlink_log_pub, kill_switch_string);
log_levels.external = events::Log::Critical;
}
events::send(events::ID("commander_kill_sw_engaged"), log_levels, "Kill engaged");
_status_changed = true;
_actuator_armed.kill = true;
}
break;
case action_request_s::ACTION_TERMINATION:
_user_mode_intention.change(vehicle_status_s::NAVIGATION_STATE_TERMINATION);
break;
case action_request_s::ACTION_SWITCH_MODE:
if (!_user_mode_intention.change(action_request.mode, ModeChangeSource::User, false)) {
printRejectMode(action_request.mode);
}
break;
}
}
void Commander::updateParameters()
{
// update parameters from storage
updateParams();
int32_t value_int32 = 0;
// MAV_TYPE -> vehicle_status.system_type
if ((_param_mav_type != PARAM_INVALID) && (param_get(_param_mav_type, &value_int32) == PX4_OK)) {
_vehicle_status.system_type = value_int32;
}
const bool is_rotary = is_rotary_wing(_vehicle_status) || (is_vtol(_vehicle_status)
&& _vtol_vehicle_status.vehicle_vtol_state != vtol_vehicle_status_s::VEHICLE_VTOL_STATE_FW);
const bool is_fixed = is_fixed_wing(_vehicle_status) || (is_vtol(_vehicle_status)
&& _vtol_vehicle_status.vehicle_vtol_state == vtol_vehicle_status_s::VEHICLE_VTOL_STATE_FW);
const bool is_ground = is_ground_vehicle(_vehicle_status);
/* disable manual override for all systems that rely on electronic stabilization */
if (is_rotary) {
_vehicle_status.vehicle_type = vehicle_status_s::VEHICLE_TYPE_ROTARY_WING;
} else if (is_fixed) {
_vehicle_status.vehicle_type = vehicle_status_s::VEHICLE_TYPE_FIXED_WING;
} else if (is_ground) {
_vehicle_status.vehicle_type = vehicle_status_s::VEHICLE_TYPE_ROVER;
}
_vehicle_status.is_vtol = is_vtol(_vehicle_status);
_vehicle_status.is_vtol_tailsitter = is_vtol_tailsitter(_vehicle_status);
// _mode_switch_mapped = (RC_MAP_FLTMODE > 0)
if (_param_rc_map_fltmode != PARAM_INVALID && (param_get(_param_rc_map_fltmode, &value_int32) == PX4_OK)) {
_mode_switch_mapped = (value_int32 > 0);
}
}
void Commander::run()
{
/* initialize */
led_init();
buzzer_init();
#if defined(BOARD_HAS_POWER_CONTROL)
{
// we need to do an initial publication to make sure uORB allocates the buffer, which cannot happen
// in IRQ context.
power_button_state_s button_state{};
button_state.timestamp = hrt_absolute_time();
button_state.event = 0xff;
power_button_state_pub = orb_advertise(ORB_ID(power_button_state), &button_state);
_power_button_state_sub.copy(&button_state);
tune_control_s tune_control{};
tune_control.timestamp = hrt_absolute_time();
tune_control_pub = orb_advertise(ORB_ID(tune_control), &tune_control);
}
if (board_register_power_state_notification_cb(power_button_state_notification_cb) != 0) {
PX4_ERR("Failed to register power notification callback");
}
#endif // BOARD_HAS_POWER_CONTROL
_boot_timestamp = hrt_absolute_time();
_arm_on_boot_requested = _param_com_arm_on_boot.get();
arm_auth_init(&_mavlink_log_pub, &_vehicle_status.system_id);
while (!should_exit()) {
perf_begin(_loop_perf);
const actuator_armed_s actuator_armed_prev{_actuator_armed};
/* update parameters */
const bool params_updated = _parameter_update_sub.updated();
if (params_updated) {
// clear update
parameter_update_s update;
_parameter_update_sub.copy(&update);
updateParameters();
_status_changed = true;
}
handlePowerButtonState();
systemPowerUpdate();
landDetectorUpdate();
safetyButtonUpdate();
_multicopter_throw_launch.update(isArmed());
vtolStatusUpdate();
_mission_in_progress = (_vehicle_status.nav_state == vehicle_status_s::NAVIGATION_STATE_AUTO_MISSION)
&& !_mission_result_sub.get().finished;
_home_position.update(_param_com_home_en.get(), !isArmed() && _vehicle_land_detected.landed && !_mission_in_progress
&& !_config_overrides.disable_auto_set_home);
handleAutoDisarm();
battery_status_check();
checkForMissionUpdate();
manualControlCheck();
manualControlLossModeSwitch();
offboardControlCheck();
// data link checks which update the status
dataLinkCheck();
// Check for failure detector status
if (_failure_detector.update(_vehicle_status, _vehicle_control_mode)) {
_status_changed = true;
}
modeManagementUpdate();
const hrt_abstime now = hrt_absolute_time();
const bool nav_state_or_failsafe_changed = handleModeIntentionAndFailsafe();
// Run arming checks @ 10Hz
if ((now >= _last_health_and_arming_check + 100_ms) || _status_changed || nav_state_or_failsafe_changed) {
_last_health_and_arming_check = now;
perf_begin(_preflight_check_perf);
_health_and_arming_checks.update();
bool pre_flight_checks_pass = _health_and_arming_checks.canArm(_vehicle_status.nav_state);
if (_vehicle_status.pre_flight_checks_pass != pre_flight_checks_pass) {
_vehicle_status.pre_flight_checks_pass = pre_flight_checks_pass;
_status_changed = true;
}
perf_end(_preflight_check_perf);
checkAndInformReadyForTakeoff();
// Arm automatically on boot once preflight checks pass
// _arm_on_boot_done prevents re-arming after disarming
const bool should_arm_on_boot = _arm_on_boot_requested
&& !_arm_on_boot_done
&& !isArmed()
&& hrt_elapsed_time(&_boot_timestamp) > 5_s
&& pre_flight_checks_pass;
if (should_arm_on_boot) {
if (arm(arm_disarm_reason_t::mission_start, false) != TRANSITION_DENIED) {
_arm_on_boot_done = true;
}
}
}
// handle commands last, as the system needs to be updated to handle them
handleCommandsFromModeExecutors();
if (_vehicle_command_sub.updated()) {
// got command
const unsigned last_generation = _vehicle_command_sub.get_last_generation();
vehicle_command_s cmd;
if (_vehicle_command_sub.copy(&cmd)) {
if (_vehicle_command_sub.get_last_generation() != last_generation + 1) {
PX4_ERR("vehicle_command lost, generation %u -> %u", last_generation, _vehicle_command_sub.get_last_generation());
}
if (handle_command(cmd)) {
_status_changed = true;
}
}
}
if (_action_request_sub.updated()) {
const unsigned last_generation = _action_request_sub.get_last_generation();
action_request_s action_request;
if (_action_request_sub.copy(&action_request)) {
if (_action_request_sub.get_last_generation() != last_generation + 1) {
PX4_ERR("action_request lost, generation %u -> %u", last_generation, _action_request_sub.get_last_generation());
}
executeActionRequest(action_request);
}
}
// update actuator_armed
_actuator_armed.armed = isArmed();
_actuator_armed.prearmed = getPrearmState();
_actuator_armed.ready_to_arm = _vehicle_status.pre_flight_checks_pass || isArmed();
_actuator_armed.lockdown = _multicopter_throw_launch.isThrowLaunchInProgress();
// _actuator_armed.kill // action_request_s::ACTION_KILL
_actuator_armed.termination = (_vehicle_status.nav_state == _vehicle_status.NAVIGATION_STATE_TERMINATION);
// _actuator_armed.in_esc_calibration_mode // VEHICLE_CMD_PREFLIGHT_CALIBRATION
// Send parachute command upon termination
if (!actuator_armed_prev.termination && _actuator_armed.termination && isArmed()) {
send_parachute_command();
}
// publish states (armed, control_mode, vehicle_status) at 2 Hz or immediately when changed
if ((now >= _vehicle_status.timestamp + 500_ms) || _status_changed || nav_state_or_failsafe_changed
|| !(_actuator_armed == actuator_armed_prev)) {
// publish actuator_armed first (used by output modules)
_actuator_armed.timestamp = hrt_absolute_time();
_actuator_armed_pub.publish(_actuator_armed);
// update and publish vehicle_control_mode
updateControlMode();
// vehicle_status publish (after prearm/preflight updates above)
_mode_management.getModeStatus(_vehicle_status.valid_nav_states_mask, _vehicle_status.can_set_nav_states_mask);
_vehicle_status.accepts_offboard_setpoints = _mode_management.currentModeAcceptsOffboardSetpoints(_vehicle_status.nav_state);
_vehicle_status.timestamp = hrt_absolute_time();
_vehicle_status_pub.publish(_vehicle_status);
_failure_detector.publishStatus(_health_and_arming_checks.getEscArmStatus(), _health_and_arming_checks.getMotorFailureMask());
}
checkWorkerThread();
updateTunes();
control_status_leds(_status_changed, _battery_warning);
_status_changed = false;
arm_auth_update(hrt_absolute_time(), params_updated);
px4_indicate_external_reset_lockout(LockoutComponent::Commander, isArmed());
perf_end(_loop_perf);
// Always sleep during calibration to avoid competing with calibration
// worker threads for CPU. Otherwise, sleep only when idle.
if (_vehicle_status.calibration_enabled
|| (!_vehicle_command_sub.updated() && !_action_request_sub.updated())) {
px4_usleep(COMMANDER_MONITORING_INTERVAL);
}
}
rgbled_set_color_and_mode(led_control_s::COLOR_WHITE, led_control_s::MODE_OFF);
/* close fds */
led_deinit();
buzzer_deinit();
}
void Commander::checkForMissionUpdate()
{
if (_mission_result_sub.updated()) {
const mission_result_s &mission_result = _mission_result_sub.get();
const uint32_t prev_mission_mission_id = mission_result.mission_id;
_mission_result_sub.update();
// if mission_result is valid for the current mission
const bool mission_result_ok = (mission_result.timestamp > _boot_timestamp)
&& (mission_result.mission_id > 0);
bool auto_mission_available = mission_result_ok && mission_result.valid;
if (mission_result_ok) {
/* Only evaluate mission state if home is set */
if (!_failsafe_flags.home_position_invalid &&
(prev_mission_mission_id != mission_result.mission_id)) {
if (!auto_mission_available) {
/* the mission is invalid */
tune_mission_fail(true);
} else if (mission_result.warning) {
/* the mission has a warning */
tune_mission_warn(true);
} else {
/* the mission is valid */
tune_mission_ok(true);
}
}
}
if (isArmed() && !_vehicle_land_detected.landed
&& (mission_result.timestamp >= _vehicle_status.nav_state_timestamp)
&& mission_result.finished
&& _mode_management.modeExecutorInCharge() == ModeExecutors::AUTOPILOT_EXECUTOR_ID) {
if (_vehicle_status.nav_state == vehicle_status_s::NAVIGATION_STATE_AUTO_TAKEOFF
|| _vehicle_status.nav_state == vehicle_status_s::NAVIGATION_STATE_AUTO_VTOL_TAKEOFF) {
// Transition to loiter when the takeoff is completed (force into the Loiter, if mode is not executable then failsafe).
_user_mode_intention.change(vehicle_status_s::NAVIGATION_STATE_AUTO_LOITER, ModeChangeSource::ModeExecutor, false, true);
} else if (_vehicle_status.nav_state == vehicle_status_s::NAVIGATION_STATE_AUTO_MISSION) {
// Transition to loiter when the mission is cleared and/or finished, and we are still in mission mode.
// However, only do so if there's no pending mode change, so there isn't already a pending change (like Return).
if (_user_mode_intention.get() == vehicle_status_s::NAVIGATION_STATE_AUTO_MISSION) {
_user_mode_intention.change(vehicle_status_s::NAVIGATION_STATE_AUTO_LOITER);
}
}
}
}
}
bool Commander::getPrearmState() const
{
if (_vehicle_status.calibration_enabled) {
return false;
}
switch ((PrearmedMode)_param_com_prearm_mode.get()) {
case PrearmedMode::DISABLED:
/* skip prearmed state */
return false;
case PrearmedMode::ALWAYS:
/* Always go into prearmed state
* (all output drivers should be started / unlocked last in the boot process
* when the rest of the system is fully initialized)
*/
return hrt_elapsed_time(&_boot_timestamp) > 5_s;
case PrearmedMode::WHEN_SAFETY_OFF:
/* prearmed state matches safety_off state */
return _safety.isSafetyOff();
}
return false;
}
void Commander::handlePowerButtonState()
{
#if defined(BOARD_HAS_POWER_CONTROL)
/* handle power button state */
if (_power_button_state_sub.updated()) {
power_button_state_s button_state;
if (_power_button_state_sub.copy(&button_state)) {
if (button_state.event == power_button_state_s::PWR_BUTTON_STATE_REQUEST_SHUTDOWN) {
if (!isArmed() && (px4_shutdown_request() == 0)) {
while (1) { px4_usleep(1); }
}
}
}
}
#endif // BOARD_HAS_POWER_CONTROL
}
void Commander::systemPowerUpdate()
{
system_power_s system_power;
if (_system_power_sub.update(&system_power)) {
if (hrt_elapsed_time(&system_power.timestamp) < 1_s) {
if (system_power.servo_valid &&
!system_power.brick_valid &&
!system_power.usb_connected) {
/* flying only on servo rail, this is unsafe */
_vehicle_status.power_input_valid = false;
} else {
_vehicle_status.power_input_valid = true;
}
}
}
}
void Commander::landDetectorUpdate()
{
if (_vehicle_land_detected_sub.updated()) {
const bool was_landed = _vehicle_land_detected.landed;
_vehicle_land_detected_sub.copy(&_vehicle_land_detected);
// Only take actions if armed
if (isArmed()) {
if (!was_landed && _vehicle_land_detected.landed) {
mavlink_log_info(&_mavlink_log_pub, "Landing detected\t");
events::send(events::ID("commander_landing_detected"), events::Log::Info, "Landing detected");
} else if (was_landed && !_vehicle_land_detected.landed) {
mavlink_log_info(&_mavlink_log_pub, "Takeoff detected\t");
events::send(events::ID("commander_takeoff_detected"), events::Log::Info, "Takeoff detected");
_vehicle_status.takeoff_time = hrt_absolute_time();
_have_taken_off_since_arming = true;
_home_position.setTakeoffTime(_vehicle_status.takeoff_time);
}
// automatically set or update home position
if (_param_com_home_en.get() && !_mission_in_progress && !_config_overrides.disable_auto_set_home) {
// set the home position when taking off
if (!_vehicle_land_detected.landed) {
if (was_landed) {
_home_position.setHomePosition();
} else if (_param_com_home_in_air.get()) {
_home_position.setInAirHomePosition();
}
}
}
}
}
}
void Commander::safetyButtonUpdate()
{
const bool safety_changed = _safety.safetyButtonHandler();
_vehicle_status.safety_button_available = _safety.isButtonAvailable();
_vehicle_status.safety_off = _safety.isSafetyOff();
if (safety_changed) {
// Notify the user if the status of the safety button changes
if (!_safety.isSafetyDisabled()) {
if (_safety.isSafetyOff()) {
set_tune(tune_control_s::TUNE_ID_NOTIFY_POSITIVE);
} else {
tune_neutral(true);
}
}
_status_changed = true;
}
}
void Commander::vtolStatusUpdate()
{
// Make sure that this is only adjusted if vehicle really is of type vtol
if (_vtol_vehicle_status_sub.update(&_vtol_vehicle_status) && is_vtol(_vehicle_status)) {
// Check if there has been any change while updating the flags (transition = rotary wing status)
const uint8_t new_vehicle_type =
_vtol_vehicle_status.vehicle_vtol_state == vtol_vehicle_status_s::VEHICLE_VTOL_STATE_FW ?
vehicle_status_s::VEHICLE_TYPE_FIXED_WING :
vehicle_status_s::VEHICLE_TYPE_ROTARY_WING;
if (new_vehicle_type != _vehicle_status.vehicle_type) {
_vehicle_status.vehicle_type = new_vehicle_type;
_status_changed = true;
}
const bool new_in_transition = _vtol_vehicle_status.vehicle_vtol_state ==
vtol_vehicle_status_s::VEHICLE_VTOL_STATE_TRANSITION_TO_FW
|| _vtol_vehicle_status.vehicle_vtol_state == vtol_vehicle_status_s::VEHICLE_VTOL_STATE_TRANSITION_TO_MC;
if (_vehicle_status.in_transition_mode != new_in_transition) {
_vehicle_status.in_transition_mode = new_in_transition;
_status_changed = true;
}
if (_vehicle_status.in_transition_to_fw != (_vtol_vehicle_status.vehicle_vtol_state ==
vtol_vehicle_status_s::VEHICLE_VTOL_STATE_TRANSITION_TO_FW)) {
_vehicle_status.in_transition_to_fw = (_vtol_vehicle_status.vehicle_vtol_state ==
vtol_vehicle_status_s::VEHICLE_VTOL_STATE_TRANSITION_TO_FW);
_status_changed = true;
}
}
}
void Commander::updateTunes()
{
// play arming and battery warning tunes
if (!_arm_tune_played && isArmed()) {
/* play tune when armed */
set_tune(tune_control_s::TUNE_ID_ARMING_WARNING);
_arm_tune_played = true;
} else if (!_vehicle_status.usb_connected &&
(_vehicle_status.hil_state != vehicle_status_s::HIL_STATE_ON) &&
(_battery_warning == battery_status_s::WARNING_CRITICAL)) {
/* play tune on battery critical */
set_tune(tune_control_s::TUNE_ID_BATTERY_WARNING_FAST);
} else if ((_vehicle_status.hil_state != vehicle_status_s::HIL_STATE_ON) &&
(_battery_warning == battery_status_s::WARNING_LOW)) {
/* play tune on battery warning */
set_tune(tune_control_s::TUNE_ID_BATTERY_WARNING_SLOW);
} else if (_vehicle_status.failsafe && isArmed()) {
tune_failsafe(true);
} else if (_multicopter_throw_launch.isReadyToThrow()) {
set_tune(tune_control_s::TUNE_ID_ARMING_WARNING);
} else {
set_tune(tune_control_s::TUNE_ID_STOP);
}
/* reset arm_tune_played when disarmed */
if (!isArmed()) {
// Notify the user that it is safe to approach the vehicle
if (_arm_tune_played) {
tune_neutral(true);
}
_arm_tune_played = false;
}
}
void Commander::checkWorkerThread()
{
// check if the worker has finished
if (_worker_thread.hasResult()) {
int ret = _worker_thread.getResultAndReset();
_actuator_armed.in_esc_calibration_mode = false;
if (_vehicle_status.calibration_enabled) { // did we do a calibration?
_vehicle_status.calibration_enabled = false;
if (ret == 0) {
tune_positive(true);
} else {
tune_negative(true);
}
}
}
}
void Commander::handleAutoDisarm()
{
// Auto disarm when landed or kill switch engaged
if (isArmed()) {
// Check for auto-disarm on landing or pre-flight
if (_param_com_disarm_land.get() > 0 || _param_com_disarm_prflt.get() > 0) {
const bool auto_disarm_land_enabled = _param_com_disarm_land.get() > 0 && !_mission_in_progress
&& !_config_overrides.disable_auto_disarm;
if (auto_disarm_land_enabled && _have_taken_off_since_arming) {
_auto_disarm_landed.set_hysteresis_time_from(false, _param_com_disarm_land.get() * 1_s);
_auto_disarm_landed.set_state_and_update(_vehicle_land_detected.landed, hrt_absolute_time());
} else if (_param_com_disarm_prflt.get() > 0 && !_have_taken_off_since_arming) {
_auto_disarm_landed.set_hysteresis_time_from(false,
(_param_com_spoolup_time.get() + _param_com_disarm_prflt.get()) * 1_s);
_auto_disarm_landed.set_state_and_update(true, hrt_absolute_time());
}
if (_auto_disarm_landed.get_state() && !_multicopter_throw_launch.isThrowLaunchInProgress()) {
if (_have_taken_off_since_arming) {
disarm(arm_disarm_reason_t::landing);
} else {
disarm(arm_disarm_reason_t::preflight_inaction);
}
}
}
// Auto disarm after 5 seconds if kill switch is engaged
_auto_disarm_killed.set_state_and_update(_actuator_armed.kill, hrt_absolute_time());
if (_auto_disarm_killed.get_state()) {
disarm(arm_disarm_reason_t::kill_switch, true);
}
} else {
_auto_disarm_landed.set_state_and_update(false, hrt_absolute_time());
_auto_disarm_killed.set_state_and_update(false, hrt_absolute_time());
}
}
bool Commander::handleModeIntentionAndFailsafe()
{
const uint8_t prev_nav_state = _vehicle_status.nav_state;
const FailsafeBase::Action prev_failsafe_action = _failsafe.selectedAction();
const uint8_t prev_failsafe_defer_state = _vehicle_status.failsafe_defer_state;
FailsafeBase::State state{};
state.armed = isArmed();
state.vtol_in_transition_mode = _vehicle_status.in_transition_mode;
state.mission_finished = _mission_result_sub.get().finished;
state.user_intended_mode = _user_mode_intention.get();
state.vehicle_type = _vehicle_status.vehicle_type;
// There might have been a mode change request without changing the user intended mode.
// If a failsafe is active we must pass the request along as it might lead to a user-takeover.
bool mode_change_requested = _user_mode_intention.getHadModeChangeAndClear();
uint8_t updated_user_intented_mode = _failsafe.update(hrt_absolute_time(), state, mode_change_requested,
_failsafe_user_override_request,
_failsafe_flags);
_failsafe_user_override_request = false;
// Force intended mode if changed by the failsafe state machine
if (state.user_intended_mode != updated_user_intented_mode) {
_user_mode_intention.change(updated_user_intented_mode, ModeChangeSource::User, false, true);
_user_mode_intention.getHadModeChangeAndClear();
}
// Handle failsafe action
_mode_management.setFailsafeState(_failsafe.selectedAction() > FailsafeBase::Action::Warn);
_vehicle_status.nav_state_user_intention = _mode_management.getNavStateReplacementIfValid(_user_mode_intention.get(),
false);
_vehicle_status.nav_state =
_mode_management.getNavStateReplacementIfValid(FailsafeBase::modeFromAction(_failsafe.selectedAction(), _user_mode_intention.get()));
_vehicle_status.executor_in_charge = _mode_management.modeExecutorInCharge(); // Set this in sync with nav_state
_vehicle_status.nav_state_display = _mode_management.getNavStateDisplay(_vehicle_status.nav_state);
switch (_failsafe.selectedAction()) {
case FailsafeBase::Action::Disarm:
disarm(arm_disarm_reason_t::failsafe, true);
break;
case FailsafeBase::Action::Terminate:
_vehicle_status.nav_state = _vehicle_status.NAVIGATION_STATE_TERMINATION;
break;
default:
break;
}
_vehicle_status.failsafe = _failsafe.inFailsafe();
_vehicle_status.failsafe_and_user_took_over = _failsafe.userTakeoverActive();
if (prev_nav_state != _vehicle_status.nav_state) {
_vehicle_status.nav_state_timestamp = hrt_absolute_time();
}
_mode_management.updateActiveConfigOverrides(_vehicle_status.nav_state, _config_overrides);
// Apply failsafe deferring & get the current state
_failsafe.deferFailsafes(_config_overrides.defer_failsafes, _config_overrides.defer_failsafes_timeout_s);
if (_failsafe.failsafeDeferred()) {
_vehicle_status.failsafe_defer_state = vehicle_status_s::FAILSAFE_DEFER_STATE_WOULD_FAILSAFE;
} else if (_failsafe.getDeferFailsafes()) {
_vehicle_status.failsafe_defer_state = vehicle_status_s::FAILSAFE_DEFER_STATE_ENABLED;
} else {
_vehicle_status.failsafe_defer_state = vehicle_status_s::FAILSAFE_DEFER_STATE_DISABLED;
}
return prev_nav_state != _vehicle_status.nav_state ||
prev_failsafe_action != _failsafe.selectedAction() ||
prev_failsafe_defer_state != _vehicle_status.failsafe_defer_state;
}
void Commander::checkAndInformReadyForTakeoff()
{
#ifdef CONFIG_ARCH_BOARD_PX4_SITL
static bool ready_for_takeoff_printed = false;
if (_vehicle_status.vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING ||
_vehicle_status.vehicle_type == vehicle_status_s::VEHICLE_TYPE_FIXED_WING) {
if (!ready_for_takeoff_printed &&
_health_and_arming_checks.canArm(vehicle_status_s::NAVIGATION_STATE_AUTO_TAKEOFF)) {
PX4_INFO("%sReady for takeoff!%s", PX4_ANSI_COLOR_GREEN, PX4_ANSI_COLOR_RESET);
ready_for_takeoff_printed = true;
}
}
#endif // CONFIG_ARCH_BOARD_PX4_SITL
}
void Commander::modeManagementUpdate()
{
ModeManagement::UpdateRequest mode_management_update{};
_mode_management.update(_vehicle_status.vehicle_type, _vehicle_status.is_vtol, isArmed(),
_vehicle_status.nav_state_user_intention, mode_management_update);
if (!isArmed() && mode_management_update.change_user_intended_nav_state) {
_user_mode_intention.change(mode_management_update.user_intended_nav_state);
}
if (mode_management_update.control_setpoint_update) {
_status_changed = true;
}
}
void Commander::control_status_leds(bool changed, const uint8_t battery_warning)
{
switch (blink_msg_state()) {
case 1:
// blinking LED message, don't touch LEDs
return;
case 2:
// blinking LED message completed, restore normal state
changed = true;
break;
default:
break;
}
const hrt_abstime time_now_us = hrt_absolute_time();
if (_cpuload_sub.updated()) {
cpuload_s cpuload;
if (_cpuload_sub.copy(&cpuload)) {
const float cpuload_percent = cpuload.load * 100.f;
bool overload = false;
// Only check CPU load if it hasn't been disabled
if (!(_param_com_cpu_max.get() < FLT_EPSILON)) {
overload = (cpuload_percent > _param_com_cpu_max.get());
}
overload = overload || (cpuload.ram_usage > 0.99f);
if (_overload_start == 0 && overload) {
_overload_start = time_now_us;
} else if (!overload) {
_overload_start = 0;
}
}
}
const bool overload = (_overload_start != 0);
// driving the RGB led
if (changed || _last_overload != overload) {
uint8_t led_mode = led_control_s::MODE_OFF;
uint8_t led_color = led_control_s::COLOR_WHITE;
bool set_normal_color = false;
uint64_t overload_warn_delay = isArmed() ? 1_ms : 250_ms;
// set mode
if (overload && (time_now_us >= _overload_start + overload_warn_delay)) {
led_mode = led_control_s::MODE_BLINK_FAST;
led_color = led_control_s::COLOR_PURPLE;
} else if (_multicopter_throw_launch.isReadyToThrow()) {
led_mode = led_control_s::MODE_BLINK_FAST;
led_color = led_control_s::COLOR_YELLOW;
} else if (isArmed()) {
led_mode = led_control_s::MODE_ON;
set_normal_color = true;
} else if (!_vehicle_status.pre_flight_checks_pass) {
led_mode = led_control_s::MODE_BLINK_FAST;
led_color = led_control_s::COLOR_RED;
} else {
led_mode = led_control_s::MODE_BREATHE;
set_normal_color = true;
}
if (set_normal_color) {
// set color
if (_vehicle_status.failsafe) {
led_color = led_control_s::COLOR_PURPLE;
} else if (battery_warning == battery_status_s::WARNING_LOW) {
led_color = led_control_s::COLOR_AMBER;
} else if (battery_warning == battery_status_s::WARNING_CRITICAL) {
led_color = led_control_s::COLOR_RED;
} else {
if (!_failsafe_flags.home_position_invalid && !_failsafe_flags.global_position_invalid) {
led_color = led_control_s::COLOR_GREEN;
} else {
led_color = led_control_s::COLOR_BLUE;
}
}
}
if (led_mode != led_control_s::MODE_OFF) {
rgbled_set_color_and_mode(led_color, led_mode);
}
}
_last_overload = overload;
#if !defined(CONFIG_ARCH_LEDS) && defined(BOARD_HAS_CONTROL_STATUS_LEDS)
if (isArmed()) {
if (_vehicle_status.failsafe) {
BOARD_ARMED_LED_OFF();
if (time_now_us >= _led_armed_state_toggle + 250_ms) {
_led_armed_state_toggle = time_now_us;
BOARD_ARMED_STATE_LED_TOGGLE();
}
} else {
BOARD_ARMED_STATE_LED_OFF();
// armed, solid
BOARD_ARMED_LED_ON();
}
} else if (_vehicle_status.pre_flight_checks_pass) {
BOARD_ARMED_LED_OFF();
// ready to arm, blink at 1Hz
if (time_now_us >= _led_armed_state_toggle + 1_s) {
_led_armed_state_toggle = time_now_us;
BOARD_ARMED_STATE_LED_TOGGLE();
}
} else {
BOARD_ARMED_LED_OFF();
// not ready to arm, blink at 10Hz
if (time_now_us >= _led_armed_state_toggle + 100_ms) {
_led_armed_state_toggle = time_now_us;
BOARD_ARMED_STATE_LED_TOGGLE();
}
}
#endif
// give system warnings on error LED
if (overload) {
if (time_now_us >= _led_overload_toggle + 50_ms) {
_led_overload_toggle = time_now_us;
BOARD_OVERLOAD_LED_TOGGLE();
}
} else {
BOARD_OVERLOAD_LED_OFF();
}
}
void Commander::updateControlMode()
{
_vehicle_control_mode = {};
const auto external_mode_setpoint_type = _mode_management.getSetpointType(_vehicle_status.nav_state);
mode_util::getVehicleControlMode(_vehicle_status.nav_state,
_vehicle_status.vehicle_type, _offboard_control_mode_sub.get(), _vehicle_control_mode, external_mode_setpoint_type);
_vehicle_control_mode.flag_armed = isArmed();
_vehicle_control_mode.flag_multicopter_position_control_enabled =
(_vehicle_status.vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING)
&& (_vehicle_control_mode.flag_control_altitude_enabled
|| _vehicle_control_mode.flag_control_climb_rate_enabled
|| _vehicle_control_mode.flag_control_position_enabled
|| _vehicle_control_mode.flag_control_velocity_enabled);
_vehicle_control_mode.timestamp = hrt_absolute_time();
_vehicle_control_mode_pub.publish(_vehicle_control_mode);
}
void Commander::printRejectMode(uint8_t nav_state)
{
if (hrt_elapsed_time(&_last_print_mode_reject_time) > 1_s) {
mavlink_log_critical(&_mavlink_log_pub, "Switching to %s is currently not available\t",
mode_util::nav_state_names[nav_state]);
px4_custom_mode custom_mode = get_px4_custom_mode(nav_state);
uint32_t mavlink_mode = custom_mode.data;
/* EVENT
* @type append_health_and_arming_messages
*/
events::send<uint32_t, events::px4::enums::navigation_mode_t>(events::ID("commander_modeswitch_not_avail"), {events::Log::Critical, events::LogInternal::Info},
"Switching to mode '{2}' is currently not possible", mavlink_mode, mode_util::navigation_mode(nav_state));
/* only buzz if armed, because else we're driving people nuts indoors
they really need to look at the leds as well. */
tune_negative(isArmed());
_last_print_mode_reject_time = hrt_absolute_time();
}
}
void Commander::answer_command(const vehicle_command_s &cmd, uint8_t result)
{
switch (result) {
case vehicle_command_ack_s::VEHICLE_CMD_RESULT_ACCEPTED:
break;
case vehicle_command_ack_s::VEHICLE_CMD_RESULT_DENIED:
PX4_DEBUG("command %" PRIu32 " denied", cmd.command);
tune_negative(true);
break;
case vehicle_command_ack_s::VEHICLE_CMD_RESULT_FAILED:
PX4_DEBUG("command %" PRIu32 " failed", cmd.command);
tune_negative(true);
break;
case vehicle_command_ack_s::VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED:
PX4_DEBUG("command %" PRIu32 " temporarily rejected", cmd.command);
tune_negative(true);
break;
case vehicle_command_ack_s::VEHICLE_CMD_RESULT_UNSUPPORTED:
PX4_WARN("command %" PRIu32 " unsupported", cmd.command);
tune_negative(true);
break;
default:
PX4_ERR("command %" PRIu32 " invalid result %d", cmd.command, result);
return;
}
/* publish ACK */
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();
_vehicle_command_ack_pub.publish(command_ack);
}
int Commander::run_trampoline(int argc, char *argv[])
{
return ModuleBase::run_trampoline_impl(desc, [](int ac, char *av[]) -> ModuleBase * {
return Commander::instantiate(ac, av);
}, argc, argv);
}
int Commander::task_spawn(int argc, char *argv[])
{
desc.task_id = px4_task_spawn_cmd("commander",
SCHED_DEFAULT,
SCHED_PRIORITY_DEFAULT + 40,
PX4_STACK_ADJUSTED(3250),
(px4_main_t)&run_trampoline,
(char *const *)argv);
if (desc.task_id < 0) {
desc.task_id = -1;
return -errno;
}
// wait until task is up & running
if (wait_until_running(desc) < 0) {
desc.task_id = -1;
return -1;
}
return 0;
}
Commander *Commander::instantiate(int argc, char *argv[])
{
Commander *instance = new Commander();
if (instance) {
if (argc >= 2 && !strcmp(argv[1], "-h")) {
instance->enable_hil();
}
}
return instance;
}
void Commander::enable_hil()
{
_vehicle_status.hil_state = vehicle_status_s::HIL_STATE_ON;
}
void Commander::dataLinkCheck()
{
// high latency data link
iridiumsbd_status_s iridium_status;
if (_iridiumsbd_status_sub.update(&iridium_status)) {
_high_latency_datalink_timestamp = iridium_status.last_at_ok_timestamp;
if (_vehicle_status.high_latency_data_link_lost &&
(_high_latency_datalink_timestamp > _high_latency_datalink_lost) &&
(_high_latency_datalink_regained == 0)
) {
_high_latency_datalink_regained = _high_latency_datalink_timestamp;
}
if (_vehicle_status.high_latency_data_link_lost && (_high_latency_datalink_regained != 0)) {
_vehicle_status.high_latency_data_link_lost = false;
_status_changed = true;
}
}
for (auto &telemetry_status : _telemetry_status_subs) {
telemetry_status_s telemetry;
if (telemetry_status.update(&telemetry)) {
// handle different radio types
switch (telemetry.type) {
case telemetry_status_s::LINK_TYPE_USB:
// set (but don't unset) telemetry via USB as active once a MAVLink connection is up
_vehicle_status.usb_connected = true;
break;
case telemetry_status_s::LINK_TYPE_IRIDIUM: {
if ((_high_latency_datalink_timestamp > 0) &&
(hrt_elapsed_time(&_high_latency_datalink_timestamp) > (_param_com_hldl_loss_t.get() * 1_s))) {
_high_latency_datalink_lost = _high_latency_datalink_timestamp;
_high_latency_datalink_regained = 0;
if (!_vehicle_status.high_latency_data_link_lost) {
_vehicle_status.high_latency_data_link_lost = true;
mavlink_log_critical(&_mavlink_log_pub, "High latency data link lost\t");
events::send(events::ID("commander_high_latency_lost"), events::Log::Critical, "High latency data link lost");
_status_changed = true;
}
}
break;
}
}
if (telemetry.heartbeat_type_gcs) {
// Initial connection or recovery from data link lost
if (_vehicle_status.gcs_connection_lost) {
_vehicle_status.gcs_connection_lost = false;
_status_changed = true;
if (_datalink_last_heartbeat_gcs != 0) {
mavlink_log_info(&_mavlink_log_pub, "GCS connection regained\t");
events::send(events::ID("commander_dl_regained"), events::Log::Info, "GCS connection regained");
}
}
_datalink_last_heartbeat_gcs = telemetry.timestamp;
}
if (telemetry.heartbeat_type_onboard_controller) {
if (_onboard_controller_lost) {
_onboard_controller_lost = false;
_status_changed = true;
if (_datalink_last_heartbeat_onboard_controller != 0) {
mavlink_log_info(&_mavlink_log_pub, "Onboard controller regained\t");
events::send(events::ID("commander_onboard_ctrl_regained"), events::Log::Info, "Onboard controller regained");
}
}
_datalink_last_heartbeat_onboard_controller = telemetry.timestamp;
}
if (telemetry.heartbeat_type_parachute) {
if (_parachute_system_lost) {
_parachute_system_lost = false;
if (_datalink_last_heartbeat_parachute_system != 0) {
mavlink_log_info(&_mavlink_log_pub, "Parachute system regained\t");
events::send(events::ID("commander_parachute_regained"), events::Log::Info, "Parachute system regained");
}
}
bool healthy = telemetry.parachute_system_healthy;
_datalink_last_heartbeat_parachute_system = telemetry.timestamp;
_vehicle_status.parachute_system_present = true;
_vehicle_status.parachute_system_healthy = healthy;
}
if (telemetry.heartbeat_type_open_drone_id) {
if (_open_drone_id_system_lost) {
_open_drone_id_system_lost = false;
if (_datalink_last_heartbeat_open_drone_id_system != 0) {
mavlink_log_info(&_mavlink_log_pub, "Remote ID system regained\t");
events::send(events::ID("commander_open_drone_id_regained"), events::Log::Info, "Remote ID system regained");
}
}
bool healthy = telemetry.open_drone_id_system_healthy;
_datalink_last_heartbeat_open_drone_id_system = telemetry.timestamp;
_vehicle_status.open_drone_id_system_present = true;
_vehicle_status.open_drone_id_system_healthy = healthy;
}
// Traffic avoidance system (ADSB or FLARM)
if (telemetry.heartbeat_type_adsb || telemetry.heartbeat_type_flarm) {
if (_traffic_avoidance_system_lost) { // recovered
_traffic_avoidance_system_lost = false;
if (_datalink_last_heartbeat_traffic_avoidance_system != 0) {
mavlink_log_info(&_mavlink_log_pub, "Traffic avoidance system regained\t");
events::send(events::ID("commander_traffic_avoidance_regained"), events::Log::Info,
"Traffic avoidance system regained");
}
}
_datalink_last_heartbeat_traffic_avoidance_system = telemetry.timestamp;
_vehicle_status.traffic_avoidance_system_present = true;
}
}
}
// GCS data link loss failsafe
if (!_vehicle_status.gcs_connection_lost) {
if ((_datalink_last_heartbeat_gcs != 0)
&& hrt_elapsed_time(&_datalink_last_heartbeat_gcs) > (_param_com_dl_loss_t.get() * 1_s)) {
_vehicle_status.gcs_connection_lost = true;
_vehicle_status.gcs_connection_lost_counter++;
mavlink_log_info(&_mavlink_log_pub, "Connection to ground station lost\t");
events::send(events::ID("commander_gcs_lost"), {events::Log::Warning, events::LogInternal::Info},
"Connection to ground control station lost");
_status_changed = true;
}
}
// ONBOARD CONTROLLER data link loss failsafe
if ((_datalink_last_heartbeat_onboard_controller > 0)
&& (hrt_elapsed_time(&_datalink_last_heartbeat_onboard_controller) > 5_s)
&& !_onboard_controller_lost) {
mavlink_log_critical(&_mavlink_log_pub, "Connection to mission computer lost\t");
events::send(events::ID("commander_mission_comp_lost"), events::Log::Critical, "Connection to mission computer lost");
_onboard_controller_lost = true;
_status_changed = true;
}
// Parachute system
if ((hrt_elapsed_time(&_datalink_last_heartbeat_parachute_system) > 3_s)
&& !_parachute_system_lost) {
mavlink_log_critical(&_mavlink_log_pub, "Parachute system lost\t");
_vehicle_status.parachute_system_present = false;
_vehicle_status.parachute_system_healthy = false;
_parachute_system_lost = true;
_status_changed = true;
}
// Remote ID system
if ((hrt_elapsed_time(&_datalink_last_heartbeat_open_drone_id_system) > 3_s)
&& !_open_drone_id_system_lost) {
mavlink_log_critical(&_mavlink_log_pub, "Remote ID system lost\t");
events::send(events::ID("commander_remote_id_lost"), events::Log::Critical, "Remote ID system lost");
_vehicle_status.open_drone_id_system_present = false;
_vehicle_status.open_drone_id_system_healthy = false;
_open_drone_id_system_lost = true;
_status_changed = true;
}
// Traffic avoidance system (ADSB/FLARM)
if ((hrt_elapsed_time(&_datalink_last_heartbeat_traffic_avoidance_system) > 3_s)
&& !_traffic_avoidance_system_lost) {
mavlink_log_critical(&_mavlink_log_pub, "Traffic avoidance system lost\t");
events::send(events::ID("commander_traffic_avoidance_lost"), events::Log::Critical, "Traffic avoidance system lost");
_vehicle_status.traffic_avoidance_system_present = false;
_traffic_avoidance_system_lost = true;
_status_changed = true;
}
}
void Commander::battery_status_check()
{
// Handle shutdown request from emergency battery action
if (_battery_warning != _failsafe_flags.battery_warning) {
if (_failsafe_flags.battery_warning == battery_status_s::WARNING_EMERGENCY) {
#if defined(BOARD_HAS_POWER_CONTROL)
if (!isArmed() && (px4_shutdown_request(60_s) == 0)) {
mavlink_log_critical(&_mavlink_log_pub, "Dangerously low battery! Shutting system down in 60 seconds\t");
events::send(events::ID("commander_low_bat_shutdown"), {events::Log::Emergency, events::LogInternal::Warning},
"Dangerously low battery! Shutting system down");
while (1) { px4_usleep(1); }
} else {
mavlink_log_critical(&_mavlink_log_pub, "System does not support shutdown\t");
/* EVENT
* @description Cannot shut down, most likely the system does not support it.
*/
events::send(events::ID("commander_low_bat_shutdown_failed"), {events::Log::Emergency, events::LogInternal::Error},
"Dangerously low battery! System shut down failed");
}
#endif // BOARD_HAS_POWER_CONTROL
}
}
_battery_warning = _failsafe_flags.battery_warning;
}
void Commander::manualControlCheck()
{
manual_control_setpoint_s manual_control_setpoint;
const bool manual_control_updated = _manual_control_setpoint_sub.update(&manual_control_setpoint);
if (manual_control_updated && manual_control_setpoint.valid) {
_last_manual_throttle = manual_control_setpoint.throttle;
if (isArmed()) {
// Hand control back to the pilot when they override with the sticks (see MAN_OVERRIDE_SPD).
// Only applies to multicopters (incl. VTOLs in MC mode) in auto/offboard modes.
const bool rotary_wing = (_vehicle_status.vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING);
const bool overridable_mode = _vehicle_control_mode.flag_control_auto_enabled || _vehicle_control_mode.flag_control_offboard_enabled;
if (manual_control_setpoint.sticks_moving && rotary_wing && overridable_mode) {
// If no failsafe is active, directly change the mode, otherwise pass the request to the failsafe state machine
if (_failsafe.selectedAction() <= FailsafeBase::Action::Warn) {
if (_user_mode_intention.change(vehicle_status_s::NAVIGATION_STATE_POSCTL, ModeChangeSource::User, true)) {
tune_positive(true);
mavlink_log_info(&_mavlink_log_pub, "Pilot took over using sticks\t");
events::send(events::ID("commander_rc_override"), events::Log::Info, "Pilot took over using sticks");
}
} else {
_failsafe_user_override_request = true;
}
}
}
}
}
void Commander::manualControlLossModeSwitch()
{
// NAV_RCL_ACT value that switches to Hold as a regular mode change instead of triggering the failsafe.
// Kept in sync with gcs_connection_loss_failsafe_mode::Hold_mode_no_failsafe (private to the failsafe).
static constexpr int32_t NAV_RCL_ACT_HOLD_NO_FAILSAFE = 7;
const bool manual_control_lost = _failsafe_flags.manual_control_signal_lost;
// Only act on the moment manual control is lost while actively flying a manual mode. Using an edge avoids
// repeatedly overriding the pilot if they command a different mode while manual control stays lost.
if (manual_control_lost && !_manual_control_lost_prev
&& isArmed()
&& _param_nav_rcl_act.get() == NAV_RCL_ACT_HOLD_NO_FAILSAFE
&& _vehicle_control_mode.flag_control_manual_enabled) {
// Force the switch to Hold as a regular mode change (no failsafe, no alarming notification).
// force=true skips the mode availability check on purpose: if Hold cannot actually run (e.g. without a
// valid position estimate), the failsafe mode-fallback escalates from there (Hold -> Return -> Land/Descend/Terminate).
_user_mode_intention.change(vehicle_status_s::NAVIGATION_STATE_AUTO_LOITER, ModeChangeSource::User, false, true);
mavlink_log_info(&_mavlink_log_pub, "Manual control lost: switching to Hold\t");
events::send(events::ID("commander_rc_loss_hold_no_failsafe"), {events::Log::Info, events::LogInternal::Info},
"Manual control lost: switching to Hold");
}
_manual_control_lost_prev = manual_control_lost;
}
void Commander::offboardControlCheck()
{
if (_offboard_control_mode_sub.update()) {
if (_failsafe_flags.offboard_control_signal_lost) {
// Run arming checks immediately to allow for offboard mode activation
_status_changed = true;
}
}
}
void Commander::send_parachute_command()
{
vehicle_command_s vcmd{};
vcmd.command = vehicle_command_s::VEHICLE_CMD_DO_PARACHUTE;
vcmd.param1 = static_cast<float>(vehicle_command_s::PARACHUTE_ACTION_RELEASE);
vcmd.source_system = _vehicle_status.system_id;
vcmd.target_system = _vehicle_status.system_id;
vcmd.source_component = _vehicle_status.component_id;
vcmd.target_component = 161; // MAV_COMP_ID_PARACHUTE
uORB::Publication<vehicle_command_s> vcmd_pub{ORB_ID(vehicle_command)};
vcmd.timestamp = hrt_absolute_time();
vcmd_pub.publish(vcmd);
set_tune_override(tune_control_s::TUNE_ID_PARACHUTE_RELEASE);
}
void Commander::onFailsafeNotifyUserTrampoline(void *arg)
{
Commander *commander = static_cast<Commander *>(arg);
commander->onFailsafeNotifyUser();
}
void Commander::onFailsafeNotifyUser()
{
// If we are about to inform about a failsafe, we need to ensure any pending health report is sent out first,
// as the failsafe message might reference that. This is only needed in case the report is currently rate-limited,
// i.e. it had a recent previous change already.
_health_and_arming_checks.reportIfUnreportedDifferences();
}
int Commander::print_usage(const char *reason)
{
if (reason) {
PX4_INFO("%s", reason);
}
PRINT_MODULE_DESCRIPTION(
R"DESCR_STR(
### Description
The commander module contains the state machine for mode switching and failsafe behavior.
)DESCR_STR");
PRINT_MODULE_USAGE_NAME("commander", "system");
PRINT_MODULE_USAGE_COMMAND("start");
PRINT_MODULE_USAGE_PARAM_FLAG('h', "Enable HIL mode", true);
#ifndef CONSTRAINED_FLASH
PRINT_MODULE_USAGE_COMMAND_DESCR("calibrate", "Run sensor calibration");
PRINT_MODULE_USAGE_ARG("mag|baro|accel|gyro|level|esc|airspeed", "Calibration type", false);
PRINT_MODULE_USAGE_ARG("quick", "Quick calibration [mag, accel (not recommended)]", false);
PRINT_MODULE_USAGE_COMMAND_DESCR("check", "Run preflight checks");
PRINT_MODULE_USAGE_COMMAND_DESCR("safety", "Change prearm safety state");
PRINT_MODULE_USAGE_ARG("on|off", "[on] to activate safety, [off] to deactivate safety and allow control surface movements", false);
PRINT_MODULE_USAGE_COMMAND_DESCR("actuator_group_test", "Drive a functional actuator group (torque/thrust/tilt) for a brief preflight check");
PRINT_MODULE_USAGE_ARG("roll|pitch|yaw|tilt|xthrust|ythrust|zthrust", "Group", false);
PRINT_MODULE_USAGE_ARG("value", "Normalized command [-1.0, +1.0]; default 1.0 for torque/tilt, 0.1 for thrust", true);
PRINT_MODULE_USAGE_COMMAND("arm");
PRINT_MODULE_USAGE_PARAM_FLAG('f', "Force arming (do not run preflight checks)", true);
PRINT_MODULE_USAGE_COMMAND("disarm");
PRINT_MODULE_USAGE_PARAM_FLAG('f', "Force disarming (disarm in air)", true);
PRINT_MODULE_USAGE_COMMAND("takeoff");
PRINT_MODULE_USAGE_COMMAND("land");
PRINT_MODULE_USAGE_COMMAND_DESCR("transition", "VTOL transition");
PRINT_MODULE_USAGE_COMMAND_DESCR("mode", "Change flight mode");
PRINT_MODULE_USAGE_ARG("manual|acro|offboard|stabilized|altctl|posctl|altitude_cruise|manual_parking|position:slow|auto:mission|auto:loiter|auto:course|auto:rtl|auto:takeoff|auto:land|auto:precland|ext1",
"Flight mode", false);
PRINT_MODULE_USAGE_COMMAND("pair");
PRINT_MODULE_USAGE_COMMAND("termination");
PRINT_MODULE_USAGE_ARG("on|off", "Turn lockdown on or off", false);
PRINT_MODULE_USAGE_COMMAND("set_ekf_origin");
PRINT_MODULE_USAGE_ARG("lat, lon, alt", "Origin Latitude, Longitude, Altitude", false);
PRINT_MODULE_USAGE_COMMAND_DESCR("lat|lon|alt", "Origin latitude longitude altitude");
PRINT_MODULE_USAGE_COMMAND_DESCR("set_heading", "Set current heading");
PRINT_MODULE_USAGE_ARG("heading", "degrees from True North [0 360]", false);
PRINT_MODULE_USAGE_COMMAND_DESCR("poweroff", "Power off board (if supported)");
#endif
PRINT_MODULE_USAGE_DEFAULT_COMMANDS();
return 1;
}