mirror of
https://gitee.com/mirrors_PX4/PX4-Autopilot.git
synced 2026-05-14 01:27:36 +08:00
Format/group/organize/alphabetize methods and variables in Commander.cpp/hpp, format GeofenceResult.msg
Scope VEHICLE_MODE_FLAG to the Commander class.
This commit is contained in:
mcsauder
+11
-11
@@ -1,14 +1,14 @@
|
||||
uint64 timestamp # time since system start (microseconds)
|
||||
uint8 GF_ACTION_NONE = 0 # no action on geofence violation
|
||||
uint8 GF_ACTION_WARN = 1 # critical mavlink message
|
||||
uint8 GF_ACTION_LOITER = 2 # switch to AUTO|LOITER
|
||||
uint8 GF_ACTION_RTL = 3 # switch to AUTO|RTL
|
||||
uint8 GF_ACTION_TERMINATE = 4 # flight termination
|
||||
uint8 GF_ACTION_LAND = 5 # switch to AUTO|LAND
|
||||
uint64 timestamp # time since system start (microseconds)
|
||||
uint8 GF_ACTION_NONE = 0 # no action on geofence violation
|
||||
uint8 GF_ACTION_WARN = 1 # critical mavlink message
|
||||
uint8 GF_ACTION_LOITER = 2 # switch to AUTO|LOITER
|
||||
uint8 GF_ACTION_RTL = 3 # switch to AUTO|RTL
|
||||
uint8 GF_ACTION_TERMINATE = 4 # flight termination
|
||||
uint8 GF_ACTION_LAND = 5 # switch to AUTO|LAND
|
||||
|
||||
uint8 geofence_violation_reason # one of geofence_violation_reason_t::*
|
||||
uint8 geofence_violation_reason # one of geofence_violation_reason_t::*
|
||||
|
||||
bool primary_geofence_breached # true if the primary geofence is breached
|
||||
uint8 primary_geofence_action # action to take when the primary geofence is breached
|
||||
bool primary_geofence_breached # true if the primary geofence is breached
|
||||
uint8 primary_geofence_action # action to take when the primary geofence is breached
|
||||
|
||||
bool home_required # true if the geofence requires a valid home position
|
||||
bool home_required # true if the geofence requires a valid home position
|
||||
|
||||
+146
-162
@@ -32,23 +32,22 @@
|
||||
****************************************************************************/
|
||||
|
||||
/**
|
||||
* @file commander.cpp
|
||||
*
|
||||
* Main state machine / business logic
|
||||
*
|
||||
* @file Commander.cpp
|
||||
* Primary state machine and flight stack logic
|
||||
*/
|
||||
|
||||
#include "Commander.hpp"
|
||||
|
||||
/* commander module headers */
|
||||
/* Commander module headers */
|
||||
#include "Arming/ArmAuthorization/ArmAuthorization.h"
|
||||
#include "commander_helper.h"
|
||||
#include "esc_calibration.h"
|
||||
#define DEFINE_GET_PX4_CUSTOM_MODE
|
||||
#include "px4_custom_mode.h"
|
||||
#include "ModeUtil/control_mode.hpp"
|
||||
#include "ModeUtil/conversions.hpp"
|
||||
|
||||
#define DEFINE_GET_PX4_CUSTOM_MODE
|
||||
#include "px4_custom_mode.h"
|
||||
|
||||
/* PX4 headers */
|
||||
#include <drivers/drv_hrt.h>
|
||||
#include <drivers/drv_tone_alarm.h>
|
||||
@@ -69,23 +68,8 @@
|
||||
#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)
|
||||
static constexpr bool operator == (const actuator_armed_s &a, const actuator_armed_s &b)
|
||||
{
|
||||
return (a.armed == b.armed &&
|
||||
a.prearmed == b.prearmed &&
|
||||
@@ -95,9 +79,11 @@ static constexpr bool operator ==(const actuator_armed_s &a, const actuator_arme
|
||||
a.force_failsafe == b.force_failsafe &&
|
||||
a.in_esc_calibration_mode == b.in_esc_calibration_mode);
|
||||
}
|
||||
|
||||
static_assert(sizeof(actuator_armed_s) == 16, "actuator_armed equality operator review");
|
||||
|
||||
#if defined(BOARD_HAS_POWER_CONTROL)
|
||||
static orb_advert_t power_button_state_pub = nullptr;
|
||||
static orb_advert_t tune_control_pub = nullptr;
|
||||
|
||||
static void play_power_button_down_tune()
|
||||
@@ -114,7 +100,6 @@ static void stop_tune()
|
||||
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
|
||||
@@ -228,6 +213,56 @@ static bool broadcast_vehicle_command(const uint32_t cmd, const float param1 = N
|
||||
}
|
||||
#endif
|
||||
|
||||
Commander::Commander() :
|
||||
ModuleParams(nullptr)
|
||||
{
|
||||
_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;
|
||||
|
||||
_vehicle_status.nav_state = _user_mode_intention.get();
|
||||
_vehicle_status.nav_state_user_intention = _user_mode_intention.get();
|
||||
_vehicle_status.nav_state_timestamp = hrt_absolute_time();
|
||||
|
||||
/* mark all signals lost as long as they haven't been found */
|
||||
_vehicle_status.gcs_connection_lost = true;
|
||||
|
||||
_vehicle_status.power_input_valid = true;
|
||||
|
||||
// default for vtol is rotary wing
|
||||
_vtol_vehicle_status.vehicle_vtol_state = vtol_vehicle_status_s::VEHICLE_VTOL_STATE_MC;
|
||||
|
||||
_param_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);
|
||||
}
|
||||
|
||||
Commander *Commander::instantiate(int argc, char *argv[])
|
||||
{
|
||||
Commander *instance = new Commander();
|
||||
|
||||
if (instance) {
|
||||
if (argc >= 2 && !strcmp(argv[1], "-h")) {
|
||||
instance->enableHIL();
|
||||
}
|
||||
}
|
||||
|
||||
return instance;
|
||||
}
|
||||
|
||||
int Commander::custom_command(int argc, char *argv[])
|
||||
{
|
||||
if (!is_running()) {
|
||||
@@ -471,22 +506,6 @@ int Commander::custom_command(int argc, char *argv[])
|
||||
return print_usage("unknown command");
|
||||
}
|
||||
|
||||
int Commander::print_status()
|
||||
{
|
||||
PX4_INFO("Arm state: %s", _arm_state_machine.getArmStateName());
|
||||
PX4_INFO("navigation mode: %s", mode_util::nav_state_names[_vehicle_status.nav_state]);
|
||||
PX4_INFO("user intended navigation mode: %s", mode_util::nav_state_names[_user_mode_intention.get()]);
|
||||
PX4_INFO("in failsafe: %s", _failsafe.inFailsafe() ? "yes" : "no");
|
||||
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::shutdownIfAllowed()
|
||||
{
|
||||
return TRANSITION_DENIED != _arm_state_machine.arming_state_transition(_vehicle_status,
|
||||
@@ -638,45 +657,7 @@ transition_result_t Commander::disarm(arm_disarm_reason_t calling_reason, bool f
|
||||
return arming_res;
|
||||
}
|
||||
|
||||
Commander::Commander() :
|
||||
ModuleParams(nullptr)
|
||||
{
|
||||
_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;
|
||||
|
||||
_vehicle_status.nav_state = _user_mode_intention.get();
|
||||
_vehicle_status.nav_state_user_intention = _user_mode_intention.get();
|
||||
_vehicle_status.nav_state_timestamp = hrt_absolute_time();
|
||||
|
||||
/* mark all signals lost as long as they haven't been found */
|
||||
_vehicle_status.gcs_connection_lost = true;
|
||||
|
||||
_vehicle_status.power_input_valid = true;
|
||||
|
||||
// default for vtol is rotary wing
|
||||
_vtol_vehicle_status.vehicle_vtol_state = vtol_vehicle_status_s::VEHICLE_VTOL_STATE_MC;
|
||||
|
||||
_param_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)
|
||||
bool Commander::handleCommand(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))
|
||||
@@ -738,7 +719,7 @@ Commander::handle_command(const vehicle_command_s &cmd)
|
||||
uint8_t desired_nav_state = vehicle_status_s::NAVIGATION_STATE_MAX;
|
||||
transition_result_t main_ret = TRANSITION_NOT_CHANGED;
|
||||
|
||||
if (base_mode & VEHICLE_MODE_FLAG_CUSTOM_MODE_ENABLED) {
|
||||
if (base_mode == VEHICLE_MODE_FLAG_CUSTOM_MODE_ENABLED) {
|
||||
/* use autopilot-specific mode */
|
||||
if (custom_main_mode == PX4_CUSTOM_MAIN_MODE_MANUAL) {
|
||||
desired_nav_state = vehicle_status_s::NAVIGATION_STATE_MANUAL;
|
||||
@@ -805,14 +786,14 @@ Commander::handle_command(const vehicle_command_s &cmd)
|
||||
|
||||
} else {
|
||||
/* use base mode */
|
||||
if (base_mode & VEHICLE_MODE_FLAG_AUTO_ENABLED) {
|
||||
if (base_mode == VEHICLE_MODE_FLAG_AUTO_ENABLED) {
|
||||
desired_nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_MISSION;
|
||||
|
||||
} else if (base_mode & VEHICLE_MODE_FLAG_MANUAL_INPUT_ENABLED) {
|
||||
if (base_mode & VEHICLE_MODE_FLAG_GUIDED_ENABLED) {
|
||||
} else if (base_mode == VEHICLE_MODE_FLAG_MANUAL_INPUT_ENABLED) {
|
||||
if (base_mode == VEHICLE_MODE_FLAG_GUIDED_ENABLED) {
|
||||
desired_nav_state = vehicle_status_s::NAVIGATION_STATE_POSCTL;
|
||||
|
||||
} else if (base_mode & VEHICLE_MODE_FLAG_STABILIZE_ENABLED) {
|
||||
} else if (base_mode == VEHICLE_MODE_FLAG_STABILIZE_ENABLED) {
|
||||
desired_nav_state = vehicle_status_s::NAVIGATION_STATE_STAB;
|
||||
|
||||
} else {
|
||||
@@ -913,7 +894,7 @@ Commander::handle_command(const vehicle_command_s &cmd)
|
||||
_actuator_armed.force_failsafe = true;
|
||||
_flight_termination_triggered = true;
|
||||
PX4_WARN("forcing failsafe (termination)");
|
||||
send_parachute_command();
|
||||
sendParachuteCommand();
|
||||
}
|
||||
|
||||
} else {
|
||||
@@ -1126,13 +1107,13 @@ Commander::handle_command(const vehicle_command_s &cmd)
|
||||
|
||||
if (param1 == 0) {
|
||||
// 0: Do nothing for autopilot
|
||||
answer_command(cmd, vehicle_command_ack_s::VEHICLE_CMD_RESULT_ACCEPTED);
|
||||
answerCommand(cmd, vehicle_command_ack_s::VEHICLE_CMD_RESULT_ACCEPTED);
|
||||
|
||||
#if defined(CONFIG_BOARDCTL_RESET)
|
||||
|
||||
} else if ((param1 == 1) && shutdownIfAllowed() && (px4_reboot_request(false, 400_ms) == 0)) {
|
||||
// 1: Reboot autopilot
|
||||
answer_command(cmd, vehicle_command_ack_s::VEHICLE_CMD_RESULT_ACCEPTED);
|
||||
answerCommand(cmd, vehicle_command_ack_s::VEHICLE_CMD_RESULT_ACCEPTED);
|
||||
|
||||
while (1) { px4_usleep(1); }
|
||||
|
||||
@@ -1142,7 +1123,7 @@ Commander::handle_command(const vehicle_command_s &cmd)
|
||||
|
||||
} else if ((param1 == 2) && shutdownIfAllowed() && (px4_shutdown_request(400_ms) == 0)) {
|
||||
// 2: Shutdown autopilot
|
||||
answer_command(cmd, vehicle_command_ack_s::VEHICLE_CMD_RESULT_ACCEPTED);
|
||||
answerCommand(cmd, vehicle_command_ack_s::VEHICLE_CMD_RESULT_ACCEPTED);
|
||||
|
||||
while (1) { px4_usleep(1); }
|
||||
|
||||
@@ -1152,14 +1133,14 @@ Commander::handle_command(const vehicle_command_s &cmd)
|
||||
|
||||
} else if ((param1 == 3) && shutdownIfAllowed() && (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);
|
||||
answerCommand(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);
|
||||
answerCommand(cmd, vehicle_command_ack_s::VEHICLE_CMD_RESULT_DENIED);
|
||||
}
|
||||
}
|
||||
|
||||
@@ -1170,7 +1151,7 @@ Commander::handle_command(const vehicle_command_s &cmd)
|
||||
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);
|
||||
answerCommand(cmd, vehicle_command_ack_s::VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED);
|
||||
|
||||
} else {
|
||||
|
||||
@@ -1181,14 +1162,14 @@ Commander::handle_command(const vehicle_command_s &cmd)
|
||||
(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);
|
||||
answerCommand(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);
|
||||
answerCommand(cmd, vehicle_command_ack_s::VEHICLE_CMD_RESULT_ACCEPTED);
|
||||
_vehicle_status.calibration_enabled = true;
|
||||
_worker_thread.startTask(WorkerThread::Request::GyroCalibration);
|
||||
|
||||
@@ -1200,19 +1181,19 @@ Commander::handle_command(const vehicle_command_s &cmd)
|
||||
|
||||
} else if ((int)(cmd.param2) == 1) {
|
||||
/* magnetometer calibration */
|
||||
answer_command(cmd, vehicle_command_ack_s::VEHICLE_CMD_RESULT_ACCEPTED);
|
||||
answerCommand(cmd, vehicle_command_ack_s::VEHICLE_CMD_RESULT_ACCEPTED);
|
||||
_vehicle_status.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);
|
||||
answerCommand(cmd, vehicle_command_ack_s::VEHICLE_CMD_RESULT_ACCEPTED);
|
||||
_vehicle_status.calibration_enabled = true;
|
||||
_worker_thread.startTask(WorkerThread::Request::BaroCalibration);
|
||||
|
||||
} else if ((int)(cmd.param4) == 1) {
|
||||
/* RC calibration */
|
||||
answer_command(cmd, vehicle_command_ack_s::VEHICLE_CMD_RESULT_ACCEPTED);
|
||||
answerCommand(cmd, vehicle_command_ack_s::VEHICLE_CMD_RESULT_ACCEPTED);
|
||||
/* disable RC control input completely */
|
||||
_vehicle_status.rc_calibration_in_progress = true;
|
||||
mavlink_log_info(&_mavlink_log_pub, "Calibration: Disabling RC input\t");
|
||||
@@ -1221,39 +1202,39 @@ Commander::handle_command(const vehicle_command_s &cmd)
|
||||
|
||||
} else if ((int)(cmd.param4) == 2) {
|
||||
/* RC trim calibration */
|
||||
answer_command(cmd, vehicle_command_ack_s::VEHICLE_CMD_RESULT_ACCEPTED);
|
||||
answerCommand(cmd, vehicle_command_ack_s::VEHICLE_CMD_RESULT_ACCEPTED);
|
||||
_vehicle_status.calibration_enabled = true;
|
||||
_worker_thread.startTask(WorkerThread::Request::RCTrimCalibration);
|
||||
|
||||
} else if ((int)(cmd.param5) == 1) {
|
||||
/* accelerometer calibration */
|
||||
answer_command(cmd, vehicle_command_ack_s::VEHICLE_CMD_RESULT_ACCEPTED);
|
||||
answerCommand(cmd, vehicle_command_ack_s::VEHICLE_CMD_RESULT_ACCEPTED);
|
||||
_vehicle_status.calibration_enabled = true;
|
||||
_worker_thread.startTask(WorkerThread::Request::AccelCalibration);
|
||||
|
||||
} else if ((int)(cmd.param5) == 2) {
|
||||
// board offset calibration
|
||||
answer_command(cmd, vehicle_command_ack_s::VEHICLE_CMD_RESULT_ACCEPTED);
|
||||
answerCommand(cmd, vehicle_command_ack_s::VEHICLE_CMD_RESULT_ACCEPTED);
|
||||
_vehicle_status.calibration_enabled = true;
|
||||
_worker_thread.startTask(WorkerThread::Request::LevelCalibration);
|
||||
|
||||
} else if ((int)(cmd.param5) == 4) {
|
||||
// accelerometer quick calibration
|
||||
answer_command(cmd, vehicle_command_ack_s::VEHICLE_CMD_RESULT_ACCEPTED);
|
||||
answerCommand(cmd, vehicle_command_ack_s::VEHICLE_CMD_RESULT_ACCEPTED);
|
||||
_vehicle_status.calibration_enabled = true;
|
||||
_worker_thread.startTask(WorkerThread::Request::AccelCalibrationQuick);
|
||||
|
||||
} else if ((int)(cmd.param6) == 1 || (int)(cmd.param6) == 2) {
|
||||
// TODO: param6 == 1 is deprecated, but we still accept it for a while (feb 2017)
|
||||
/* airspeed calibration */
|
||||
answer_command(cmd, vehicle_command_ack_s::VEHICLE_CMD_RESULT_ACCEPTED);
|
||||
answerCommand(cmd, vehicle_command_ack_s::VEHICLE_CMD_RESULT_ACCEPTED);
|
||||
_vehicle_status.calibration_enabled = true;
|
||||
_worker_thread.startTask(WorkerThread::Request::AirspeedCalibration);
|
||||
|
||||
} else if ((int)(cmd.param7) == 1) {
|
||||
/* do esc calibration */
|
||||
if (check_battery_disconnected(&_mavlink_log_pub)) {
|
||||
answer_command(cmd, vehicle_command_ack_s::VEHICLE_CMD_RESULT_ACCEPTED);
|
||||
answerCommand(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");
|
||||
@@ -1267,7 +1248,7 @@ Commander::handle_command(const vehicle_command_s &cmd)
|
||||
}
|
||||
|
||||
} else {
|
||||
answer_command(cmd, vehicle_command_ack_s::VEHICLE_CMD_RESULT_DENIED);
|
||||
answerCommand(cmd, vehicle_command_ack_s::VEHICLE_CMD_RESULT_DENIED);
|
||||
}
|
||||
|
||||
} else if ((int)(cmd.param4) == 0) {
|
||||
@@ -1280,10 +1261,10 @@ Commander::handle_command(const vehicle_command_s &cmd)
|
||||
"Calibration: Restoring RC input");
|
||||
}
|
||||
|
||||
answer_command(cmd, vehicle_command_ack_s::VEHICLE_CMD_RESULT_ACCEPTED);
|
||||
answerCommand(cmd, vehicle_command_ack_s::VEHICLE_CMD_RESULT_ACCEPTED);
|
||||
|
||||
} else {
|
||||
answer_command(cmd, vehicle_command_ack_s::VEHICLE_CMD_RESULT_UNSUPPORTED);
|
||||
answerCommand(cmd, vehicle_command_ack_s::VEHICLE_CMD_RESULT_UNSUPPORTED);
|
||||
}
|
||||
}
|
||||
|
||||
@@ -1295,10 +1276,10 @@ Commander::handle_command(const vehicle_command_s &cmd)
|
||||
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);
|
||||
answerCommand(cmd, vehicle_command_ack_s::VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED);
|
||||
|
||||
} else {
|
||||
answer_command(cmd, vehicle_command_ack_s::VEHICLE_CMD_RESULT_ACCEPTED);
|
||||
answerCommand(cmd, vehicle_command_ack_s::VEHICLE_CMD_RESULT_ACCEPTED);
|
||||
// parameter 1: Heading (degrees)
|
||||
// parameter 3: Latitude (degrees)
|
||||
// parameter 4: Longitude (degrees)
|
||||
@@ -1330,28 +1311,28 @@ Commander::handle_command(const vehicle_command_s &cmd)
|
||||
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);
|
||||
answerCommand(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);
|
||||
answerCommand(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);
|
||||
answerCommand(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);
|
||||
answerCommand(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);
|
||||
answerCommand(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);
|
||||
answerCommand(cmd, vehicle_command_ack_s::VEHICLE_CMD_RESULT_ACCEPTED);
|
||||
_worker_thread.startTask(WorkerThread::Request::ParamResetAll);
|
||||
}
|
||||
}
|
||||
@@ -1361,7 +1342,7 @@ Commander::handle_command(const vehicle_command_s &cmd)
|
||||
|
||||
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);
|
||||
answerCommand(cmd, vehicle_command_ack_s::VEHICLE_CMD_RESULT_ACCEPTED);
|
||||
break;
|
||||
|
||||
case vehicle_command_s::VEHICLE_CMD_START_RX_PAIR:
|
||||
@@ -1410,13 +1391,13 @@ Commander::handle_command(const vehicle_command_s &cmd)
|
||||
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);
|
||||
answerCommand(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);
|
||||
answerCommand(cmd, cmd_result);
|
||||
}
|
||||
|
||||
return true;
|
||||
@@ -1540,7 +1521,7 @@ void Commander::executeActionRequest(const action_request_s &action_request)
|
||||
|
||||
_status_changed = true;
|
||||
_actuator_armed.manual_lockdown = true;
|
||||
send_parachute_command();
|
||||
sendParachuteCommand();
|
||||
}
|
||||
|
||||
break;
|
||||
@@ -1678,7 +1659,7 @@ void Commander::run()
|
||||
|
||||
handleAutoDisarm();
|
||||
|
||||
battery_status_check();
|
||||
batteryStatusCheck();
|
||||
|
||||
/* If in INIT state, try to proceed to STANDBY state */
|
||||
if (!_vehicle_status.calibration_enabled && _arm_state_machine.isInit()) {
|
||||
@@ -1735,7 +1716,7 @@ void Commander::run()
|
||||
PX4_ERR("vehicle_command lost, generation %u -> %u", last_generation, _vehicle_command_sub.get_last_generation());
|
||||
}
|
||||
|
||||
if (handle_command(cmd)) {
|
||||
if (handleCommand(cmd)) {
|
||||
_status_changed = true;
|
||||
}
|
||||
}
|
||||
@@ -1818,7 +1799,7 @@ void Commander::run()
|
||||
checkWorkerThread();
|
||||
|
||||
updateTunes();
|
||||
control_status_leds(_status_changed, _battery_warning);
|
||||
controlStatusLEDs(_status_changed, _battery_warning);
|
||||
|
||||
_status_changed = false;
|
||||
|
||||
@@ -2214,7 +2195,7 @@ bool Commander::handleModeIntentionAndFailsafe()
|
||||
|
||||
if (!_flight_termination_triggered) {
|
||||
_flight_termination_triggered = true;
|
||||
send_parachute_command();
|
||||
sendParachuteCommand();
|
||||
}
|
||||
|
||||
break;
|
||||
@@ -2250,7 +2231,7 @@ void Commander::checkAndInformReadyForTakeoff()
|
||||
#endif // CONFIG_ARCH_BOARD_PX4_SITL
|
||||
}
|
||||
|
||||
void Commander::control_status_leds(bool changed, const uint8_t battery_warning)
|
||||
void Commander::controlStatusLEDs(bool changed, const uint8_t battery_warning)
|
||||
{
|
||||
switch (blink_msg_state()) {
|
||||
case 1:
|
||||
@@ -2432,7 +2413,7 @@ void Commander::printRejectMode(uint8_t nav_state)
|
||||
}
|
||||
}
|
||||
|
||||
void Commander::answer_command(const vehicle_command_s &cmd, uint8_t result)
|
||||
void Commander::answerCommand(const vehicle_command_s &cmd, uint8_t result)
|
||||
{
|
||||
switch (result) {
|
||||
case vehicle_command_ack_s::VEHICLE_CMD_RESULT_ACCEPTED:
|
||||
@@ -2468,43 +2449,7 @@ void Commander::answer_command(const vehicle_command_s &cmd, uint8_t result)
|
||||
_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()
|
||||
void Commander::enableHIL()
|
||||
{
|
||||
_vehicle_status.hil_state = vehicle_status_s::HIL_STATE_ON;
|
||||
}
|
||||
@@ -2690,7 +2635,7 @@ void Commander::dataLinkCheck()
|
||||
}
|
||||
}
|
||||
|
||||
void Commander::battery_status_check()
|
||||
void Commander::batteryStatusCheck()
|
||||
{
|
||||
// Handle shutdown request from emergency battery action
|
||||
if (_battery_warning != _failsafe_flags.battery_warning) {
|
||||
@@ -2788,7 +2733,7 @@ void Commander::offboardControlCheck()
|
||||
}
|
||||
}
|
||||
|
||||
void Commander::send_parachute_command()
|
||||
void Commander::sendParachuteCommand()
|
||||
{
|
||||
vehicle_command_s vcmd{};
|
||||
vcmd.command = vehicle_command_s::VEHICLE_CMD_DO_PARACHUTE;
|
||||
@@ -2807,6 +2752,17 @@ void Commander::send_parachute_command()
|
||||
set_tune_override(tune_control_s::TUNE_ID_PARACHUTE_RELEASE);
|
||||
}
|
||||
|
||||
int Commander::print_status()
|
||||
{
|
||||
PX4_INFO("Arm state: %s", _arm_state_machine.getArmStateName());
|
||||
PX4_INFO("navigation mode: %s", mode_util::nav_state_names[_vehicle_status.nav_state]);
|
||||
PX4_INFO("user intended navigation mode: %s", mode_util::nav_state_names[_user_mode_intention.get()]);
|
||||
PX4_INFO("in failsafe: %s", _failsafe.inFailsafe() ? "yes" : "no");
|
||||
perf_print_counter(_loop_perf);
|
||||
perf_print_counter(_preflight_check_perf);
|
||||
return 0;
|
||||
}
|
||||
|
||||
int Commander::print_usage(const char *reason)
|
||||
{
|
||||
if (reason) {
|
||||
@@ -2849,3 +2805,31 @@ The commander module contains the state machine for mode switching and failsafe
|
||||
|
||||
return 1;
|
||||
}
|
||||
|
||||
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;
|
||||
}
|
||||
|
||||
extern "C" __EXPORT int commander_main(int argc, char *argv[])
|
||||
{
|
||||
return Commander::main(argc, argv);
|
||||
}
|
||||
|
||||
+143
-141
@@ -50,25 +50,21 @@
|
||||
#include <px4_platform_common/module.h>
|
||||
#include <px4_platform_common/module_params.h>
|
||||
|
||||
// publications
|
||||
#include <uORB/Publication.hpp>
|
||||
#include <uORB/topics/actuator_armed.h>
|
||||
#include <uORB/topics/actuator_test.h>
|
||||
#include <uORB/topics/failure_detector_status.h>
|
||||
#include <uORB/topics/vehicle_command_ack.h>
|
||||
#include <uORB/topics/vehicle_control_mode.h>
|
||||
#include <uORB/topics/vehicle_status.h>
|
||||
|
||||
// subscriptions
|
||||
#include <uORB/Subscription.hpp>
|
||||
#include <uORB/SubscriptionInterval.hpp>
|
||||
#include <uORB/SubscriptionMultiArray.hpp>
|
||||
|
||||
#include <uORB/topics/actuator_armed.h>
|
||||
#include <uORB/topics/actuator_test.h>
|
||||
#include <uORB/topics/failure_detector_status.h>
|
||||
#include <uORB/topics/action_request.h>
|
||||
#include <uORB/topics/airspeed.h>
|
||||
#include <uORB/topics/battery_status.h>
|
||||
#include <uORB/topics/cpuload.h>
|
||||
#include <uORB/topics/distance_sensor.h>
|
||||
#include <uORB/topics/iridiumsbd_status.h>
|
||||
#include <uORB/topics/mavlink_log.h>
|
||||
#include <uORB/topics/manual_control_setpoint.h>
|
||||
#include <uORB/topics/mission_result.h>
|
||||
#include <uORB/topics/offboard_control_mode.h>
|
||||
@@ -78,10 +74,14 @@
|
||||
#include <uORB/topics/sensor_gps.h>
|
||||
#include <uORB/topics/system_power.h>
|
||||
#include <uORB/topics/telemetry_status.h>
|
||||
#include <uORB/topics/tune_control.h>
|
||||
#include <uORB/topics/vehicle_command.h>
|
||||
#include <uORB/topics/vehicle_command_ack.h>
|
||||
#include <uORB/topics/vehicle_control_mode.h>
|
||||
#include <uORB/topics/vehicle_global_position.h>
|
||||
#include <uORB/topics/vehicle_land_detected.h>
|
||||
#include <uORB/topics/vehicle_local_position.h>
|
||||
#include <uORB/topics/vehicle_status.h>
|
||||
#include <uORB/topics/vtol_vehicle_status.h>
|
||||
|
||||
using math::constrain;
|
||||
@@ -96,13 +96,13 @@ public:
|
||||
~Commander();
|
||||
|
||||
/** @see ModuleBase */
|
||||
static int task_spawn(int argc, char *argv[]);
|
||||
static int custom_command(int argc, char *argv[]);
|
||||
|
||||
/** @see ModuleBase */
|
||||
static Commander *instantiate(int argc, char *argv[]);
|
||||
|
||||
/** @see ModuleBase */
|
||||
static int custom_command(int argc, char *argv[]);
|
||||
/** @see ModuleBase::print_status() */
|
||||
int print_status() override;
|
||||
|
||||
/** @see ModuleBase */
|
||||
static int print_usage(const char *reason = nullptr);
|
||||
@@ -110,79 +110,12 @@ public:
|
||||
/** @see ModuleBase::run() */
|
||||
void run() override;
|
||||
|
||||
/** @see ModuleBase::print_status() */
|
||||
int print_status() override;
|
||||
/** @see ModuleBase */
|
||||
static int task_spawn(int argc, char *argv[]);
|
||||
|
||||
void enable_hil();
|
||||
void enableHIL();
|
||||
|
||||
private:
|
||||
void answer_command(const vehicle_command_s &cmd, uint8_t result);
|
||||
|
||||
transition_result_t arm(arm_disarm_reason_t calling_reason, bool run_preflight_checks = true);
|
||||
|
||||
transition_result_t disarm(arm_disarm_reason_t calling_reason, bool forced = false);
|
||||
|
||||
void battery_status_check();
|
||||
|
||||
void control_status_leds(bool changed, const uint8_t battery_warning);
|
||||
|
||||
/**
|
||||
* Checks the status of all available data links and handles switching between different system telemetry states.
|
||||
*/
|
||||
void dataLinkCheck();
|
||||
|
||||
void manualControlCheck();
|
||||
|
||||
void offboardControlCheck();
|
||||
|
||||
/**
|
||||
* @brief Handle incoming vehicle command relavant to Commander
|
||||
*
|
||||
* It ignores irrelevant vehicle commands defined inside the switch case statement
|
||||
* in the function.
|
||||
*
|
||||
* @param cmd Vehicle command to handle
|
||||
*/
|
||||
bool handle_command(const vehicle_command_s &cmd);
|
||||
|
||||
unsigned handleCommandActuatorTest(const vehicle_command_s &cmd);
|
||||
|
||||
void executeActionRequest(const action_request_s &action_request);
|
||||
|
||||
void printRejectMode(uint8_t nav_state);
|
||||
|
||||
void updateControlMode();
|
||||
|
||||
bool shutdownIfAllowed();
|
||||
|
||||
void send_parachute_command();
|
||||
|
||||
void checkForMissionUpdate();
|
||||
|
||||
void handlePowerButtonState();
|
||||
|
||||
void systemPowerUpdate();
|
||||
|
||||
void landDetectorUpdate();
|
||||
|
||||
void safetyButtonUpdate();
|
||||
|
||||
void vtolStatusUpdate();
|
||||
|
||||
void updateTunes();
|
||||
|
||||
void checkWorkerThread();
|
||||
|
||||
bool getPrearmState() const;
|
||||
|
||||
void handleAutoDisarm();
|
||||
|
||||
bool handleModeIntentionAndFailsafe();
|
||||
|
||||
void updateParameters();
|
||||
|
||||
void checkAndInformReadyForTakeoff();
|
||||
|
||||
enum class PrearmedMode {
|
||||
DISABLED = 0,
|
||||
SAFETY_BUTTON = 1,
|
||||
@@ -194,108 +127,177 @@ private:
|
||||
OFFBOARD_MODE_BIT = (1 << 1),
|
||||
};
|
||||
|
||||
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, /* | */
|
||||
};
|
||||
|
||||
transition_result_t arm(arm_disarm_reason_t calling_reason, bool run_preflight_checks = true);
|
||||
|
||||
transition_result_t disarm(arm_disarm_reason_t calling_reason, bool forced = false);
|
||||
|
||||
void answerCommand(const vehicle_command_s &cmd, uint8_t result);
|
||||
|
||||
void batteryStatusCheck();
|
||||
|
||||
void checkAndInformReadyForTakeoff();
|
||||
|
||||
void checkForMissionUpdate();
|
||||
|
||||
void checkWorkerThread();
|
||||
|
||||
void controlStatusLEDs(bool changed, const uint8_t battery_warning);
|
||||
|
||||
/** Checks the status of all available data links and handles switching between different system telemetry states. */
|
||||
void dataLinkCheck();
|
||||
|
||||
void executeActionRequest(const action_request_s &action_request);
|
||||
|
||||
bool getPrearmState() const;
|
||||
|
||||
void handleAutoDisarm();
|
||||
|
||||
bool handleCommand(const vehicle_command_s &cmd);
|
||||
|
||||
unsigned handleCommandActuatorTest(const vehicle_command_s &cmd);
|
||||
|
||||
bool handleModeIntentionAndFailsafe();
|
||||
|
||||
void handlePowerButtonState();
|
||||
|
||||
void landDetectorUpdate();
|
||||
|
||||
void manualControlCheck();
|
||||
|
||||
void offboardControlCheck();
|
||||
|
||||
void printRejectMode(uint8_t nav_state);
|
||||
|
||||
void safetyButtonUpdate();
|
||||
|
||||
void sendParachuteCommand();
|
||||
|
||||
bool shutdownIfAllowed();
|
||||
|
||||
void systemPowerUpdate();
|
||||
|
||||
void updateControlMode();
|
||||
|
||||
void updateParameters();
|
||||
|
||||
void updateTunes();
|
||||
|
||||
void vtolStatusUpdate();
|
||||
|
||||
/* Decouple update interval and hysteresis counters, all depends on intervals */
|
||||
static constexpr uint64_t COMMANDER_MONITORING_INTERVAL{10_ms};
|
||||
static constexpr uint64_t INAIR_RESTART_HOLDOFF_INTERVAL{500_ms};
|
||||
|
||||
vehicle_status_s _vehicle_status{};
|
||||
|
||||
ArmStateMachine _arm_state_machine{};
|
||||
Failsafe _failsafe_instance{this};
|
||||
FailsafeBase &_failsafe{_failsafe_instance};
|
||||
FailureDetector _failure_detector{this};
|
||||
HealthAndArmingChecks _health_and_arming_checks{this, _vehicle_status};
|
||||
Safety _safety{};
|
||||
UserModeIntention _user_mode_intention{this, _vehicle_status, _health_and_arming_checks};
|
||||
WorkerThread _worker_thread{};
|
||||
ArmStateMachine _arm_state_machine{};
|
||||
Failsafe _failsafe_instance{this};
|
||||
FailsafeBase &_failsafe{_failsafe_instance};
|
||||
FailureDetector _failure_detector{this};
|
||||
HealthAndArmingChecks _health_and_arming_checks{this, _vehicle_status};
|
||||
Safety _safety{};
|
||||
UserModeIntention _user_mode_intention{this, _vehicle_status, _health_and_arming_checks};
|
||||
WorkerThread _worker_thread{};
|
||||
|
||||
const failsafe_flags_s &_failsafe_flags{_health_and_arming_checks.failsafeFlags()};
|
||||
HomePosition _home_position{_failsafe_flags};
|
||||
|
||||
HomePosition _home_position{_failsafe_flags};
|
||||
|
||||
Hysteresis _auto_disarm_landed{false};
|
||||
Hysteresis _auto_disarm_killed{false};
|
||||
|
||||
hrt_abstime _boot_timestamp{0};
|
||||
|
||||
hrt_abstime _datalink_last_heartbeat_open_drone_id_system{0};
|
||||
hrt_abstime _datalink_last_heartbeat_avoidance_system{0};
|
||||
hrt_abstime _datalink_last_heartbeat_gcs{0};
|
||||
hrt_abstime _datalink_last_heartbeat_onboard_controller{0};
|
||||
hrt_abstime _datalink_last_heartbeat_parachute_system{0};
|
||||
|
||||
hrt_abstime _last_print_mode_reject_time{0}; ///< To remember when last notification was sent
|
||||
|
||||
hrt_abstime _high_latency_datalink_heartbeat{0};
|
||||
hrt_abstime _high_latency_datalink_lost{0};
|
||||
|
||||
hrt_abstime _boot_timestamp{0};
|
||||
hrt_abstime _last_disarmed_timestamp{0};
|
||||
hrt_abstime _overload_start{0}; ///< time when CPU overload started
|
||||
hrt_abstime _last_health_and_arming_check{0};
|
||||
hrt_abstime _last_print_mode_reject_time{0}; ///< To remember when last notification was sent
|
||||
|
||||
hrt_abstime _led_overload_toggle{0};
|
||||
|
||||
hrt_abstime _overload_start{0}; ///< time when CPU overload started
|
||||
|
||||
#if !defined(CONFIG_ARCH_LEDS) && defined(BOARD_HAS_CONTROL_STATUS_LEDS)
|
||||
hrt_abstime _led_armed_state_toggle {0};
|
||||
#endif
|
||||
hrt_abstime _led_overload_toggle {0};
|
||||
|
||||
hrt_abstime _last_health_and_arming_check{0};
|
||||
actuator_armed_s _actuator_armed{};
|
||||
vehicle_control_mode_s _vehicle_control_mode{};
|
||||
vehicle_land_detected_s _vehicle_land_detected{};
|
||||
vehicle_status_s _vehicle_status{};
|
||||
vtol_vehicle_status_s _vtol_vehicle_status{};
|
||||
|
||||
uint8_t _battery_warning{battery_status_s::BATTERY_WARNING_NONE};
|
||||
uint8_t _battery_warning{battery_status_s::BATTERY_WARNING_NONE};
|
||||
|
||||
bool _failsafe_user_override_request{false}; ///< override request due to stick movements
|
||||
|
||||
bool _flight_termination_triggered{false};
|
||||
bool _lockdown_triggered{false};
|
||||
|
||||
bool _open_drone_id_system_lost{true};
|
||||
bool _arm_tune_played{false};
|
||||
bool _avoidance_system_lost{false};
|
||||
bool _onboard_controller_lost{false};
|
||||
bool _parachute_system_lost{true};
|
||||
|
||||
bool _last_overload{false};
|
||||
bool _mode_switch_mapped{false};
|
||||
bool _failsafe_user_override_request{false}; // override request due to stick movements
|
||||
bool _flight_termination_triggered{false};
|
||||
|
||||
bool _have_taken_off_since_arming{false};
|
||||
|
||||
bool _is_throttle_above_center{false};
|
||||
bool _is_throttle_low{false};
|
||||
|
||||
bool _arm_tune_played{false};
|
||||
bool _was_armed{false};
|
||||
bool _have_taken_off_since_arming{false};
|
||||
bool _last_overload{false};
|
||||
bool _lockdown_triggered{false};
|
||||
|
||||
bool _mode_switch_mapped{false};
|
||||
|
||||
bool _onboard_controller_lost{false};
|
||||
bool _open_drone_id_system_lost{true};
|
||||
|
||||
bool _parachute_system_lost{true};
|
||||
|
||||
bool _status_changed{true};
|
||||
|
||||
vehicle_land_detected_s _vehicle_land_detected{};
|
||||
|
||||
// commander publications
|
||||
actuator_armed_s _actuator_armed{};
|
||||
vehicle_control_mode_s _vehicle_control_mode{};
|
||||
vtol_vehicle_status_s _vtol_vehicle_status{};
|
||||
bool _was_armed{false};
|
||||
|
||||
// Subscriptions
|
||||
uORB::Subscription _action_request_sub{ORB_ID(action_request)};
|
||||
uORB::Subscription _cpuload_sub{ORB_ID(cpuload)};
|
||||
uORB::Subscription _iridiumsbd_status_sub{ORB_ID(iridiumsbd_status)};
|
||||
uORB::Subscription _manual_control_setpoint_sub{ORB_ID(manual_control_setpoint)};
|
||||
uORB::Subscription _system_power_sub{ORB_ID(system_power)};
|
||||
uORB::Subscription _vehicle_command_sub{ORB_ID(vehicle_command)};
|
||||
uORB::Subscription _vehicle_land_detected_sub{ORB_ID(vehicle_land_detected)};
|
||||
uORB::Subscription _vtol_vehicle_status_sub{ORB_ID(vtol_vehicle_status)};
|
||||
|
||||
uORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 1_s};
|
||||
|
||||
uORB::SubscriptionMultiArray<telemetry_status_s> _telemetry_status_subs{ORB_ID::telemetry_status};
|
||||
uORB::Subscription _action_request_sub{ORB_ID(action_request)};
|
||||
uORB::Subscription _cpuload_sub{ORB_ID(cpuload)};
|
||||
uORB::Subscription _iridiumsbd_status_sub{ORB_ID(iridiumsbd_status)};
|
||||
uORB::Subscription _manual_control_setpoint_sub{ORB_ID(manual_control_setpoint)};
|
||||
uORB::Subscription _system_power_sub{ORB_ID(system_power)};
|
||||
uORB::Subscription _vehicle_command_sub{ORB_ID(vehicle_command)};
|
||||
uORB::Subscription _vehicle_land_detected_sub{ORB_ID(vehicle_land_detected)};
|
||||
uORB::Subscription _vtol_vehicle_status_sub{ORB_ID(vtol_vehicle_status)};
|
||||
|
||||
#if defined(BOARD_HAS_POWER_CONTROL)
|
||||
uORB::Subscription _power_button_state_sub {ORB_ID(power_button_state)};
|
||||
uORB::Subscription _power_button_state_sub {ORB_ID(power_button_state)};
|
||||
#endif // BOARD_HAS_POWER_CONTROL
|
||||
|
||||
uORB::SubscriptionData<mission_result_s> _mission_result_sub{ORB_ID(mission_result)};
|
||||
uORB::SubscriptionData<offboard_control_mode_s> _offboard_control_mode_sub{ORB_ID(offboard_control_mode)};
|
||||
uORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 1_s};
|
||||
uORB::SubscriptionMultiArray<telemetry_status_s> _telemetry_status_subs{ORB_ID::telemetry_status};
|
||||
|
||||
uORB::SubscriptionData<mission_result_s> _mission_result_sub{ORB_ID(mission_result)};
|
||||
uORB::SubscriptionData<offboard_control_mode_s> _offboard_control_mode_sub{ORB_ID(offboard_control_mode)};
|
||||
|
||||
// Publications
|
||||
uORB::Publication<actuator_armed_s> _actuator_armed_pub{ORB_ID(actuator_armed)};
|
||||
uORB::Publication<actuator_test_s> _actuator_test_pub{ORB_ID(actuator_test)};
|
||||
uORB::Publication<failure_detector_status_s> _failure_detector_status_pub{ORB_ID(failure_detector_status)};
|
||||
uORB::Publication<vehicle_command_ack_s> _vehicle_command_ack_pub{ORB_ID(vehicle_command_ack)};
|
||||
uORB::Publication<vehicle_control_mode_s> _vehicle_control_mode_pub{ORB_ID(vehicle_control_mode)};
|
||||
uORB::Publication<vehicle_status_s> _vehicle_status_pub{ORB_ID(vehicle_status)};
|
||||
uORB::Publication<actuator_armed_s> _actuator_armed_pub{ORB_ID(actuator_armed)};
|
||||
uORB::Publication<actuator_test_s> _actuator_test_pub{ORB_ID(actuator_test)};
|
||||
uORB::Publication<failure_detector_status_s> _failure_detector_status_pub{ORB_ID(failure_detector_status)};
|
||||
uORB::Publication<vehicle_command_ack_s> _vehicle_command_ack_pub{ORB_ID(vehicle_command_ack)};
|
||||
uORB::Publication<vehicle_control_mode_s> _vehicle_control_mode_pub{ORB_ID(vehicle_control_mode)};
|
||||
uORB::Publication<vehicle_status_s> _vehicle_status_pub{ORB_ID(vehicle_status)};
|
||||
|
||||
orb_advert_t _mavlink_log_pub{nullptr};
|
||||
|
||||
|
||||
Reference in New Issue
Block a user