mirror of
https://gitee.com/mirrors_PX4/PX4-Autopilot.git
synced 2026-04-14 10:07:39 +08:00
2542b1bb26
This feature allows user to use a Gripper type pacakge delivery mechanism on a drone to trigger the delivery during a mission via the mission item `DO_GRIPPER`. This is a minimal change that is intended to have simplest pacakge delivery feature on PX4, however the future scope would extend this feature out of Navigator, and rather move towards a federated PX4 (flight-mode flexibility) architecture. But until then, this will serve the purpose. Update Tools/sitl_gazebo submodule to remove sdf file overwrite error - There was an error happening due to .sdf file being overwritten, it was caused by a wrongfully added. sdf file. - This update pulls in the PR commit: https://github.com/Auterion/sitl_gazebo/pull/147 Initial cut on supporing PAYLOAD_PLACE mission item Tidy and comment on navigation.h to clarify mission item definition - Convert vehicle command ack subscription data type to SubscriptionData, to not care about having a dedicated struct for copying the latest data - Tidy and comment on navigation.h to clarify the definition of mission_item_s, which is confusing as it is an intergration of MAVLink Standard into PX4's internal Mission Item structure Rename mission_block's mission item reached function & cleanup navigator - Isolated Handle Vehicle Commands function inside the Navigator - Rename mission_block's mission item reached function to 'reached or completed', as the navigation command can also be an action (e.g. DO_SET_SERVO, which doesn't make sense to refer to as 'reached' when we have successfully done executed the command) Include MAVLink PR commit to include payload_drop message More changes to add payload_drop MAVLink message support - Comitting for testing purposes Add mission item payload_drop to vehicle command payload drop link - Now with a mission item with the nav_cmd set to 'payload drop', the appropriate 'payload drop' vehicle command will be issued Make Payload drop executable via Mission Plan Implement payload_drop module to simulate payload delivery - Simple module that acknowledges the payload drop vehicle command after certain time, to simulate a successful delivery Additional changes - payload drop module not working yet - Need to do more thread stuff to make it work :( Fix Payload Drop enum mismatch in vehicle_command enums - First functional Payload Drop Implementation MVP - Simple Ack & resuming mission from Navigator tested successfully Hold the position while executing payload drop mission item - Still the position hold is not solid, maybe I am missing something in the position setpoint part and all the internal implications of Navigator :( Add DO_WINCH command support Some fixes after rebase on develop branch - Some missed brackets - Some comment edits, etc Add DO_WINCH command support - Still has a problem of flying away from the waypoint while the DO_WINCH is being executed, probably position setpoint related stuff :( Apply braking of the vehicle for DO_WINCH command - Copies the behavior of NAV_CMD_DELAY, which executes a smooth, braking behavior when executing the delay because of the braking condition in `set_mission_items` function - This will not apply to Fixed wings - The payload deploy getting triggered may be too early, as right now as soon as the vehicle approaches the waypoint within the acceptance threshold, the payload gets deployed Add DO_GRIPPER support Implement Gripper actual Hardware triggering support - Currently not working, possibly in the mixer there's a bug - Implemented the publishing of actuator_controls_1 uORB topic - Implemented the test command for the payload_drop module, to test the grpiper functionality - Edited px4board file to include the payload_drop module - Added Holybro X500 V2 airframe file, to enable testing on X500 V2 - Created new Quad X Payload Delivery mixer, which maps the actuator controls 1 topic's data into the MAIN pin 5 output Make Payload Drop Gripper Work - Initialization of the Gripper position to CLOSED on Constructor of the payload_drop module - Setting the OPEN and CLOSED value to the appropriate actuator controls input Set vehicle_command_ack message's timestamp correctly - By not setting the timestamp, the ack commands were not correctly graphed in PlotJuggler! Rename payload drop module to payload deliverer - I think it's a more complex name (harder to type), but more generic Add Gripper class (WIP) Add Gripper class functionalities - Add gripper uORB message - Add gripper state machine Use Gripper class as main interface in payload_deliverer - Utilizes Gripper class functions for doing Gripper functionality Remove mixer based package delivery trigger logic - Remove custom mixer files that mapped actuator controls to outputs statically Additional improvements of the payload_deliverer Fix payload_deliverer module not starting - _task_id wasn't geting set appropriately in task_spawn function, which led to runtime failure Add Gripper Function to mixer_module - Still not showing up as function mapping in QGC, needs fix Add parameters to control gripper behavior - Now user can enable / disable gripper - Also select which type of gripper to use Applying review from nuno Remove timeout fetching from mission item and use gripper's timeout - Previously, it was planned to use a custom DO_GRIPPER and DO_WINCH MAVLink message definitions with information on timeout, but since now we are using original message definition, only relevant timeout information is defined in the payload_deliverer class - This change brings in the timeout parameter to the Navigator, which then sets the timeout in the mission_block class level, which then processes the timeout logic Make payload deployment work for Allmend test :P Support gripper open/close test commands in payload_deliverer Move enum definition for GRIPPER_ACTION to vehicle_command.msg Remove double call for ` ${R}etc/init.d/rc.vehicle_setup` - Was introduced during the rebase - Was causing module already running & uORB topic can't be advertised errors Fix format via `make format` command Modify S500 airframe file to use for control allocation usage - Added Control allocation related parameters as default to not have it reset every time the airframe is selected Implement mission specific payload deploy timeout and more changes Switch payload_deliverer to run on work queue Remove unnecessary files - Airframe changes from enabling control allocation are removed Address review comments - Remove debug messages - Remove unnecessary or verbose comments & code - Properly call parameter_update() function Switch payload_deliverer to scheduled interval work item & refactor - Switch to Schedeuled on Interval Work Item, as previous vehicle command subscription callback based behavior led to vehicle comamnd ack not being sent accordingly (since the Run() wouldn't be called unless there's a new vehicle command), leading to ack command not being sent out - Also, old vehicle commands were getting fetched due to the subscription callback as well, which was removed with this patch - Fix the wrong population of floating point param2 field of vehicle command by int8_t type gripper action by creating dedicated function - Refactor and add comments to increase readability Add gripper::grabbing() method and handle this in parameter update - Previously, the intermediate state 'grabbing' was not considered, and when the parameter update was called after the first initialization of the gripper, the grab() function was being called again, which would produce unnecessary duplicate vehicle command. - Also replaced direct .grab() access to sending vehicle comamnd, which unifies the gripper actuation mechanism through vehicle commands. Navigator: Change SubscriptionData to Subscription to reduce memory usage - Also removed unused vehicle command ack sub PayloadDeliverer: Remove unnecessary changes & Bring back vehicle_command sub cb
3520 lines
124 KiB
C++
3520 lines
124 KiB
C++
/****************************************************************************
|
|
*
|
|
* Copyright (c) 2013-2022 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
|
|
*
|
|
* @TODO This application is currently in a rewrite process. Main changes:
|
|
* - Calibration routines are moved into the event system
|
|
* - Commander is rewritten as class
|
|
* - State machines will be model driven
|
|
*/
|
|
|
|
#include "Commander.hpp"
|
|
|
|
/* commander module headers */
|
|
#include "Arming/ArmAuthorization/ArmAuthorization.h"
|
|
#include "commander_helper.h"
|
|
#include "esc_calibration.h"
|
|
#include "px4_custom_mode.h"
|
|
#include "state_machine_helper.h"
|
|
#include "ModeUtil/control_mode.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 <circuit_breaker/circuit_breaker.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>
|
|
|
|
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.manual_lockdown == b.manual_lockdown &&
|
|
a.force_failsafe == b.force_failsafe &&
|
|
a.in_esc_calibration_mode == b.in_esc_calibration_mode &&
|
|
a.soft_stop == b.soft_stop);
|
|
}
|
|
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()) {
|
|
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: VEHICLE_CMD_FIXED_MAG_CAL_YAW
|
|
send_vehicle_command(vehicle_command_s::VEHICLE_CMD_FIXED_MAG_CAL_YAW);
|
|
|
|
} 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")) {
|
|
uORB::SubscriptionData<vehicle_status_s> vehicle_status_sub{ORB_ID(vehicle_status)};
|
|
send_vehicle_command(vehicle_command_s::VEHICLE_CMD_RUN_PREARM_CHECKS);
|
|
|
|
uORB::SubscriptionData<vehicle_status_flags_s> vehicle_status_flags_sub{ORB_ID(vehicle_status_flags)};
|
|
PX4_INFO("Preflight check: %s", vehicle_status_flags_sub.get().pre_flight_checks_pass ? "OK" : "FAILED");
|
|
|
|
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], "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: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 {
|
|
PX4_ERR("argument %s unsupported.", argv[1]);
|
|
}
|
|
|
|
return 0;
|
|
|
|
} else {
|
|
PX4_ERR("missing argument");
|
|
}
|
|
}
|
|
|
|
if (!strcmp(argv[0], "lockdown")) {
|
|
|
|
if (argc < 2) {
|
|
Commander::print_usage("not enough arguments, missing [on, off]");
|
|
return 1;
|
|
}
|
|
|
|
bool ret = send_vehicle_command(vehicle_command_s::VEHICLE_CMD_DO_FLIGHTTERMINATION,
|
|
strcmp(argv[1], "off") ? 2.0f : 0.0f /* lockdown */, 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], "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("Arm state: %s", _arm_state_machine.getArmStateName());
|
|
PX4_INFO("navigation: %s", nav_state_names[_vehicle_status.nav_state]);
|
|
perf_print_counter(_loop_perf);
|
|
perf_print_counter(_preflight_check_perf);
|
|
return 0;
|
|
}
|
|
|
|
extern "C" __EXPORT int commander_main(int argc, char *argv[])
|
|
{
|
|
return Commander::main(argc, argv);
|
|
}
|
|
|
|
bool Commander::shutdown_if_allowed()
|
|
{
|
|
return TRANSITION_DENIED != _arm_state_machine.arming_state_transition(_vehicle_status,
|
|
vehicle_status_s::ARMING_STATE_SHUTDOWN, _actuator_armed, _health_and_arming_checks,
|
|
false /* fRunPreArmChecks */, &_mavlink_log_pub, arm_disarm_reason_t::shutdown);
|
|
}
|
|
|
|
static constexpr const char *arm_disarm_reason_str(arm_disarm_reason_t calling_reason)
|
|
{
|
|
switch (calling_reason) {
|
|
case arm_disarm_reason_t::transition_to_standby: return "";
|
|
|
|
case arm_disarm_reason_t::rc_stick: return "RC";
|
|
|
|
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::auto_disarm_land: return "landing";
|
|
|
|
case arm_disarm_reason_t::auto_disarm_preflight: return "auto preflight disarming";
|
|
|
|
case arm_disarm_reason_t::kill_switch: return "kill-switch";
|
|
|
|
case arm_disarm_reason_t::lockdown: return "lockdown";
|
|
|
|
case arm_disarm_reason_t::failure_detector: return "failure detector";
|
|
|
|
case arm_disarm_reason_t::shutdown: return "shutdown request";
|
|
|
|
case arm_disarm_reason_t::unit_test: return "unit tests";
|
|
|
|
case arm_disarm_reason_t::rc_button: return "RC (button)";
|
|
}
|
|
|
|
return "";
|
|
};
|
|
|
|
|
|
using navigation_mode_t = events::px4::enums::navigation_mode_t;
|
|
|
|
static inline navigation_mode_t navigation_mode(uint8_t main_state)
|
|
{
|
|
switch (main_state) {
|
|
case commander_state_s::MAIN_STATE_MANUAL: return navigation_mode_t::manual;
|
|
|
|
case commander_state_s::MAIN_STATE_ALTCTL: return navigation_mode_t::altctl;
|
|
|
|
case commander_state_s::MAIN_STATE_POSCTL: return navigation_mode_t::posctl;
|
|
|
|
case commander_state_s::MAIN_STATE_AUTO_MISSION: return navigation_mode_t::auto_mission;
|
|
|
|
case commander_state_s::MAIN_STATE_AUTO_LOITER: return navigation_mode_t::auto_loiter;
|
|
|
|
case commander_state_s::MAIN_STATE_AUTO_RTL: return navigation_mode_t::auto_rtl;
|
|
|
|
case commander_state_s::MAIN_STATE_ACRO: return navigation_mode_t::acro;
|
|
|
|
case commander_state_s::MAIN_STATE_OFFBOARD: return navigation_mode_t::offboard;
|
|
|
|
case commander_state_s::MAIN_STATE_STAB: return navigation_mode_t::stab;
|
|
|
|
case commander_state_s::MAIN_STATE_AUTO_TAKEOFF: return navigation_mode_t::auto_takeoff;
|
|
|
|
case commander_state_s::MAIN_STATE_AUTO_LAND: return navigation_mode_t::auto_land;
|
|
|
|
case commander_state_s::MAIN_STATE_AUTO_FOLLOW_TARGET: return navigation_mode_t::auto_follow_target;
|
|
|
|
case commander_state_s::MAIN_STATE_AUTO_PRECLAND: return navigation_mode_t::auto_precland;
|
|
|
|
case commander_state_s::MAIN_STATE_ORBIT: return navigation_mode_t::orbit;
|
|
|
|
case commander_state_s::MAIN_STATE_AUTO_VTOL_TAKEOFF: return navigation_mode_t::auto_vtol_takeoff;
|
|
}
|
|
|
|
static_assert(commander_state_s::MAIN_STATE_MAX - 1 == (int)navigation_mode_t::auto_vtol_takeoff,
|
|
"enum definition mismatch");
|
|
|
|
return navigation_mode_t::unknown;
|
|
}
|
|
|
|
static constexpr const char *main_state_str(uint8_t main_state)
|
|
{
|
|
switch (main_state) {
|
|
case commander_state_s::MAIN_STATE_MANUAL: return "Manual";
|
|
|
|
case commander_state_s::MAIN_STATE_ALTCTL: return "Altitude";
|
|
|
|
case commander_state_s::MAIN_STATE_POSCTL: return "Position";
|
|
|
|
case commander_state_s::MAIN_STATE_AUTO_MISSION: return "Mission";
|
|
|
|
case commander_state_s::MAIN_STATE_AUTO_LOITER: return "Hold";
|
|
|
|
case commander_state_s::MAIN_STATE_AUTO_RTL: return "RTL";
|
|
|
|
case commander_state_s::MAIN_STATE_ACRO: return "Acro";
|
|
|
|
case commander_state_s::MAIN_STATE_OFFBOARD: return "Offboard";
|
|
|
|
case commander_state_s::MAIN_STATE_STAB: return "Stabilized";
|
|
|
|
case commander_state_s::MAIN_STATE_AUTO_TAKEOFF: return "Takeoff";
|
|
|
|
case commander_state_s::MAIN_STATE_AUTO_LAND: return "Land";
|
|
|
|
case commander_state_s::MAIN_STATE_AUTO_FOLLOW_TARGET: return "Follow target";
|
|
|
|
case commander_state_s::MAIN_STATE_AUTO_PRECLAND: return "Precision land";
|
|
|
|
case commander_state_s::MAIN_STATE_ORBIT: return "Orbit";
|
|
|
|
default: return "Unknown";
|
|
}
|
|
}
|
|
|
|
transition_result_t Commander::arm(arm_disarm_reason_t calling_reason, bool run_preflight_checks)
|
|
{
|
|
// allow a grace period for re-arming: preflight checks don't need to pass during that time, for example for accidential in-air disarming
|
|
if (calling_reason == arm_disarm_reason_t::rc_switch
|
|
&& (hrt_elapsed_time(&_last_disarmed_timestamp) < 5_s)) {
|
|
run_preflight_checks = false;
|
|
}
|
|
|
|
if (run_preflight_checks && !_arm_state_machine.isArmed()) {
|
|
if (_vehicle_control_mode.flag_control_manual_enabled) {
|
|
if (_vehicle_control_mode.flag_control_climb_rate_enabled &&
|
|
!_vehicle_status.rc_signal_lost && _is_throttle_above_center) {
|
|
mavlink_log_critical(&_mavlink_log_pub, "Arming denied: throttle above center\t");
|
|
events::send(events::ID("commander_arm_denied_throttle_center"),
|
|
{events::Log::Critical, events::LogInternal::Info},
|
|
"Arming denied: throttle above center");
|
|
tune_negative(true);
|
|
return TRANSITION_DENIED;
|
|
}
|
|
|
|
if (!_vehicle_control_mode.flag_control_climb_rate_enabled &&
|
|
!_vehicle_status.rc_signal_lost && !_is_throttle_low
|
|
&& _vehicle_status.vehicle_type != vehicle_status_s::VEHICLE_TYPE_ROVER) {
|
|
mavlink_log_critical(&_mavlink_log_pub, "Arming denied: high throttle\t");
|
|
events::send(events::ID("commander_arm_denied_throttle_high"),
|
|
{events::Log::Critical, events::LogInternal::Info},
|
|
"Arming denied: high throttle");
|
|
tune_negative(true);
|
|
return TRANSITION_DENIED;
|
|
}
|
|
|
|
} else if (calling_reason == arm_disarm_reason_t::rc_stick
|
|
|| calling_reason == arm_disarm_reason_t::rc_switch
|
|
|| calling_reason == arm_disarm_reason_t::rc_button) {
|
|
mavlink_log_critical(&_mavlink_log_pub, "Arming denied: switch to manual mode first\t");
|
|
events::send(events::ID("commander_arm_denied_not_manual"),
|
|
{events::Log::Critical, events::LogInternal::Info},
|
|
"Arming denied: switch to manual mode first");
|
|
tune_negative(true);
|
|
return TRANSITION_DENIED;
|
|
}
|
|
|
|
if ((_geofence_result.geofence_action == geofence_result_s::GF_ACTION_RTL)
|
|
&& !_vehicle_status_flags.home_position_valid) {
|
|
mavlink_log_critical(&_mavlink_log_pub, "Arming denied: Geofence RTL requires valid home\t");
|
|
events::send(events::ID("commander_arm_denied_geofence_rtl"),
|
|
{events::Log::Critical, events::LogInternal::Info},
|
|
"Arming denied: Geofence RTL requires valid home");
|
|
tune_negative(true);
|
|
return TRANSITION_DENIED;
|
|
}
|
|
}
|
|
|
|
transition_result_t arming_res = _arm_state_machine.arming_state_transition(_vehicle_status,
|
|
vehicle_status_s::ARMING_STATE_ARMED, _actuator_armed, _health_and_arming_checks, run_preflight_checks,
|
|
&_mavlink_log_pub, calling_reason);
|
|
|
|
if (arming_res == TRANSITION_CHANGED) {
|
|
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;
|
|
|
|
} else if (arming_res == TRANSITION_DENIED) {
|
|
tune_negative(true);
|
|
}
|
|
|
|
return arming_res;
|
|
}
|
|
|
|
transition_result_t Commander::disarm(arm_disarm_reason_t calling_reason, bool forced)
|
|
{
|
|
if (!forced) {
|
|
const bool landed = (_vehicle_land_detected.landed || _vehicle_land_detected.maybe_landed
|
|
|| is_ground_rover(_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::rc_stick)
|
|
|| (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::rc_stick) {
|
|
mavlink_log_critical(&_mavlink_log_pub, "Disarming denied! Not landed\t");
|
|
events::send(events::ID("commander_disarming_denied_not_landed"),
|
|
{events::Log::Critical, events::LogInternal::Info},
|
|
"Disarming denied, not landed");
|
|
}
|
|
|
|
return TRANSITION_DENIED;
|
|
}
|
|
}
|
|
|
|
transition_result_t arming_res = _arm_state_machine.arming_state_transition(_vehicle_status,
|
|
vehicle_status_s::ARMING_STATE_STANDBY, _actuator_armed, _health_and_arming_checks, false,
|
|
&_mavlink_log_pub, calling_reason);
|
|
|
|
if (arming_res == TRANSITION_CHANGED) {
|
|
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();
|
|
}
|
|
|
|
_status_changed = true;
|
|
|
|
} else if (arming_res == TRANSITION_DENIED) {
|
|
tune_negative(true);
|
|
}
|
|
|
|
return arming_res;
|
|
}
|
|
|
|
Commander::Commander() :
|
|
ModuleParams(nullptr),
|
|
_failure_detector(this),
|
|
_health_and_arming_checks(this, _vehicle_status_flags, _vehicle_status)
|
|
{
|
|
_vehicle_land_detected.landed = true;
|
|
|
|
_vehicle_status.system_id = 1;
|
|
_vehicle_status.component_id = 1;
|
|
|
|
_vehicle_status.system_type = 0;
|
|
_vehicle_status.vehicle_type = vehicle_status_s::VEHICLE_TYPE_UNKNOWN;
|
|
|
|
// We want to accept RC inputs as default
|
|
_vehicle_status.nav_state = vehicle_status_s::NAVIGATION_STATE_MANUAL;
|
|
_vehicle_status.nav_state_timestamp = hrt_absolute_time();
|
|
|
|
/* mark all signals lost as long as they haven't been found */
|
|
_vehicle_status.rc_signal_lost = true;
|
|
_vehicle_status.data_link_lost = true;
|
|
|
|
_vehicle_status_flags.offboard_control_signal_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_mav_comp_id = param_find("MAV_COMP_ID");
|
|
_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");
|
|
|
|
updateParameters();
|
|
}
|
|
|
|
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 */
|
|
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.
|
|
|
|
// Check if a mode switch had been requested
|
|
if ((((uint32_t)cmd.param2) & 1) > 0) {
|
|
transition_result_t main_ret = main_state_transition(_vehicle_status, commander_state_s::MAIN_STATE_AUTO_LOITER,
|
|
_vehicle_status_flags, _commander_state);
|
|
|
|
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, "Reposition command rejected\t");
|
|
/* EVENT
|
|
* @description Check for a valid position estimate
|
|
*/
|
|
events::send(events::ID("commander_reposition_rejected"),
|
|
{events::Log::Error, events::LogInternal::Info},
|
|
"Reposition command rejected");
|
|
}
|
|
|
|
} else {
|
|
cmd_result = vehicle_command_ack_s::VEHICLE_CMD_RESULT_ACCEPTED;
|
|
}
|
|
}
|
|
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_main_state = commander_state_s::MAIN_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_main_state = commander_state_s::MAIN_STATE_MANUAL;
|
|
|
|
} else if (custom_main_mode == PX4_CUSTOM_MAIN_MODE_ALTCTL) {
|
|
desired_main_state = commander_state_s::MAIN_STATE_ALTCTL;
|
|
|
|
} else if (custom_main_mode == PX4_CUSTOM_MAIN_MODE_POSCTL) {
|
|
desired_main_state = commander_state_s::MAIN_STATE_POSCTL;
|
|
|
|
} 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_main_state = commander_state_s::MAIN_STATE_AUTO_LOITER;
|
|
break;
|
|
|
|
case PX4_CUSTOM_SUB_MODE_AUTO_MISSION:
|
|
desired_main_state = commander_state_s::MAIN_STATE_AUTO_MISSION;
|
|
break;
|
|
|
|
case PX4_CUSTOM_SUB_MODE_AUTO_RTL:
|
|
desired_main_state = commander_state_s::MAIN_STATE_AUTO_RTL;
|
|
break;
|
|
|
|
case PX4_CUSTOM_SUB_MODE_AUTO_TAKEOFF:
|
|
desired_main_state = commander_state_s::MAIN_STATE_AUTO_TAKEOFF;
|
|
break;
|
|
|
|
case PX4_CUSTOM_SUB_MODE_AUTO_LAND:
|
|
desired_main_state = commander_state_s::MAIN_STATE_AUTO_LAND;
|
|
break;
|
|
|
|
case PX4_CUSTOM_SUB_MODE_AUTO_FOLLOW_TARGET:
|
|
desired_main_state = commander_state_s::MAIN_STATE_AUTO_FOLLOW_TARGET;
|
|
break;
|
|
|
|
case PX4_CUSTOM_SUB_MODE_AUTO_PRECLAND:
|
|
desired_main_state = commander_state_s::MAIN_STATE_AUTO_PRECLAND;
|
|
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_main_state = commander_state_s::MAIN_STATE_AUTO_MISSION;
|
|
}
|
|
|
|
} else if (custom_main_mode == PX4_CUSTOM_MAIN_MODE_ACRO) {
|
|
desired_main_state = commander_state_s::MAIN_STATE_ACRO;
|
|
|
|
} else if (custom_main_mode == PX4_CUSTOM_MAIN_MODE_STABILIZED) {
|
|
desired_main_state = commander_state_s::MAIN_STATE_STAB;
|
|
|
|
} else if (custom_main_mode == PX4_CUSTOM_MAIN_MODE_OFFBOARD) {
|
|
desired_main_state = commander_state_s::MAIN_STATE_OFFBOARD;
|
|
}
|
|
|
|
} else {
|
|
/* use base mode */
|
|
if (base_mode & VEHICLE_MODE_FLAG_AUTO_ENABLED) {
|
|
desired_main_state = commander_state_s::MAIN_STATE_AUTO_MISSION;
|
|
|
|
} else if (base_mode & VEHICLE_MODE_FLAG_MANUAL_INPUT_ENABLED) {
|
|
if (base_mode & VEHICLE_MODE_FLAG_GUIDED_ENABLED) {
|
|
desired_main_state = commander_state_s::MAIN_STATE_POSCTL;
|
|
|
|
} else if (base_mode & VEHICLE_MODE_FLAG_STABILIZE_ENABLED) {
|
|
desired_main_state = commander_state_s::MAIN_STATE_STAB;
|
|
|
|
} else {
|
|
desired_main_state = commander_state_s::MAIN_STATE_MANUAL;
|
|
}
|
|
}
|
|
}
|
|
|
|
if (desired_main_state != commander_state_s::MAIN_STATE_MAX) {
|
|
main_ret = main_state_transition(_vehicle_status, desired_main_state, _vehicle_status_flags, _commander_state);
|
|
}
|
|
|
|
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_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);
|
|
const bool cmd_from_io = (static_cast<int>(roundf(cmd.param3)) == 1234);
|
|
|
|
// Flick to in-air restore first if this comes from an onboard system and from IO
|
|
if (!forced && cmd_from_io
|
|
&& (cmd.source_system == _vehicle_status.system_id)
|
|
&& (cmd.source_component == _vehicle_status.component_id)
|
|
&& (arming_action == vehicle_command_s::ARMING_ACTION_ARM)) {
|
|
// TODO: replace with a proper allowed transition
|
|
_arm_state_machine.forceArmState(vehicle_status_s::ARMING_STATE_IN_AIR_RESTORE);
|
|
}
|
|
|
|
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;
|
|
|
|
/* update home position on arming if at least 500 ms from commander start spent to avoid setting home on in-air restart */
|
|
if ((arming_action == vehicle_command_s::ARMING_ACTION_ARM) && (arming_res == TRANSITION_CHANGED)
|
|
&& (hrt_absolute_time() > (_boot_timestamp + INAIR_RESTART_HOLDOFF_INTERVAL))
|
|
&& (_param_com_home_en.get())) {
|
|
_home_position.setHomePosition();
|
|
}
|
|
}
|
|
}
|
|
}
|
|
break;
|
|
|
|
case vehicle_command_s::VEHICLE_CMD_DO_FLIGHTTERMINATION: {
|
|
if (cmd.param1 > 1.5f) {
|
|
// Test termination command triggers lockdown but not actual termination.
|
|
if (!_lockdown_triggered) {
|
|
_actuator_armed.lockdown = true;
|
|
_lockdown_triggered = true;
|
|
PX4_WARN("forcing lockdown (motors off)");
|
|
}
|
|
|
|
} else if (cmd.param1 > 0.5f) {
|
|
// Trigger real termination.
|
|
if (!_flight_termination_triggered) {
|
|
_actuator_armed.force_failsafe = true;
|
|
_flight_termination_triggered = true;
|
|
PX4_WARN("forcing failsafe (termination)");
|
|
send_parachute_command();
|
|
}
|
|
|
|
} else {
|
|
_actuator_armed.force_failsafe = false;
|
|
_actuator_armed.lockdown = false;
|
|
|
|
_lockdown_triggered = false;
|
|
_flight_termination_triggered = false;
|
|
|
|
PX4_WARN("disabling failsafe and lockdown");
|
|
}
|
|
|
|
cmd_result = vehicle_command_ack_s::VEHICLE_CMD_RESULT_ACCEPTED;
|
|
}
|
|
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 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, 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 RTL which ends the mission */
|
|
if (TRANSITION_CHANGED == main_state_transition(_vehicle_status, commander_state_s::MAIN_STATE_AUTO_RTL,
|
|
_vehicle_status_flags,
|
|
_commander_state)) {
|
|
mavlink_log_info(&_mavlink_log_pub, "Returning to launch\t");
|
|
events::send(events::ID("commander_rtl"), events::Log::Info, "Returning to launch");
|
|
cmd_result = vehicle_command_ack_s::VEHICLE_CMD_RESULT_ACCEPTED;
|
|
|
|
} else {
|
|
mavlink_log_critical(&_mavlink_log_pub, "Return to launch denied\t");
|
|
/* EVENT
|
|
* @description Check for a valid position estimate
|
|
*/
|
|
events::send(events::ID("commander_rtl_denied"), {events::Log::Critical, events::LogInternal::Info},
|
|
"Return to launch denied");
|
|
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 (TRANSITION_CHANGED == main_state_transition(_vehicle_status, commander_state_s::MAIN_STATE_AUTO_TAKEOFF,
|
|
_vehicle_status_flags,
|
|
_commander_state)) {
|
|
cmd_result = vehicle_command_ack_s::VEHICLE_CMD_RESULT_ACCEPTED;
|
|
|
|
} else if (_commander_state.main_state == commander_state_s::MAIN_STATE_AUTO_TAKEOFF) {
|
|
cmd_result = vehicle_command_ack_s::VEHICLE_CMD_RESULT_ACCEPTED;
|
|
|
|
} else {
|
|
mavlink_log_critical(&_mavlink_log_pub, "Takeoff denied!\t");
|
|
/* EVENT
|
|
* @description Check for a valid position estimate
|
|
*/
|
|
events::send(events::ID("commander_takeoff_denied"), {events::Log::Critical, events::LogInternal::Info},
|
|
"Takeoff denied!");
|
|
cmd_result = vehicle_command_ack_s::VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED;
|
|
}
|
|
}
|
|
break;
|
|
|
|
case vehicle_command_s::VEHICLE_CMD_NAV_VTOL_TAKEOFF:
|
|
|
|
/* ok, home set, use it to take off */
|
|
if (TRANSITION_CHANGED == main_state_transition(_vehicle_status, commander_state_s::MAIN_STATE_AUTO_VTOL_TAKEOFF,
|
|
_vehicle_status_flags,
|
|
_commander_state)) {
|
|
cmd_result = vehicle_command_ack_s::VEHICLE_CMD_RESULT_ACCEPTED;
|
|
|
|
} else if (_commander_state.main_state == commander_state_s::MAIN_STATE_AUTO_VTOL_TAKEOFF) {
|
|
cmd_result = vehicle_command_ack_s::VEHICLE_CMD_RESULT_ACCEPTED;
|
|
|
|
} else {
|
|
mavlink_log_critical(&_mavlink_log_pub, "VTOL Takeoff denied! Please disarm and retry");
|
|
cmd_result = vehicle_command_ack_s::VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED;
|
|
}
|
|
|
|
break;
|
|
|
|
case vehicle_command_s::VEHICLE_CMD_NAV_LAND: {
|
|
if (TRANSITION_DENIED != main_state_transition(_vehicle_status, commander_state_s::MAIN_STATE_AUTO_LAND,
|
|
_vehicle_status_flags,
|
|
_commander_state)) {
|
|
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 {
|
|
mavlink_log_critical(&_mavlink_log_pub, "Landing denied! Please land manually\t");
|
|
/* EVENT
|
|
* @description Check for a valid position estimate
|
|
*/
|
|
events::send(events::ID("commander_landing_current_pos_denied"), {events::Log::Critical, events::LogInternal::Info},
|
|
"Landing denied! Please land manually");
|
|
cmd_result = vehicle_command_ack_s::VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED;
|
|
}
|
|
}
|
|
break;
|
|
|
|
case vehicle_command_s::VEHICLE_CMD_NAV_PRECLAND: {
|
|
if (TRANSITION_DENIED != main_state_transition(_vehicle_status, commander_state_s::MAIN_STATE_AUTO_PRECLAND,
|
|
_vehicle_status_flags,
|
|
_commander_state)) {
|
|
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 {
|
|
mavlink_log_critical(&_mavlink_log_pub, "Precision landing denied! Please land manually\t");
|
|
/* EVENT
|
|
* @description Check for a valid position estimate
|
|
*/
|
|
events::send(events::ID("commander_landing_prec_land_denied"), {events::Log::Critical, events::LogInternal::Info},
|
|
"Precision landing denied! Please land manually");
|
|
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 (_vehicle_status.auto_mission_available) {
|
|
|
|
// 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 ((TRANSITION_DENIED != main_state_transition(_vehicle_status, commander_state_s::MAIN_STATE_AUTO_MISSION,
|
|
_vehicle_status_flags,
|
|
_commander_state))
|
|
&& (TRANSITION_DENIED != arm(arm_disarm_reason_t::mission_start))) {
|
|
|
|
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, "Mission start denied\t");
|
|
/* EVENT
|
|
* @description Check for a valid position estimate
|
|
*/
|
|
events::send(events::ID("commander_mission_start_denied"), {events::Log::Critical, events::LogInternal::Info},
|
|
"Mission start denied");
|
|
}
|
|
}
|
|
|
|
} 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_heartbeat > _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
|
|
main_ret = main_state_transition(_vehicle_status, commander_state_s::MAIN_STATE_AUTO_LOITER,
|
|
_vehicle_status_flags, _commander_state);
|
|
|
|
} else {
|
|
// Switch to orbit state and let the orbit task handle the command further
|
|
main_ret = main_state_transition(_vehicle_status, commander_state_s::MAIN_STATE_ORBIT, _vehicle_status_flags,
|
|
_commander_state);
|
|
}
|
|
|
|
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_MOTOR_TEST:
|
|
cmd_result = handle_command_motor_test(cmd);
|
|
break;
|
|
|
|
case vehicle_command_s::VEHICLE_CMD_ACTUATOR_TEST:
|
|
cmd_result = handle_command_actuator_test(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) && shutdown_if_allowed() && (px4_reboot_request(false, 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) && shutdown_if_allowed() && (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) && shutdown_if_allowed() && (px4_reboot_request(true, 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);
|
|
}
|
|
}
|
|
|
|
break;
|
|
|
|
case vehicle_command_s::VEHICLE_CMD_PREFLIGHT_CALIBRATION: {
|
|
|
|
if (_arm_state_machine.isArmed() || _arm_state_machine.isShutdown() || _worker_thread.isBusy()) {
|
|
|
|
// reject if armed or shutting down
|
|
answer_command(cmd, vehicle_command_ack_s::VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED);
|
|
|
|
} else {
|
|
|
|
/* try to go to INIT/PREFLIGHT arming state */
|
|
if (TRANSITION_DENIED == _arm_state_machine.arming_state_transition(_vehicle_status,
|
|
vehicle_status_s::ARMING_STATE_INIT, _actuator_armed, _health_and_arming_checks,
|
|
false /* fRunPreArmChecks */, &_mavlink_log_pub,
|
|
(cmd.from_external ? arm_disarm_reason_t::command_external : arm_disarm_reason_t::command_internal))
|
|
) {
|
|
|
|
answer_command(cmd, vehicle_command_ack_s::VEHICLE_CMD_RESULT_DENIED);
|
|
break;
|
|
|
|
}
|
|
|
|
if ((int)(cmd.param1) == 1) {
|
|
/* gyro calibration */
|
|
answer_command(cmd, vehicle_command_ack_s::VEHICLE_CMD_RESULT_ACCEPTED);
|
|
_vehicle_status_flags.calibration_enabled = true;
|
|
_worker_thread.startTask(WorkerThread::Request::GyroCalibration);
|
|
|
|
} else if ((int)(cmd.param1) == vehicle_command_s::PREFLIGHT_CALIBRATION_TEMPERATURE_CALIBRATION ||
|
|
(int)(cmd.param5) == vehicle_command_s::PREFLIGHT_CALIBRATION_TEMPERATURE_CALIBRATION ||
|
|
(int)(cmd.param7) == vehicle_command_s::PREFLIGHT_CALIBRATION_TEMPERATURE_CALIBRATION) {
|
|
/* temperature calibration: handled in events module */
|
|
break;
|
|
|
|
} else if ((int)(cmd.param2) == 1) {
|
|
/* magnetometer calibration */
|
|
answer_command(cmd, vehicle_command_ack_s::VEHICLE_CMD_RESULT_ACCEPTED);
|
|
_vehicle_status_flags.calibration_enabled = true;
|
|
_worker_thread.startTask(WorkerThread::Request::MagCalibration);
|
|
|
|
} else if ((int)(cmd.param3) == 1) {
|
|
/* baro calibration */
|
|
answer_command(cmd, vehicle_command_ack_s::VEHICLE_CMD_RESULT_ACCEPTED);
|
|
_vehicle_status_flags.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_flags.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_flags.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_flags.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_flags.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_flags.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_flags.calibration_enabled = true;
|
|
_worker_thread.startTask(WorkerThread::Request::AirspeedCalibration);
|
|
|
|
} else if ((int)(cmd.param7) == 1) {
|
|
/* do esc calibration */
|
|
if (check_battery_disconnected(&_mavlink_log_pub)) {
|
|
answer_command(cmd, vehicle_command_ack_s::VEHICLE_CMD_RESULT_ACCEPTED);
|
|
|
|
if (_safety.isButtonAvailable() && !_safety.isSafetyOff()) {
|
|
mavlink_log_critical(&_mavlink_log_pub, "ESC calibration denied! Press safety button first\t");
|
|
events::send(events::ID("commander_esc_calibration_denied"), events::Log::Critical,
|
|
"ESCs calibration denied");
|
|
|
|
} else {
|
|
_vehicle_status_flags.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_flags.rc_calibration_in_progress) {
|
|
/* enable RC control input */
|
|
_vehicle_status_flags.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);
|
|
}
|
|
}
|
|
|
|
break;
|
|
}
|
|
|
|
case vehicle_command_s::VEHICLE_CMD_FIXED_MAG_CAL_YAW: {
|
|
// Magnetometer quick calibration using world magnetic model and known heading
|
|
if (_arm_state_machine.isArmed() || _arm_state_machine.isShutdown() || _worker_thread.isBusy()) {
|
|
|
|
// reject if armed or shutting down
|
|
answer_command(cmd, vehicle_command_ack_s::VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED);
|
|
|
|
} else {
|
|
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_flags.calibration_enabled = true;
|
|
_worker_thread.setMagQuickData(heading_radians, latitude, longitude);
|
|
_worker_thread.startTask(WorkerThread::Request::MagCalibrationQuick);
|
|
}
|
|
|
|
break;
|
|
}
|
|
|
|
case vehicle_command_s::VEHICLE_CMD_PREFLIGHT_STORAGE: {
|
|
|
|
if (_arm_state_machine.isArmed() || _arm_state_machine.isShutdown() || _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);
|
|
}
|
|
}
|
|
|
|
break;
|
|
}
|
|
|
|
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);
|
|
break;
|
|
|
|
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_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_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_DO_SET_ACTUATOR:
|
|
case vehicle_command_s::VEHICLE_CMD_REQUEST_MESSAGE:
|
|
case vehicle_command_s::VEHICLE_CMD_DO_WINCH:
|
|
case vehicle_command_s::VEHICLE_CMD_DO_GRIPPER:
|
|
/* ignore commands that are handled by other parts of the system */
|
|
break;
|
|
|
|
default:
|
|
/* Warn about unsupported commands, this makes sense because only commands
|
|
* to this component ID (or all) are passed by mavlink. */
|
|
answer_command(cmd, vehicle_command_ack_s::VEHICLE_CMD_RESULT_UNSUPPORTED);
|
|
break;
|
|
}
|
|
|
|
if (cmd_result != vehicle_command_ack_s::VEHICLE_CMD_RESULT_UNSUPPORTED) {
|
|
/* already warned about unsupported commands in "default" case */
|
|
answer_command(cmd, cmd_result);
|
|
}
|
|
|
|
return true;
|
|
}
|
|
|
|
unsigned
|
|
Commander::handle_command_motor_test(const vehicle_command_s &cmd)
|
|
{
|
|
if (_arm_state_machine.isArmed() || (_safety.isButtonAvailable() && !_safety.isSafetyOff())) {
|
|
return vehicle_command_ack_s::VEHICLE_CMD_RESULT_DENIED;
|
|
}
|
|
|
|
if (_param_com_mot_test_en.get() != 1) {
|
|
return vehicle_command_ack_s::VEHICLE_CMD_RESULT_DENIED;
|
|
}
|
|
|
|
test_motor_s test_motor{};
|
|
test_motor.timestamp = hrt_absolute_time();
|
|
test_motor.motor_number = (int)(cmd.param1 + 0.5f) - 1;
|
|
|
|
int throttle_type = (int)(cmd.param2 + 0.5f);
|
|
|
|
if (throttle_type != 0) { // 0: MOTOR_TEST_THROTTLE_PERCENT
|
|
return vehicle_command_ack_s::VEHICLE_CMD_RESULT_UNSUPPORTED;
|
|
}
|
|
|
|
int motor_count = (int)(cmd.param5 + 0.5);
|
|
|
|
if (motor_count > 1) {
|
|
return vehicle_command_ack_s::VEHICLE_CMD_RESULT_UNSUPPORTED;
|
|
}
|
|
|
|
test_motor.action = test_motor_s::ACTION_RUN;
|
|
test_motor.value = math::constrain(cmd.param3 / 100.f, 0.f, 1.f);
|
|
|
|
if (test_motor.value < FLT_EPSILON) {
|
|
// the message spec is not clear on whether 0 means stop, but it should be closer to what a user expects
|
|
test_motor.value = -1.f;
|
|
}
|
|
|
|
test_motor.timeout_ms = (int)(cmd.param4 * 1000.f + 0.5f);
|
|
|
|
// enforce a timeout and a maximum limit
|
|
if (test_motor.timeout_ms == 0 || test_motor.timeout_ms > 3000) {
|
|
test_motor.timeout_ms = 3000;
|
|
}
|
|
|
|
test_motor.driver_instance = 0; // the mavlink command does not allow to specify the instance, so set to 0 for now
|
|
_test_motor_pub.publish(test_motor);
|
|
|
|
return vehicle_command_ack_s::VEHICLE_CMD_RESULT_ACCEPTED;
|
|
}
|
|
|
|
unsigned
|
|
Commander::handle_command_actuator_test(const vehicle_command_s &cmd)
|
|
{
|
|
if (_arm_state_machine.isArmed() || (_safety.isButtonAvailable() && !_safety.isSafetyOff())) {
|
|
return vehicle_command_ack_s::VEHICLE_CMD_RESULT_DENIED;
|
|
}
|
|
|
|
if (_param_com_mot_test_en.get() != 1) {
|
|
return vehicle_command_ack_s::VEHICLE_CMD_RESULT_DENIED;
|
|
}
|
|
|
|
actuator_test_s actuator_test{};
|
|
actuator_test.timestamp = hrt_absolute_time();
|
|
actuator_test.function = (int)(cmd.param5 + 0.5);
|
|
|
|
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 = (int)(cmd.param2 * 1000.f + 0.5f);
|
|
|
|
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_flags.rc_calibration_in_progress
|
|
&& (action_request.source == action_request_s::SOURCE_RC_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_RC_STICK_GESTURE: arm_disarm_reason = arm_disarm_reason_t::rc_stick; 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 (_arm_state_machine.isArmed()) {
|
|
disarm(arm_disarm_reason);
|
|
|
|
} else {
|
|
arm(arm_disarm_reason);
|
|
}
|
|
|
|
break;
|
|
|
|
case action_request_s::ACTION_UNKILL:
|
|
if (arm_disarm_reason == arm_disarm_reason_t::rc_switch && _actuator_armed.manual_lockdown) {
|
|
mavlink_log_info(&_mavlink_log_pub, "Kill-switch disengaged\t");
|
|
events::send(events::ID("commander_kill_sw_disengaged"), events::Log::Info, "Kill-switch disengaged");
|
|
_status_changed = true;
|
|
_actuator_armed.manual_lockdown = false;
|
|
}
|
|
|
|
break;
|
|
|
|
case action_request_s::ACTION_KILL:
|
|
if (arm_disarm_reason == arm_disarm_reason_t::rc_switch && !_actuator_armed.manual_lockdown) {
|
|
const char kill_switch_string[] = "Kill-switch 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-switch engaged");
|
|
|
|
_status_changed = true;
|
|
_actuator_armed.manual_lockdown = true;
|
|
send_parachute_command();
|
|
}
|
|
|
|
break;
|
|
|
|
case action_request_s::ACTION_SWITCH_MODE:
|
|
|
|
// if there's never been a mode change force RC switch as initial state
|
|
if (action_request.source == action_request_s::SOURCE_RC_MODE_SLOT
|
|
&& !_arm_state_machine.isArmed() && (_commander_state.main_state_changes == 0)
|
|
&& (action_request.mode == commander_state_s::MAIN_STATE_ALTCTL
|
|
|| action_request.mode == commander_state_s::MAIN_STATE_POSCTL)) {
|
|
_commander_state.main_state = action_request.mode;
|
|
_commander_state.main_state_changes++;
|
|
}
|
|
|
|
int ret = main_state_transition(_vehicle_status, action_request.mode, _vehicle_status_flags, _commander_state);
|
|
|
|
if (ret == transition_result_t::TRANSITION_DENIED) {
|
|
print_reject_mode(action_request.mode);
|
|
}
|
|
|
|
break;
|
|
}
|
|
}
|
|
|
|
|
|
void Commander::updateParameters()
|
|
{
|
|
// update parameters from storage
|
|
updateParams();
|
|
|
|
get_circuit_breaker_params();
|
|
|
|
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;
|
|
}
|
|
|
|
// 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;
|
|
}
|
|
|
|
|
|
_vehicle_status.avoidance_system_required = _param_com_obs_avoid.get();
|
|
|
|
_auto_disarm_killed.set_hysteresis_time_from(false, _param_com_kill_disarm.get() * 1_s);
|
|
_offboard_available.set_hysteresis_time_from(true, _param_com_of_loss_t.get() * 1_s);
|
|
|
|
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_rover(_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{};
|
|
button_state.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_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);
|
|
|
|
/* update parameters */
|
|
if (!_arm_state_machine.isArmed()) {
|
|
updateParameters();
|
|
|
|
_status_changed = true;
|
|
}
|
|
}
|
|
|
|
/* Update OA parameter */
|
|
_vehicle_status.avoidance_system_required = _param_com_obs_avoid.get();
|
|
|
|
handlePowerButtonState();
|
|
|
|
offboard_control_update();
|
|
|
|
systemPowerUpdate();
|
|
|
|
landDetectorUpdate();
|
|
|
|
safetyButtonUpdate();
|
|
|
|
vtolStatusUpdate();
|
|
|
|
_home_position.update(_param_com_home_en.get(), !_arm_state_machine.isArmed() && _vehicle_land_detected.landed);
|
|
_vehicle_status_flags.home_position_valid = _home_position.valid();
|
|
|
|
handleAutoDisarm();
|
|
|
|
if (_geofence_warning_action_on
|
|
&& _commander_state.main_state != commander_state_s::MAIN_STATE_AUTO_RTL
|
|
&& _commander_state.main_state != commander_state_s::MAIN_STATE_AUTO_LOITER
|
|
&& _commander_state.main_state != commander_state_s::MAIN_STATE_AUTO_LAND) {
|
|
|
|
// reset flag again when we switched out of it
|
|
_geofence_warning_action_on = false;
|
|
}
|
|
|
|
battery_status_check();
|
|
|
|
/* If in INIT state, try to proceed to STANDBY state */
|
|
if (!_vehicle_status_flags.calibration_enabled && _arm_state_machine.isInit()) {
|
|
|
|
_arm_state_machine.arming_state_transition(_vehicle_status,
|
|
vehicle_status_s::ARMING_STATE_STANDBY, _actuator_armed, _health_and_arming_checks,
|
|
true /* fRunPreArmChecks */, &_mavlink_log_pub, arm_disarm_reason_t::transition_to_standby);
|
|
}
|
|
|
|
checkForMissionUpdate();
|
|
|
|
/* start geofence result check */
|
|
if (_geofence_result_sub.update(&_geofence_result)) {
|
|
_vehicle_status.geofence_violated = _geofence_result.geofence_violated;
|
|
}
|
|
|
|
const bool in_low_battery_failsafe_delay = _battery_failsafe_timestamp != 0;
|
|
|
|
// Geofence actions
|
|
if (_arm_state_machine.isArmed()
|
|
&& (_geofence_result.geofence_action != geofence_result_s::GF_ACTION_NONE)
|
|
&& !in_low_battery_failsafe_delay) {
|
|
|
|
// check for geofence violation transition
|
|
if (_geofence_result.geofence_violated && !_geofence_violated_prev) {
|
|
|
|
switch (_geofence_result.geofence_action) {
|
|
case (geofence_result_s::GF_ACTION_NONE) : {
|
|
// do nothing
|
|
break;
|
|
}
|
|
|
|
case (geofence_result_s::GF_ACTION_WARN) : {
|
|
// do nothing, mavlink critical messages are sent by navigator
|
|
break;
|
|
}
|
|
|
|
case (geofence_result_s::GF_ACTION_LOITER) : {
|
|
if (TRANSITION_CHANGED == main_state_transition(_vehicle_status, commander_state_s::MAIN_STATE_AUTO_LOITER,
|
|
_vehicle_status_flags,
|
|
_commander_state)) {
|
|
_geofence_loiter_on = true;
|
|
}
|
|
|
|
break;
|
|
}
|
|
|
|
case (geofence_result_s::GF_ACTION_RTL) : {
|
|
if (TRANSITION_CHANGED == main_state_transition(_vehicle_status, commander_state_s::MAIN_STATE_AUTO_RTL,
|
|
_vehicle_status_flags,
|
|
_commander_state)) {
|
|
_geofence_rtl_on = true;
|
|
}
|
|
|
|
break;
|
|
}
|
|
|
|
case (geofence_result_s::GF_ACTION_LAND) : {
|
|
if (TRANSITION_CHANGED == main_state_transition(_vehicle_status, commander_state_s::MAIN_STATE_AUTO_LAND,
|
|
_vehicle_status_flags,
|
|
_commander_state)) {
|
|
_geofence_land_on = true;
|
|
}
|
|
|
|
break;
|
|
}
|
|
|
|
case (geofence_result_s::GF_ACTION_TERMINATE) : {
|
|
PX4_WARN("Flight termination because of geofence");
|
|
|
|
if (!_flight_termination_triggered && !_lockdown_triggered) {
|
|
_flight_termination_triggered = true;
|
|
mavlink_log_critical(&_mavlink_log_pub, "Geofence violation! Flight terminated\t");
|
|
events::send(events::ID("commander_geofence_termination"), {events::Log::Alert, events::LogInternal::Warning},
|
|
"Geofence violation! Flight terminated");
|
|
_actuator_armed.force_failsafe = true;
|
|
_status_changed = true;
|
|
send_parachute_command();
|
|
}
|
|
|
|
break;
|
|
}
|
|
}
|
|
}
|
|
|
|
_geofence_violated_prev = _geofence_result.geofence_violated;
|
|
|
|
// reset if no longer in LOITER or if manually switched to LOITER
|
|
const bool in_loiter_mode = _commander_state.main_state == commander_state_s::MAIN_STATE_AUTO_LOITER;
|
|
|
|
if (!in_loiter_mode) {
|
|
_geofence_loiter_on = false;
|
|
}
|
|
|
|
|
|
// reset if no longer in RTL or if manually switched to RTL
|
|
const bool in_rtl_mode = _commander_state.main_state == commander_state_s::MAIN_STATE_AUTO_RTL;
|
|
|
|
if (!in_rtl_mode) {
|
|
_geofence_rtl_on = false;
|
|
}
|
|
|
|
// reset if no longer in LAND or if manually switched to LAND
|
|
const bool in_land_mode = _commander_state.main_state == commander_state_s::MAIN_STATE_AUTO_LAND;
|
|
|
|
if (!in_land_mode) {
|
|
_geofence_land_on = false;
|
|
}
|
|
|
|
_geofence_warning_action_on = _geofence_warning_action_on || (_geofence_loiter_on || _geofence_rtl_on
|
|
|| _geofence_land_on);
|
|
|
|
} else {
|
|
// No geofence checks, reset flags
|
|
_geofence_loiter_on = false;
|
|
_geofence_rtl_on = false;
|
|
_geofence_land_on = false;
|
|
_geofence_warning_action_on = false;
|
|
_geofence_violated_prev = false;
|
|
}
|
|
|
|
manual_control_check();
|
|
|
|
// data link checks which update the status
|
|
data_link_check();
|
|
|
|
/* check if we are disarmed and there is a better mode to wait in */
|
|
if (!_arm_state_machine.isArmed()) {
|
|
/* if there is no radio control but GPS lock the user might want to fly using
|
|
* just a tablet. Since the RC will force its mode switch setting on connecting
|
|
* we can as well just wait in a hold mode which enables tablet control.
|
|
*/
|
|
if (_vehicle_status.rc_signal_lost && (_commander_state.main_state_changes == 0)
|
|
&& _vehicle_status_flags.global_position_valid) {
|
|
|
|
main_state_transition(_vehicle_status, commander_state_s::MAIN_STATE_AUTO_LOITER, _vehicle_status_flags,
|
|
_commander_state);
|
|
}
|
|
}
|
|
|
|
/* handle commands last, as the system needs to be updated to handle them */
|
|
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);
|
|
}
|
|
}
|
|
|
|
/* Check for failure detector status */
|
|
if (_failure_detector.update(_vehicle_status, _vehicle_control_mode)) {
|
|
const bool motor_failure_changed = ((_vehicle_status.failure_detector_status & vehicle_status_s::FAILURE_MOTOR) > 0) !=
|
|
_failure_detector.getStatus().flags.motor;
|
|
_vehicle_status.failure_detector_status = _failure_detector.getStatus().value;
|
|
auto fd_status_flags = _failure_detector.getStatusFlags();
|
|
_status_changed = true;
|
|
|
|
if (_arm_state_machine.isArmed()) {
|
|
if (fd_status_flags.arm_escs) {
|
|
// Checks have to pass within the spool up time
|
|
if (hrt_elapsed_time(&_vehicle_status.armed_time) < _param_com_spoolup_time.get() * 1_s) {
|
|
disarm(arm_disarm_reason_t::failure_detector);
|
|
mavlink_log_critical(&_mavlink_log_pub, "ESCs did not respond to arm request\t");
|
|
events::send(events::ID("commander_fd_escs_not_arming"), events::Log::Critical, "ESCs did not respond to arm request");
|
|
}
|
|
}
|
|
|
|
if (fd_status_flags.roll || fd_status_flags.pitch || fd_status_flags.alt || fd_status_flags.ext) {
|
|
const bool is_right_after_takeoff = hrt_elapsed_time(&_vehicle_status.takeoff_time) <
|
|
(1_s * _param_com_lkdown_tko.get());
|
|
|
|
if (is_right_after_takeoff && !_lockdown_triggered) {
|
|
// This handles the case where something fails during the early takeoff phase
|
|
_actuator_armed.lockdown = true;
|
|
_lockdown_triggered = true;
|
|
mavlink_log_emergency(&_mavlink_log_pub, "Critical failure detected: lockdown\t");
|
|
/* EVENT
|
|
* @description
|
|
* When a critical failure is detected right after takeoff, the system turns off the motors.
|
|
* Failures include an exceeding tilt angle, altitude failure or an external failure trigger.
|
|
*
|
|
* <profile name="dev">
|
|
* This can be configured with the parameter <param>COM_LKDOWN_TKO</param>.
|
|
* </profile>
|
|
*/
|
|
events::send(events::ID("commander_fd_lockdown"), {events::Log::Emergency, events::LogInternal::Warning},
|
|
"Critical failure detected: lockdown");
|
|
|
|
} else if (!_circuit_breaker_flight_termination_disabled &&
|
|
!_flight_termination_triggered && !_lockdown_triggered) {
|
|
|
|
_actuator_armed.force_failsafe = true;
|
|
_flight_termination_triggered = true;
|
|
mavlink_log_emergency(&_mavlink_log_pub, "Critical failure detected: terminate flight\t");
|
|
/* EVENT
|
|
* @description
|
|
* Critical failures include an exceeding tilt angle, altitude failure or an external failure trigger.
|
|
*
|
|
* <profile name="dev">
|
|
* Flight termination can be disabled with the parameter <param>CBRK_FLIGHTTERM</param>.
|
|
* </profile>
|
|
*/
|
|
events::send(events::ID("commander_fd_terminate"), {events::Log::Emergency, events::LogInternal::Warning},
|
|
"Critical failure detected: terminate flight");
|
|
send_parachute_command();
|
|
}
|
|
}
|
|
|
|
if (fd_status_flags.imbalanced_prop
|
|
&& !_imbalanced_propeller_check_triggered) {
|
|
_status_changed = true;
|
|
_imbalanced_propeller_check_triggered = true;
|
|
imbalanced_prop_failsafe(&_mavlink_log_pub, _vehicle_status, _vehicle_status_flags, &_commander_state,
|
|
(imbalanced_propeller_action_t)_param_com_imb_prop_act.get());
|
|
}
|
|
}
|
|
|
|
// One-time actions based on motor failure
|
|
if (motor_failure_changed) {
|
|
if (fd_status_flags.motor) {
|
|
mavlink_log_critical(&_mavlink_log_pub, "Motor failure detected\t");
|
|
events::send(events::ID("commander_motor_failure"), events::Log::Emergency,
|
|
"Motor failure! Land immediately");
|
|
|
|
} else {
|
|
mavlink_log_critical(&_mavlink_log_pub, "Motor recovered\t");
|
|
events::send(events::ID("commander_motor_recovered"), events::Log::Warning,
|
|
"Motor recovered, landing still advised");
|
|
}
|
|
}
|
|
|
|
if (fd_status_flags.motor) {
|
|
switch ((ActuatorFailureActions)_param_com_actuator_failure_act.get()) {
|
|
case ActuatorFailureActions::AUTO_LOITER:
|
|
mavlink_log_critical(&_mavlink_log_pub, "Loitering due to actuator failure\t");
|
|
events::send(events::ID("commander_act_failure_loiter"), events::Log::Warning,
|
|
"Loitering due to actuator failure");
|
|
main_state_transition(_vehicle_status, commander_state_s::MAIN_STATE_POSCTL, _vehicle_status_flags, _commander_state);
|
|
_status_changed = true;
|
|
break;
|
|
|
|
case ActuatorFailureActions::AUTO_LAND:
|
|
mavlink_log_critical(&_mavlink_log_pub, "Landing due to actuator failure\t");
|
|
events::send(events::ID("commander_act_failure_land"), events::Log::Warning,
|
|
"Landing due to actuator failure");
|
|
main_state_transition(_vehicle_status, commander_state_s::MAIN_STATE_AUTO_LAND, _vehicle_status_flags,
|
|
_commander_state);
|
|
_status_changed = true;
|
|
break;
|
|
|
|
case ActuatorFailureActions::AUTO_RTL:
|
|
mavlink_log_critical(&_mavlink_log_pub, "Returning home due to actuator failure\t");
|
|
events::send(events::ID("commander_act_failure_rtl"), events::Log::Warning,
|
|
"Returning home due to actuator failure");
|
|
main_state_transition(_vehicle_status, commander_state_s::MAIN_STATE_AUTO_RTL, _vehicle_status_flags, _commander_state);
|
|
_status_changed = true;
|
|
break;
|
|
|
|
case ActuatorFailureActions::TERMINATE:
|
|
if (!_actuator_armed.manual_lockdown) {
|
|
mavlink_log_critical(&_mavlink_log_pub, "Flight termination due to actuator failure\t");
|
|
events::send(events::ID("commander_act_failure_term"), events::Log::Warning,
|
|
"Flight termination due to actuator failure");
|
|
_status_changed = true;
|
|
_actuator_armed.manual_lockdown = true;
|
|
send_parachute_command();
|
|
}
|
|
|
|
break;
|
|
|
|
default:
|
|
// nothing to do here
|
|
break;
|
|
}
|
|
|
|
}
|
|
}
|
|
|
|
// Check wind speed actions if either high wind warning or high wind RTL is enabled
|
|
if ((_param_com_wind_warn.get() > FLT_EPSILON || _param_com_wind_max.get() > FLT_EPSILON)
|
|
&& !_vehicle_land_detected.landed) {
|
|
checkWindSpeedThresholds();
|
|
}
|
|
|
|
/* Get current timestamp */
|
|
const hrt_abstime now = hrt_absolute_time();
|
|
|
|
// Trigger RTL if flight time is larger than max flight time specified in COM_FLT_TIME_MAX.
|
|
// The user is not able to override once above threshold, except for triggering Land.
|
|
if (!_vehicle_land_detected.landed
|
|
&& _param_com_flt_time_max.get() > FLT_EPSILON
|
|
&& _commander_state.main_state != commander_state_s::MAIN_STATE_AUTO_RTL
|
|
&& _commander_state.main_state != commander_state_s::MAIN_STATE_AUTO_LAND
|
|
&& (now - _vehicle_status.takeoff_time) > (1_s * _param_com_flt_time_max.get())) {
|
|
main_state_transition(_vehicle_status, commander_state_s::MAIN_STATE_AUTO_RTL, _vehicle_status_flags, _commander_state);
|
|
_status_changed = true;
|
|
mavlink_log_critical(&_mavlink_log_pub, "Maximum flight time reached, abort operation and RTL");
|
|
/* EVENT
|
|
* @description
|
|
* Maximal flight time reached, return to launch.
|
|
*/
|
|
events::send(events::ID("commander_max_flight_time_rtl"), {events::Log::Critical, events::LogInternal::Warning},
|
|
"Maximum flight time reached, abort operation and RTL");
|
|
}
|
|
|
|
// check for arming state changes
|
|
if (_was_armed != _arm_state_machine.isArmed()) {
|
|
_status_changed = true;
|
|
}
|
|
|
|
if (!_was_armed && _arm_state_machine.isArmed() && !_vehicle_land_detected.landed) {
|
|
_have_taken_off_since_arming = true;
|
|
}
|
|
|
|
if (_was_armed && !_arm_state_machine.isArmed()) {
|
|
const int32_t flight_uuid = _param_flight_uuid.get() + 1;
|
|
_param_flight_uuid.set(flight_uuid);
|
|
_param_flight_uuid.commit_no_notification();
|
|
|
|
_last_disarmed_timestamp = hrt_absolute_time();
|
|
|
|
// Switch back to Hold mode after autonomous landing
|
|
if (_vehicle_control_mode.flag_control_auto_enabled) {
|
|
main_state_transition(_vehicle_status, commander_state_s::MAIN_STATE_AUTO_LOITER,
|
|
_vehicle_status_flags, _commander_state);
|
|
}
|
|
}
|
|
|
|
if (!_arm_state_machine.isArmed()) {
|
|
/* Reset the flag if disarmed. */
|
|
_have_taken_off_since_arming = false;
|
|
_imbalanced_propeller_check_triggered = false;
|
|
}
|
|
|
|
/* now set navigation state according to failsafe and main state */
|
|
bool nav_state_changed = set_nav_state(_vehicle_status,
|
|
_actuator_armed,
|
|
_commander_state,
|
|
&_mavlink_log_pub,
|
|
static_cast<link_loss_actions_t>(_param_nav_dll_act.get()),
|
|
_mission_result_sub.get().finished,
|
|
_mission_result_sub.get().stay_in_failsafe,
|
|
_vehicle_status_flags,
|
|
_vehicle_land_detected.landed,
|
|
static_cast<link_loss_actions_t>(_param_nav_rcl_act.get()),
|
|
static_cast<offboard_loss_actions_t>(_param_com_obl_act.get()),
|
|
static_cast<quadchute_actions_t>(_param_com_qc_act.get()),
|
|
static_cast<offboard_loss_rc_actions_t>(_param_com_obl_rc_act.get()),
|
|
static_cast<position_nav_loss_actions_t>(_param_com_posctl_navl.get()),
|
|
_param_com_rcl_act_t.get(),
|
|
_param_com_rcl_except.get());
|
|
|
|
if (nav_state_changed) {
|
|
_vehicle_status.nav_state_timestamp = hrt_absolute_time();
|
|
}
|
|
|
|
if (_vehicle_status.failsafe != _failsafe_old) {
|
|
_status_changed = true;
|
|
|
|
if (_vehicle_status.failsafe) {
|
|
mavlink_log_info(&_mavlink_log_pub, "Failsafe mode activated\t");
|
|
events::send(events::ID("commander_failsafe_activated"), events::Log::Info, "Failsafe mode activated");
|
|
|
|
} else {
|
|
mavlink_log_info(&_mavlink_log_pub, "Failsafe mode deactivated\t");
|
|
events::send(events::ID("commander_failsafe_deactivated"), events::Log::Info, "Failsafe mode deactivated");
|
|
}
|
|
|
|
_failsafe_old = _vehicle_status.failsafe;
|
|
}
|
|
|
|
_actuator_armed.prearmed = getPrearmState();
|
|
|
|
// publish states (armed, control_mode, vehicle_status, commander_state, vehicle_status_flags, failure_detector_status) at 2 Hz or immediately when changed
|
|
if (hrt_elapsed_time(&_vehicle_status.timestamp) >= 500_ms || _status_changed || nav_state_changed
|
|
|| !(_actuator_armed == actuator_armed_prev)) {
|
|
|
|
// Evaluate current prearm status (skip during arm <-> disarm transitions as checks are run there already)
|
|
if (_actuator_armed.armed == actuator_armed_prev.armed && !_vehicle_status_flags.calibration_enabled) {
|
|
perf_begin(_preflight_check_perf);
|
|
_health_and_arming_checks.update();
|
|
_vehicle_status_flags.pre_flight_checks_pass = _health_and_arming_checks.canArm(
|
|
_vehicle_status.nav_state);
|
|
perf_end(_preflight_check_perf);
|
|
|
|
check_and_inform_ready_for_takeoff();
|
|
}
|
|
|
|
// publish actuator_armed first (used by output modules)
|
|
_actuator_armed.armed = _arm_state_machine.isArmed();
|
|
_actuator_armed.ready_to_arm = _arm_state_machine.isArmed() || _arm_state_machine.isStandby();
|
|
_actuator_armed.timestamp = hrt_absolute_time();
|
|
_actuator_armed_pub.publish(_actuator_armed);
|
|
|
|
// update and publish vehicle_control_mode
|
|
update_control_mode();
|
|
|
|
// vehicle_status publish (after prearm/preflight updates above)
|
|
_vehicle_status.arming_state = _arm_state_machine.getArmState();
|
|
_vehicle_status.timestamp = hrt_absolute_time();
|
|
_vehicle_status_pub.publish(_vehicle_status);
|
|
|
|
// publish vehicle_status_flags (after prearm/preflight updates above)
|
|
_vehicle_status_flags.timestamp = hrt_absolute_time();
|
|
_vehicle_status_flags_pub.publish(_vehicle_status_flags);
|
|
|
|
// commander_state publish internal state for logging purposes
|
|
_commander_state.timestamp = hrt_absolute_time();
|
|
_commander_state_pub.publish(_commander_state);
|
|
|
|
// failure_detector_status publish
|
|
failure_detector_status_s fd_status{};
|
|
fd_status.fd_roll = _failure_detector.getStatusFlags().roll;
|
|
fd_status.fd_pitch = _failure_detector.getStatusFlags().pitch;
|
|
fd_status.fd_alt = _failure_detector.getStatusFlags().alt;
|
|
fd_status.fd_ext = _failure_detector.getStatusFlags().ext;
|
|
fd_status.fd_arm_escs = _failure_detector.getStatusFlags().arm_escs;
|
|
fd_status.fd_battery = _failure_detector.getStatusFlags().battery;
|
|
fd_status.fd_imbalanced_prop = _failure_detector.getStatusFlags().imbalanced_prop;
|
|
fd_status.fd_motor = _failure_detector.getStatusFlags().motor;
|
|
fd_status.imbalanced_prop_metric = _failure_detector.getImbalancedPropMetric();
|
|
fd_status.motor_failure_mask = _failure_detector.getMotorFailures();
|
|
fd_status.timestamp = hrt_absolute_time();
|
|
_failure_detector_status_pub.publish(fd_status);
|
|
}
|
|
|
|
checkWorkerThread();
|
|
|
|
updateTunes();
|
|
control_status_leds(_status_changed, _battery_warning);
|
|
|
|
_status_changed = false;
|
|
|
|
_was_armed = _arm_state_machine.isArmed();
|
|
|
|
arm_auth_update(now, params_updated);
|
|
|
|
px4_indicate_external_reset_lockout(LockoutComponent::Commander, _arm_state_machine.isArmed());
|
|
|
|
perf_end(_loop_perf);
|
|
|
|
// sleep if there are no vehicle_commands or action_requests to process
|
|
if (!_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 auto prev_mission_instance_count = mission_result.instance_count;
|
|
_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.instance_count > 0);
|
|
|
|
_vehicle_status.auto_mission_available = mission_result_ok && mission_result.valid;
|
|
|
|
if (mission_result_ok) {
|
|
if (_vehicle_status.mission_failure != mission_result.failure) {
|
|
_vehicle_status.mission_failure = mission_result.failure;
|
|
_status_changed = true;
|
|
|
|
if (_vehicle_status.mission_failure) {
|
|
// navigator sends out the exact reason
|
|
mavlink_log_critical(&_mavlink_log_pub, "Mission cannot be completed\t");
|
|
events::send(events::ID("commander_mission_cannot_be_completed"), {events::Log::Critical, events::LogInternal::Info},
|
|
"Mission cannot be completed");
|
|
}
|
|
}
|
|
|
|
/* Only evaluate mission state if home is set */
|
|
if (_vehicle_status_flags.home_position_valid &&
|
|
(prev_mission_instance_count != mission_result.instance_count)) {
|
|
|
|
if (!_vehicle_status.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);
|
|
}
|
|
}
|
|
}
|
|
|
|
// Transition main state to loiter or auto-mission after takeoff is completed.
|
|
if (_arm_state_machine.isArmed() && !_vehicle_land_detected.landed
|
|
&& (_vehicle_status.nav_state == vehicle_status_s::NAVIGATION_STATE_AUTO_TAKEOFF ||
|
|
_vehicle_status.nav_state == vehicle_status_s::NAVIGATION_STATE_AUTO_VTOL_TAKEOFF)
|
|
&& (mission_result.timestamp >= _vehicle_status.nav_state_timestamp)
|
|
&& mission_result.finished) {
|
|
|
|
if ((_param_takeoff_finished_action.get() == 1) && _vehicle_status.auto_mission_available) {
|
|
main_state_transition(_vehicle_status, commander_state_s::MAIN_STATE_AUTO_MISSION, _vehicle_status_flags,
|
|
_commander_state);
|
|
|
|
} else {
|
|
main_state_transition(_vehicle_status, commander_state_s::MAIN_STATE_AUTO_LOITER, _vehicle_status_flags,
|
|
_commander_state);
|
|
}
|
|
}
|
|
|
|
// Check for mission flight termination
|
|
if (_arm_state_machine.isArmed() && mission_result.flight_termination &&
|
|
!_circuit_breaker_flight_termination_disabled) {
|
|
|
|
if (!_flight_termination_triggered && !_lockdown_triggered) {
|
|
// navigator only requests flight termination on GPS failure
|
|
mavlink_log_critical(&_mavlink_log_pub, "GPS failure: Flight terminated\t");
|
|
events::send(events::ID("commander_mission_termination"), {events::Log::Alert, events::LogInternal::Warning},
|
|
"GPS failure: Flight terminated");
|
|
_flight_termination_triggered = true;
|
|
_actuator_armed.force_failsafe = true;
|
|
_status_changed = true;
|
|
send_parachute_command();
|
|
}
|
|
|
|
if (hrt_elapsed_time(&_last_termination_message_sent) > 4_s) {
|
|
_last_termination_message_sent = hrt_absolute_time();
|
|
mavlink_log_critical(&_mavlink_log_pub, "Flight termination active\t");
|
|
events::send(events::ID("commander_mission_termination_active"), {events::Log::Alert, events::LogInternal::Warning},
|
|
"Flight termination active");
|
|
}
|
|
}
|
|
}
|
|
}
|
|
|
|
bool Commander::getPrearmState() const
|
|
{
|
|
switch ((PrearmedMode)_param_com_prearm_mode.get()) {
|
|
case PrearmedMode::DISABLED:
|
|
/* skip prearmed state */
|
|
return false;
|
|
|
|
case PrearmedMode::ALWAYS:
|
|
/* safety is not present, go into prearmed
|
|
* (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::SAFETY_BUTTON:
|
|
if (_safety.isButtonAvailable()) {
|
|
/* safety button is present, go into prearmed if safety is off */
|
|
return _safety.isSafetyOff();
|
|
}
|
|
|
|
/* safety button is not present, do not go into prearmed */
|
|
return false;
|
|
}
|
|
|
|
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 (shutdown_if_allowed() && (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 (_arm_state_machine.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");
|
|
_vehicle_status.takeoff_time = 0;
|
|
|
|
} 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;
|
|
}
|
|
|
|
// automatically set or update home position
|
|
if (_param_com_home_en.get()) {
|
|
// set the home position when taking off, but only if we were previously disarmed
|
|
// and at least 500 ms from commander start spent to avoid setting home on in-air restart
|
|
if (!_vehicle_land_detected.landed && (hrt_elapsed_time(&_boot_timestamp) > INAIR_RESTART_HOLDOFF_INTERVAL)) {
|
|
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 auto 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;
|
|
}
|
|
|
|
if (_vehicle_status_flags.vtol_transition_failure != _vtol_vehicle_status.vtol_transition_failsafe) {
|
|
_vehicle_status_flags.vtol_transition_failure = _vtol_vehicle_status.vtol_transition_failsafe;
|
|
_status_changed = true;
|
|
}
|
|
|
|
const bool should_soft_stop = (_vehicle_status.vehicle_type != vehicle_status_s::VEHICLE_TYPE_ROTARY_WING);
|
|
|
|
if (_actuator_armed.soft_stop != should_soft_stop) {
|
|
_actuator_armed.soft_stop = should_soft_stop;
|
|
_status_changed = true;
|
|
}
|
|
}
|
|
}
|
|
|
|
void Commander::updateTunes()
|
|
{
|
|
// play arming and battery warning tunes
|
|
if (!_arm_tune_played && _arm_state_machine.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::BATTERY_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::BATTERY_WARNING_LOW)) {
|
|
/* play tune on battery warning */
|
|
set_tune(tune_control_s::TUNE_ID_BATTERY_WARNING_SLOW);
|
|
|
|
} else if (_vehicle_status.failsafe && _arm_state_machine.isArmed()) {
|
|
tune_failsafe(true);
|
|
|
|
} else {
|
|
set_tune(tune_control_s::TUNE_ID_STOP);
|
|
}
|
|
|
|
/* reset arm_tune_played when disarmed */
|
|
if (!_arm_state_machine.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_flags.calibration_enabled) { // did we do a calibration?
|
|
_vehicle_status_flags.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 (_arm_state_machine.isArmed()) {
|
|
|
|
// Check for auto-disarm on landing or pre-flight
|
|
if (_param_com_disarm_land.get() > 0 || _param_com_disarm_preflight.get() > 0) {
|
|
|
|
const bool landed_amid_mission = (_vehicle_status.nav_state == vehicle_status_s::NAVIGATION_STATE_AUTO_MISSION)
|
|
&& !_mission_result_sub.get().finished;
|
|
|
|
if (_param_com_disarm_land.get() > 0 && _have_taken_off_since_arming && !landed_amid_mission) {
|
|
_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_preflight.get() > 0 && !_have_taken_off_since_arming) {
|
|
_auto_disarm_landed.set_hysteresis_time_from(false, _param_com_disarm_preflight.get() * 1_s);
|
|
_auto_disarm_landed.set_state_and_update(true, hrt_absolute_time());
|
|
}
|
|
|
|
if (_auto_disarm_landed.get_state()) {
|
|
if (_have_taken_off_since_arming) {
|
|
disarm(arm_disarm_reason_t::auto_disarm_land);
|
|
|
|
} else {
|
|
disarm(arm_disarm_reason_t::auto_disarm_preflight);
|
|
}
|
|
}
|
|
}
|
|
|
|
// Auto disarm after 5 seconds if kill switch is engaged
|
|
bool auto_disarm = _actuator_armed.manual_lockdown;
|
|
|
|
// auto disarm if locked down to avoid user confusion
|
|
// skipped in HITL where lockdown is enabled for safety
|
|
if (_vehicle_status.hil_state != vehicle_status_s::HIL_STATE_ON) {
|
|
auto_disarm |= _actuator_armed.lockdown;
|
|
}
|
|
|
|
_auto_disarm_killed.set_state_and_update(auto_disarm, hrt_absolute_time());
|
|
|
|
if (_auto_disarm_killed.get_state()) {
|
|
if (_actuator_armed.manual_lockdown) {
|
|
disarm(arm_disarm_reason_t::kill_switch, true);
|
|
|
|
} else {
|
|
disarm(arm_disarm_reason_t::lockdown, true);
|
|
}
|
|
}
|
|
|
|
} else {
|
|
_auto_disarm_landed.set_state_and_update(false, hrt_absolute_time());
|
|
_auto_disarm_killed.set_state_and_update(false, hrt_absolute_time());
|
|
}
|
|
}
|
|
|
|
void
|
|
Commander::get_circuit_breaker_params()
|
|
{
|
|
_circuit_breaker_flight_termination_disabled = circuit_breaker_enabled_by_val(
|
|
_param_cbrk_flightterm.get(),
|
|
CBRK_FLIGHTTERM_KEY);
|
|
}
|
|
|
|
void Commander::check_and_inform_ready_for_takeoff()
|
|
{
|
|
#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::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)) {
|
|
|
|
bool overload = (cpuload.load > 0.95f) || (cpuload.ram_usage > 0.98f);
|
|
|
|
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 = _arm_state_machine.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 (_arm_state_machine.isArmed()) {
|
|
led_mode = led_control_s::MODE_ON;
|
|
set_normal_color = true;
|
|
|
|
} else if (!_vehicle_status_flags.pre_flight_checks_pass) {
|
|
led_mode = led_control_s::MODE_BLINK_FAST;
|
|
led_color = led_control_s::COLOR_RED;
|
|
|
|
} else if (_arm_state_machine.isStandby()) {
|
|
led_mode = led_control_s::MODE_BREATHE;
|
|
set_normal_color = true;
|
|
|
|
} else if (_arm_state_machine.isInit()) {
|
|
// if in init status it should not be in the error state
|
|
led_mode = led_control_s::MODE_OFF;
|
|
|
|
} else {
|
|
// STANDBY_ERROR and other states
|
|
led_mode = led_control_s::MODE_BLINK_NORMAL;
|
|
led_color = led_control_s::COLOR_RED;
|
|
}
|
|
|
|
if (set_normal_color) {
|
|
// set color
|
|
if (_vehicle_status.failsafe) {
|
|
led_color = led_control_s::COLOR_PURPLE;
|
|
|
|
} else if (battery_warning == battery_status_s::BATTERY_WARNING_LOW) {
|
|
led_color = led_control_s::COLOR_AMBER;
|
|
|
|
} else if (battery_warning == battery_status_s::BATTERY_WARNING_CRITICAL) {
|
|
led_color = led_control_s::COLOR_RED;
|
|
|
|
} else {
|
|
if (_vehicle_status_flags.home_position_valid && _vehicle_status_flags.global_position_valid) {
|
|
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 (_arm_state_machine.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 (_arm_state_machine.isStandby()) {
|
|
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::update_control_mode()
|
|
{
|
|
_vehicle_control_mode = {};
|
|
mode_util::getVehicleControlMode(_arm_state_machine.isArmed(), _vehicle_status.nav_state,
|
|
_vehicle_status.vehicle_type, _offboard_control_mode_sub.get(), _vehicle_control_mode);
|
|
_vehicle_control_mode.timestamp = hrt_absolute_time();
|
|
_vehicle_control_mode_pub.publish(_vehicle_control_mode);
|
|
}
|
|
|
|
void
|
|
Commander::print_reject_mode(uint8_t main_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", main_state_str(main_state));
|
|
/* EVENT
|
|
* @description Check for a valid position estimate
|
|
*/
|
|
events::send<events::px4::enums::navigation_mode_t>(events::ID("commander_modeswitch_not_avail"), {events::Log::Critical, events::LogInternal::Info},
|
|
"Switching to mode '{1}' is currently not possible", navigation_mode(main_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(_arm_state_machine.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:
|
|
tune_negative(true);
|
|
break;
|
|
|
|
case vehicle_command_ack_s::VEHICLE_CMD_RESULT_FAILED:
|
|
tune_negative(true);
|
|
break;
|
|
|
|
case vehicle_command_ack_s::VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED:
|
|
tune_negative(true);
|
|
break;
|
|
|
|
case vehicle_command_ack_s::VEHICLE_CMD_RESULT_UNSUPPORTED:
|
|
tune_negative(true);
|
|
break;
|
|
|
|
default:
|
|
break;
|
|
}
|
|
|
|
/* 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::task_spawn(int argc, char *argv[])
|
|
{
|
|
_task_id = px4_task_spawn_cmd("commander",
|
|
SCHED_DEFAULT,
|
|
SCHED_PRIORITY_DEFAULT + 40,
|
|
3250,
|
|
(px4_main_t)&run_trampoline,
|
|
(char *const *)argv);
|
|
|
|
if (_task_id < 0) {
|
|
_task_id = -1;
|
|
return -errno;
|
|
}
|
|
|
|
// wait until task is up & running
|
|
if (wait_until_running() < 0) {
|
|
_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::data_link_check()
|
|
{
|
|
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: {
|
|
iridiumsbd_status_s iridium_status;
|
|
|
|
if (_iridiumsbd_status_sub.update(&iridium_status)) {
|
|
_high_latency_datalink_heartbeat = iridium_status.last_heartbeat;
|
|
|
|
if (_vehicle_status.high_latency_data_link_lost) {
|
|
if (hrt_elapsed_time(&_high_latency_datalink_lost) > (_param_com_hldl_reg_t.get() * 1_s)) {
|
|
_vehicle_status.high_latency_data_link_lost = false;
|
|
_status_changed = true;
|
|
}
|
|
}
|
|
}
|
|
|
|
break;
|
|
}
|
|
}
|
|
|
|
if (telemetry.heartbeat_type_gcs) {
|
|
// Initial connection or recovery from data link lost
|
|
if (_vehicle_status.data_link_lost) {
|
|
_vehicle_status.data_link_lost = false;
|
|
_status_changed = true;
|
|
|
|
if (_datalink_last_heartbeat_gcs != 0) {
|
|
mavlink_log_info(&_mavlink_log_pub, "Data link regained\t");
|
|
events::send(events::ID("commander_dl_regained"), events::Log::Info, "Data link 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_component_obstacle_avoidance) {
|
|
if (_avoidance_system_lost) {
|
|
_avoidance_system_lost = false;
|
|
_status_changed = true;
|
|
}
|
|
|
|
_datalink_last_heartbeat_avoidance_system = telemetry.timestamp;
|
|
_vehicle_status.avoidance_system_valid = telemetry.avoidance_system_healthy;
|
|
}
|
|
}
|
|
}
|
|
|
|
|
|
// GCS data link loss failsafe
|
|
if (!_vehicle_status.data_link_lost) {
|
|
if ((_datalink_last_heartbeat_gcs != 0)
|
|
&& hrt_elapsed_time(&_datalink_last_heartbeat_gcs) > (_param_com_dl_loss_t.get() * 1_s)) {
|
|
|
|
_vehicle_status.data_link_lost = true;
|
|
_vehicle_status.data_link_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 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) > (_param_com_obc_loss_t.get() * 1_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");
|
|
_vehicle_status.parachute_system_present = false;
|
|
_vehicle_status.parachute_system_healthy = false;
|
|
_parachute_system_lost = true;
|
|
_status_changed = true;
|
|
}
|
|
|
|
// AVOIDANCE SYSTEM state check (only if it is enabled)
|
|
if (_vehicle_status.avoidance_system_required && !_onboard_controller_lost) {
|
|
// if heartbeats stop
|
|
if (!_avoidance_system_lost && (_datalink_last_heartbeat_avoidance_system > 0)
|
|
&& (hrt_elapsed_time(&_datalink_last_heartbeat_avoidance_system) > 5_s)) {
|
|
|
|
_avoidance_system_lost = true;
|
|
_vehicle_status.avoidance_system_valid = false;
|
|
}
|
|
}
|
|
|
|
// high latency data link loss failsafe
|
|
if (_high_latency_datalink_heartbeat > 0
|
|
&& hrt_elapsed_time(&_high_latency_datalink_heartbeat) > (_param_com_hldl_loss_t.get() * 1_s)) {
|
|
_high_latency_datalink_lost = hrt_absolute_time();
|
|
|
|
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;
|
|
}
|
|
}
|
|
}
|
|
|
|
void Commander::battery_status_check()
|
|
{
|
|
// Compare estimate of RTL time to estimate of remaining flight time
|
|
if (_vehicle_status_flags.battery_low_remaining_time
|
|
&& _arm_state_machine.isArmed()
|
|
&& !_vehicle_land_detected.ground_contact // not in any landing stage
|
|
&& !_rtl_time_actions_done
|
|
&& _commander_state.main_state != commander_state_s::MAIN_STATE_AUTO_RTL
|
|
&& _commander_state.main_state != commander_state_s::MAIN_STATE_AUTO_LAND) {
|
|
// Try to trigger RTL
|
|
if (main_state_transition(_vehicle_status, commander_state_s::MAIN_STATE_AUTO_RTL, _vehicle_status_flags,
|
|
_commander_state) == TRANSITION_CHANGED) {
|
|
mavlink_log_emergency(&_mavlink_log_pub, "Remaining flight time low, returning to land\t");
|
|
events::send(events::ID("commander_remaining_flight_time_rtl"), {events::Log::Critical, events::LogInternal::Info},
|
|
"Remaining flight time low, returning to land");
|
|
|
|
} else {
|
|
mavlink_log_emergency(&_mavlink_log_pub, "Remaining flight time low, land now!\t");
|
|
events::send(events::ID("commander_remaining_flight_time_land"), {events::Log::Critical, events::LogInternal::Info},
|
|
"Remaining flight time low, land now!");
|
|
}
|
|
|
|
_rtl_time_actions_done = true;
|
|
}
|
|
|
|
bool battery_warning_level_increased_while_armed = false;
|
|
bool update_internal_battery_state = false;
|
|
|
|
if (_arm_state_machine.isArmed()) {
|
|
if (_vehicle_status_flags.battery_warning > _battery_warning) {
|
|
battery_warning_level_increased_while_armed = true;
|
|
update_internal_battery_state = true;
|
|
}
|
|
|
|
} else {
|
|
if (_battery_warning != _vehicle_status_flags.battery_warning) {
|
|
update_internal_battery_state = true;
|
|
}
|
|
}
|
|
|
|
if (update_internal_battery_state) {
|
|
_battery_warning = _vehicle_status_flags.battery_warning;
|
|
}
|
|
|
|
// execute battery failsafe if the state has gotten worse while we are armed
|
|
if (battery_warning_level_increased_while_armed) {
|
|
uint8_t failsafe_action = get_battery_failsafe_action(_commander_state, _battery_warning,
|
|
(low_battery_action_t)_param_com_low_bat_act.get());
|
|
|
|
warn_user_about_battery(&_mavlink_log_pub, _battery_warning,
|
|
failsafe_action, _param_com_bat_act_t.get(),
|
|
main_state_str(failsafe_action), navigation_mode(failsafe_action));
|
|
_battery_failsafe_timestamp = hrt_absolute_time();
|
|
|
|
// Switch to loiter to wait for the reaction delay
|
|
if (_param_com_bat_act_t.get() > 0.f
|
|
&& failsafe_action != commander_state_s::MAIN_STATE_MAX) {
|
|
main_state_transition(_vehicle_status, commander_state_s::MAIN_STATE_AUTO_LOITER, _vehicle_status_flags,
|
|
_commander_state);
|
|
}
|
|
}
|
|
|
|
if (_battery_failsafe_timestamp != 0
|
|
&& hrt_elapsed_time(&_battery_failsafe_timestamp) > _param_com_bat_act_t.get() * 1_s
|
|
&& (_commander_state.main_state == commander_state_s::MAIN_STATE_AUTO_LOITER
|
|
|| _vehicle_control_mode.flag_control_auto_enabled)) {
|
|
_battery_failsafe_timestamp = 0;
|
|
uint8_t failsafe_action = get_battery_failsafe_action(_commander_state, _battery_warning,
|
|
(low_battery_action_t)_param_com_low_bat_act.get());
|
|
|
|
if (failsafe_action != commander_state_s::MAIN_STATE_MAX) {
|
|
_commander_state.main_state = failsafe_action;
|
|
_commander_state.main_state_changes++;
|
|
_commander_state.timestamp = hrt_absolute_time();
|
|
}
|
|
}
|
|
|
|
// Handle shutdown request from emergency battery action
|
|
if (update_internal_battery_state) {
|
|
|
|
if (_battery_warning == battery_status_s::BATTERY_WARNING_EMERGENCY) {
|
|
#if defined(BOARD_HAS_POWER_CONTROL)
|
|
|
|
if (shutdown_if_allowed() && (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
|
|
}
|
|
}
|
|
}
|
|
|
|
void Commander::manual_control_check()
|
|
{
|
|
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) {
|
|
|
|
if (!_vehicle_status_flags.rc_signal_found_once) {
|
|
_vehicle_status_flags.rc_signal_found_once = true;
|
|
|
|
} else if (_vehicle_status.rc_signal_lost) {
|
|
if (_last_valid_manual_control_setpoint > 0) {
|
|
float elapsed = hrt_elapsed_time(&_last_valid_manual_control_setpoint) * 1e-6f;
|
|
mavlink_log_info(&_mavlink_log_pub, "Manual control regained after %.1fs\t", (double)elapsed);
|
|
events::send<float>(events::ID("commander_rc_regained"), events::Log::Info,
|
|
"Manual control regained after {1:.1} s", elapsed);
|
|
}
|
|
}
|
|
|
|
if (_vehicle_status.rc_signal_lost) {
|
|
_vehicle_status.rc_signal_lost = false;
|
|
_status_changed = true;
|
|
}
|
|
|
|
_last_valid_manual_control_setpoint = manual_control_setpoint.timestamp;
|
|
_is_throttle_above_center = (manual_control_setpoint.z > 0.6f);
|
|
_is_throttle_low = (manual_control_setpoint.z < 0.1f);
|
|
|
|
if (_arm_state_machine.isArmed()) {
|
|
// Abort autonomous mode and switch to position mode if sticks are moved significantly
|
|
// but only if actually in air.
|
|
if (manual_control_setpoint.sticks_moving
|
|
&& !_vehicle_control_mode.flag_control_manual_enabled
|
|
&& (_vehicle_status.vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING)
|
|
) {
|
|
bool override_enabled = false;
|
|
|
|
if (_vehicle_control_mode.flag_control_auto_enabled) {
|
|
if (_param_com_rc_override.get() & static_cast<int32_t>(RcOverrideBits::AUTO_MODE_BIT)) {
|
|
override_enabled = true;
|
|
}
|
|
}
|
|
|
|
if (_vehicle_control_mode.flag_control_offboard_enabled) {
|
|
if (_param_com_rc_override.get() & static_cast<int32_t>(RcOverrideBits::OFFBOARD_MODE_BIT)) {
|
|
override_enabled = true;
|
|
}
|
|
}
|
|
|
|
const bool in_low_battery_failsafe_delay = (_battery_failsafe_timestamp != 0);
|
|
|
|
if (override_enabled && !in_low_battery_failsafe_delay && !_geofence_warning_action_on) {
|
|
|
|
const transition_result_t posctl_result =
|
|
main_state_transition(_vehicle_status, commander_state_s::MAIN_STATE_POSCTL, _vehicle_status_flags, _commander_state);
|
|
|
|
if (posctl_result == TRANSITION_CHANGED) {
|
|
tune_positive(true);
|
|
mavlink_log_info(&_mavlink_log_pub, "Pilot took over position control using sticks\t");
|
|
events::send(events::ID("commander_rc_override_pos"), events::Log::Info,
|
|
"Pilot took over position control using sticks");
|
|
_status_changed = true;
|
|
|
|
} else if (posctl_result == TRANSITION_DENIED) {
|
|
// If transition to POSCTL was denied, then we can try again with ALTCTL.
|
|
const transition_result_t altctl_result =
|
|
main_state_transition(_vehicle_status, commander_state_s::MAIN_STATE_ALTCTL, _vehicle_status_flags, _commander_state);
|
|
|
|
if (altctl_result == TRANSITION_CHANGED) {
|
|
tune_positive(true);
|
|
mavlink_log_info(&_mavlink_log_pub, "Pilot took over altitude control using sticks\t");
|
|
events::send(events::ID("commander_rc_override_alt"), events::Log::Info,
|
|
"Pilot took over altitude control using sticks");
|
|
_status_changed = true;
|
|
}
|
|
}
|
|
}
|
|
}
|
|
|
|
} else {
|
|
const bool is_mavlink = (manual_control_setpoint.data_source > manual_control_setpoint_s::SOURCE_RC);
|
|
|
|
// disarmed
|
|
// if there's never been a mode change force position control as initial state
|
|
if (_commander_state.main_state_changes == 0) {
|
|
if (is_mavlink || !_mode_switch_mapped) {
|
|
_commander_state.main_state = commander_state_s::MAIN_STATE_POSCTL;
|
|
_commander_state.main_state_changes++;
|
|
}
|
|
}
|
|
}
|
|
|
|
} else if ((manual_control_updated && !manual_control_setpoint.valid)
|
|
|| hrt_elapsed_time(&_last_valid_manual_control_setpoint) > _param_com_rc_loss_t.get() * 1_s) {
|
|
|
|
// prohibit stick use in case of reported invalidity or data timeout
|
|
if (!_vehicle_status.rc_signal_lost) {
|
|
_vehicle_status.rc_signal_lost = true;
|
|
_status_changed = true;
|
|
|
|
mavlink_log_critical(&_mavlink_log_pub, "Manual control lost\t");
|
|
events::send(events::ID("commander_rc_lost"), {events::Log::Critical, events::LogInternal::Info},
|
|
"Manual control lost");
|
|
}
|
|
}
|
|
}
|
|
|
|
void
|
|
Commander::offboard_control_update()
|
|
{
|
|
bool offboard_available = false;
|
|
|
|
if (_offboard_control_mode_sub.updated()) {
|
|
const offboard_control_mode_s old = _offboard_control_mode_sub.get();
|
|
|
|
if (_offboard_control_mode_sub.update()) {
|
|
const offboard_control_mode_s &ocm = _offboard_control_mode_sub.get();
|
|
|
|
if (old.position != ocm.position ||
|
|
old.velocity != ocm.velocity ||
|
|
old.acceleration != ocm.acceleration ||
|
|
old.attitude != ocm.attitude ||
|
|
old.body_rate != ocm.body_rate ||
|
|
old.actuator != ocm.actuator) {
|
|
|
|
_status_changed = true;
|
|
}
|
|
|
|
if (ocm.position || ocm.velocity || ocm.acceleration || ocm.attitude || ocm.body_rate || ocm.actuator) {
|
|
offboard_available = true;
|
|
}
|
|
}
|
|
}
|
|
|
|
if (_offboard_control_mode_sub.get().position && !_vehicle_status_flags.local_position_valid) {
|
|
offboard_available = false;
|
|
|
|
} else if (_offboard_control_mode_sub.get().velocity && !_vehicle_status_flags.local_velocity_valid) {
|
|
offboard_available = false;
|
|
|
|
} else if (_offboard_control_mode_sub.get().acceleration && !_vehicle_status_flags.local_velocity_valid) {
|
|
// OFFBOARD acceleration handled by position controller
|
|
offboard_available = false;
|
|
}
|
|
|
|
_offboard_available.set_state_and_update(offboard_available, hrt_absolute_time());
|
|
|
|
const bool offboard_lost = !_offboard_available.get_state();
|
|
|
|
if (_vehicle_status_flags.offboard_control_signal_lost != offboard_lost) {
|
|
_vehicle_status_flags.offboard_control_signal_lost = offboard_lost;
|
|
_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);
|
|
|
|
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 = 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::checkWindSpeedThresholds()
|
|
{
|
|
wind_s wind_estimate;
|
|
|
|
if (_wind_sub.update(&wind_estimate)) {
|
|
const matrix::Vector2f wind(wind_estimate.windspeed_north, wind_estimate.windspeed_east);
|
|
|
|
// publish a warning if it's the first since in air or 60s have passed since the last warning
|
|
const bool warning_timeout_passed = _last_wind_warning == 0 || hrt_elapsed_time(&_last_wind_warning) > 60_s;
|
|
|
|
if (_param_com_wind_max.get() > FLT_EPSILON
|
|
&& wind.longerThan(_param_com_wind_max.get())
|
|
&& _commander_state.main_state != commander_state_s::MAIN_STATE_AUTO_RTL
|
|
&& _commander_state.main_state != commander_state_s::MAIN_STATE_AUTO_LAND) {
|
|
|
|
main_state_transition(_vehicle_status, commander_state_s::MAIN_STATE_AUTO_RTL, _vehicle_status_flags, _commander_state);
|
|
_status_changed = true;
|
|
mavlink_log_critical(&_mavlink_log_pub, "Wind speeds above limit, abort operation and RTL (%.1f m/s)\t",
|
|
(double)wind.norm());
|
|
|
|
events::send<float>(events::ID("commander_high_wind_rtl"),
|
|
{events::Log::Warning, events::LogInternal::Info},
|
|
"Wind speeds above limit, abort operation and RTL ({1:.1m/s})", wind.norm());
|
|
|
|
} else if (_param_com_wind_warn.get() > FLT_EPSILON
|
|
&& wind.longerThan(_param_com_wind_warn.get())
|
|
&& warning_timeout_passed
|
|
&& _commander_state.main_state != commander_state_s::MAIN_STATE_AUTO_RTL
|
|
&& _commander_state.main_state != commander_state_s::MAIN_STATE_AUTO_LAND) {
|
|
|
|
mavlink_log_critical(&_mavlink_log_pub, "High wind speed detected (%.1f m/s), landing advised\t", (double)wind.norm());
|
|
|
|
events::send<float>(events::ID("commander_high_wind_warning"),
|
|
{events::Log::Warning, events::LogInternal::Info},
|
|
"High wind speed detected ({1:.1m/s}), landing advised", wind.norm());
|
|
_last_wind_warning = hrt_absolute_time();
|
|
}
|
|
}
|
|
}
|
|
|
|
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 (accel only, not recommended)", false);
|
|
PRINT_MODULE_USAGE_COMMAND_DESCR("check", "Run preflight checks");
|
|
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|auto:mission|auto:loiter|auto:rtl|auto:takeoff|auto:land|auto:precland",
|
|
"Flight mode", false);
|
|
PRINT_MODULE_USAGE_COMMAND("pair");
|
|
PRINT_MODULE_USAGE_COMMAND("lockdown");
|
|
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("poweroff", "Power off board (if supported)");
|
|
#endif
|
|
PRINT_MODULE_USAGE_DEFAULT_COMMANDS();
|
|
|
|
return 1;
|
|
}
|