diff --git a/msg/commander_state.msg b/msg/commander_state.msg index 9733d72a515e..d79582261b4a 100644 --- a/msg/commander_state.msg +++ b/msg/commander_state.msg @@ -16,5 +16,6 @@ uint8 MAIN_STATE_AUTO_LAND = 11 uint8 MAIN_STATE_AUTO_FOLLOW_TARGET = 12 uint8 MAIN_STATE_AUTO_PRECLAND = 13 uint8 MAIN_STATE_MAX = 14 +uint8 MAIN_STATE_CUSTOM = 15 uint8 main_state # main state machine diff --git a/msg/vehicle_status.msg b/msg/vehicle_status.msg index 5b52b534f2c0..60a748497c6c 100644 --- a/msg/vehicle_status.msg +++ b/msg/vehicle_status.msg @@ -35,7 +35,8 @@ uint8 NAVIGATION_STATE_AUTO_TAKEOFF = 17 # Takeoff uint8 NAVIGATION_STATE_AUTO_LAND = 18 # Land uint8 NAVIGATION_STATE_AUTO_FOLLOW_TARGET = 19 # Auto Follow uint8 NAVIGATION_STATE_AUTO_PRECLAND = 20 # Precision land with landing target -uint8 NAVIGATION_STATE_MAX = 21 +uint8 NAVIGATION_STATE_CUSTOM = 21 # Custom flight task from vehicle command +uint8 NAVIGATION_STATE_MAX = 22 uint8 RC_IN_MODE_DEFAULT = 0 uint8 RC_IN_MODE_OFF = 1 diff --git a/src/lib/FlightTasks/FlightTasks.cpp b/src/lib/FlightTasks/FlightTasks.cpp index f14fe3ee89c2..5748f692ff5f 100644 --- a/src/lib/FlightTasks/FlightTasks.cpp +++ b/src/lib/FlightTasks/FlightTasks.cpp @@ -17,8 +17,6 @@ FlightTasks::FlightTasks() bool FlightTasks::update() { - _updateCommand(); - if (isAnyTaskActive()) { _subscription_array.update(); return _current_task.task->updateInitialize() && _current_task.task->update(); @@ -112,54 +110,20 @@ int FlightTasks::switchTask(int new_task_index) return -1; } -void FlightTasks::handleParameterUpdate() -{ - if (isAnyTaskActive()) { - _current_task.task->handleParameterUpdate(); - } -} - -const char *FlightTasks::errorToString(const int error) +int FlightTasks::switchTask(const vehicle_command_s &command, uint8_t &cmd_result) { - for (auto e : _taskError) { - if (e.error == error) { - return e.msg; - } - } - - return "This error is not mapped to a string or is unknown."; -} - -void FlightTasks::_updateCommand() -{ - // lazy subscription to command topic - if (_sub_vehicle_command < 0) { - _sub_vehicle_command = orb_subscribe(ORB_ID(vehicle_command)); - } - - // check if there's any new command - bool updated = false; - orb_check(_sub_vehicle_command, &updated); - - if (!updated) { - return; - } - - // get command - struct vehicle_command_s command; - orb_copy(ORB_ID(vehicle_command), _sub_vehicle_command, &command); - // check what command it is FlightTaskIndex desired_task = switchVehicleCommand(command.command); - if (desired_task == FlightTaskIndex::None) { + cmd_result = vehicle_command_ack_s::VEHICLE_RESULT_FAILED; + + if (desired_task == FlightTaskIndex::None) // ignore all unkown commands - return; + { + return -1; } - // switch to the commanded task int switch_result = switchTask(desired_task); - uint8_t cmd_result = vehicle_command_ack_s::VEHICLE_RESULT_FAILED; // if we are in/switched to the desired task if (switch_result >= 0) { @@ -168,29 +132,26 @@ void FlightTasks::_updateCommand() // if the task is running apply parameters to it and see if it rejects if (isAnyTaskActive() && !_current_task.task->applyCommandParameters(command)) { cmd_result = vehicle_command_ack_s::VEHICLE_RESULT_DENIED; - - // if we just switched and parameters are not accepted, go to failsafe - if (switch_result == 1) { - switchTask(FlightTaskIndex::ManualPosition); - cmd_result = vehicle_command_ack_s::VEHICLE_RESULT_FAILED; - } } } - // send back acknowledgment - vehicle_command_ack_s command_ack = {}; - command_ack.command = command.command; - command_ack.result = cmd_result; - command_ack.result_param1 = switch_result; - command_ack.target_system = command.source_system; - command_ack.target_component = command.source_component; - - if (_pub_vehicle_command_ack == nullptr) { - _pub_vehicle_command_ack = orb_advertise_queue(ORB_ID(vehicle_command_ack), &command_ack, - vehicle_command_ack_s::ORB_QUEUE_LENGTH); + return switch_result; +} - } else { - orb_publish(ORB_ID(vehicle_command_ack), _pub_vehicle_command_ack, &command_ack); +void FlightTasks::handleParameterUpdate() +{ + if (isAnyTaskActive()) { + _current_task.task->handleParameterUpdate(); + } +} +const char *FlightTasks::errorToString(const int error) +{ + for (auto e : _taskError) { + if (e.error == error) { + return e.msg; + } } + + return "This error is not mapped to a string or is unknown."; } diff --git a/src/lib/FlightTasks/FlightTasks.hpp b/src/lib/FlightTasks/FlightTasks.hpp index c795aeb55cbb..f14d769dd9a3 100644 --- a/src/lib/FlightTasks/FlightTasks.hpp +++ b/src/lib/FlightTasks/FlightTasks.hpp @@ -102,6 +102,7 @@ class FlightTasks */ int switchTask(FlightTaskIndex new_task_index); int switchTask(int new_task_index); + int switchTask(const vehicle_command_s &command, uint8_t &cmd_result); /** * Get the number of the active task @@ -157,12 +158,9 @@ class FlightTasks {-3, "Activation Failed"} }; /** - * Check for vehicle commands (received via MAVLink), evaluate and acknowledge them + * Map vehicle command to Flght Task */ - void _updateCommand(); FlightTaskIndex switchVehicleCommand(const int command); - int _sub_vehicle_command = -1; /**< topic handle on which commands are received */ - orb_advert_t _pub_vehicle_command_ack = nullptr; /**< topic handle to which commands get acknowledged */ int _initTask(FlightTaskIndex task_index); }; diff --git a/src/modules/commander/Commander.cpp b/src/modules/commander/Commander.cpp index c83415b9c9f5..e2f78f20bbfc 100644 --- a/src/modules/commander/Commander.cpp +++ b/src/modules/commander/Commander.cpp @@ -991,6 +991,18 @@ Commander::handle_command(vehicle_status_s *status_local, const vehicle_command_ } break; + case vehicle_command_s::VEHICLE_CMD_DO_ORBIT: { + /* ok, home set, use it to take off */ + if (TRANSITION_CHANGED == main_state_transition(*status_local, commander_state_s::MAIN_STATE_CUSTOM, status_flags, &internal_state)) { + cmd_result = vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED; + + } else { + mavlink_log_critical(&mavlink_log_pub, "Custom command rejected"); + cmd_result = vehicle_command_s::VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED; + } + } + break; + case vehicle_command_s::VEHICLE_CMD_CUSTOM_0: case vehicle_command_s::VEHICLE_CMD_CUSTOM_1: case vehicle_command_s::VEHICLE_CMD_CUSTOM_2: @@ -1022,7 +1034,6 @@ Commander::handle_command(vehicle_status_s *status_local, const vehicle_command_ case vehicle_command_s::VEHICLE_CMD_DO_SET_ROI_LOCATION: case vehicle_command_s::VEHICLE_CMD_DO_SET_ROI_WPNEXT_OFFSET: case vehicle_command_s::VEHICLE_CMD_DO_SET_ROI_NONE: - case vehicle_command_s::VEHICLE_CMD_DO_ORBIT: /* ignore commands that are handled by other parts of the system */ break; @@ -3280,6 +3291,7 @@ set_control_mode() switch (status.nav_state) { case vehicle_status_s::NAVIGATION_STATE_MANUAL: + case vehicle_status_s::NAVIGATION_STATE_CUSTOM: control_mode.flag_control_manual_enabled = true; control_mode.flag_control_auto_enabled = false; control_mode.flag_control_rates_enabled = stabilization_required(); diff --git a/src/modules/commander/state_machine_helper.cpp b/src/modules/commander/state_machine_helper.cpp index e0f39ab78a21..23dd1b023052 100644 --- a/src/modules/commander/state_machine_helper.cpp +++ b/src/modules/commander/state_machine_helper.cpp @@ -350,6 +350,13 @@ main_state_transition(const vehicle_status_s &status, const main_state_t new_mai break; + case commander_state_s::MAIN_STATE_CUSTOM: + + // checks will be done in flight tasks + ret = TRANSITION_CHANGED; + + break; + case commander_state_s::MAIN_STATE_MAX: default: break; @@ -531,6 +538,30 @@ bool set_nav_state(vehicle_status_s *status, actuator_armed_s *armed, commander_ } break; + case commander_state_s::MAIN_STATE_CUSTOM: { + + if (rc_lost && is_armed) { + enable_failsafe(status, old_failsafe, mavlink_log_pub, reason_no_rc); + + set_link_loss_nav_state(status, armed, status_flags, internal_state, rc_loss_act, + vehicle_status_s::NAVIGATION_STATE_AUTO_RCRECOVER); + + /* As long as there is RC, we can fallback to ALTCTL, or STAB. */ + /* A local position estimate is enough for POSCTL for multirotors, + * this enables POSCTL using e.g. flow. + * For fixedwing, a global position is needed. */ + + } else if (is_armed + && check_invalid_pos_nav_state(status, old_failsafe, mavlink_log_pub, status_flags, !(posctl_nav_loss_act == 1), + !status->is_rotary_wing)) { + // nothing to do - everything done in check_invalid_pos_nav_state + + } else { + status->nav_state = vehicle_status_s::NAVIGATION_STATE_CUSTOM; + } + } + break; + case commander_state_s::MAIN_STATE_AUTO_MISSION: /* go into failsafe diff --git a/src/modules/mc_pos_control/mc_pos_control_main.cpp b/src/modules/mc_pos_control/mc_pos_control_main.cpp index c9ee900d5e17..cb8466e03858 100644 --- a/src/modules/mc_pos_control/mc_pos_control_main.cpp +++ b/src/modules/mc_pos_control/mc_pos_control_main.cpp @@ -58,6 +58,8 @@ #include #include #include +#include +#include #include #include @@ -98,35 +100,37 @@ class MulticopterPositionControl : public control::SuperBlock, public ModulePara private: - bool _task_should_exit = false; /**) _takeoff_ramp_time, /**< time constant for smooth takeoff ramp */ @@ -145,8 +149,8 @@ class MulticopterPositionControl : public control::SuperBlock, public ModulePara control::BlockDerivative _vel_y_deriv; /**< velocity derivative in y */ control::BlockDerivative _vel_z_deriv; /**< velocity derivative in z */ - FlightTasks _flight_tasks; /**< class that generates position controller tracking setpoints*/ - PositionControl _control; /**< class that handles the core PID position controller */ + FlightTasks _flight_tasks; /**< class that generates position controller tracking setpoints*/ + PositionControl _control; /**< class that handles the core PID position controller */ PositionControlStates _states; /**< structure that contains required state information for position control */ hrt_abstime _last_warn = 0; /**< timer when the last warn message was sent out */ @@ -268,6 +272,11 @@ class MulticopterPositionControl : public control::SuperBlock, public ModulePara */ static int task_main_trampoline(int argc, char *argv[]); + /** + * send acknowledgement of vehicle command + */ + void send_command_ack(const vehicle_command_s &command, const uint8_t &cmd_result, const int &switch_result); + /** * Main sensor collection task. */ @@ -376,6 +385,12 @@ MulticopterPositionControl::poll_subscriptions() } } + orb_check(_vehicle_command_sub, &updated); + + if (updated) { + orb_copy(ORB_ID(vehicle_command), _vehicle_command_sub, &_vehicle_command); + } + orb_check(_vehicle_land_detected_sub, &updated); if (updated) { @@ -514,6 +529,7 @@ MulticopterPositionControl::task_main() { // do subscriptions _vehicle_status_sub = orb_subscribe(ORB_ID(vehicle_status)); + _vehicle_command_sub = orb_subscribe(ORB_ID(vehicle_command)); _vehicle_land_detected_sub = orb_subscribe(ORB_ID(vehicle_land_detected)); _control_mode_sub = orb_subscribe(ORB_ID(vehicle_control_mode)); _params_sub = orb_subscribe(ORB_ID(parameter_update)); @@ -765,8 +781,20 @@ MulticopterPositionControl::start_flight_task() task_failure = true; } - } else if (_control_mode.flag_control_auto_enabled) { - // Auto relate maneuvers + } + + // Auto related maneuvers + if (_vehicle_status.nav_state == vehicle_status_s::NAVIGATION_STATE_AUTO_MISSION || + _vehicle_status.nav_state == vehicle_status_s::NAVIGATION_STATE_AUTO_LOITER || + _vehicle_status.nav_state == vehicle_status_s::NAVIGATION_STATE_AUTO_RTL || + _vehicle_status.nav_state == vehicle_status_s::NAVIGATION_STATE_AUTO_RCRECOVER || + _vehicle_status.nav_state == vehicle_status_s::NAVIGATION_STATE_AUTO_RTGS || + _vehicle_status.nav_state == vehicle_status_s::NAVIGATION_STATE_AUTO_LANDENGFAIL || + _vehicle_status.nav_state == vehicle_status_s::NAVIGATION_STATE_AUTO_TAKEOFF || + _vehicle_status.nav_state == vehicle_status_s::NAVIGATION_STATE_AUTO_LAND || + _vehicle_status.nav_state == vehicle_status_s::NAVIGATION_STATE_AUTO_PRECLAND || + _vehicle_status.nav_state == vehicle_status_s::NAVIGATION_STATE_DESCEND) { + // Auto related maneuvers int error = _flight_tasks.switchTask(FlightTaskIndex::AutoLine); if (error != 0) { @@ -775,6 +803,20 @@ MulticopterPositionControl::start_flight_task() } } + // custom flight task + if (_vehicle_status.nav_state == vehicle_status_s::NAVIGATION_STATE_CUSTOM) { + uint8_t cmd_result; + int error = _flight_tasks.switchTask(_vehicle_command, cmd_result); + + if (error != 0) { + PX4_WARN("Custom command %d activation failed with error: %s", _vehicle_command.command, + _flight_tasks.errorToString(error)); + task_failure = true; + } + + send_command_ack(_vehicle_command, cmd_result, error); + } + // manual position control if (_vehicle_status.nav_state == vehicle_status_s::NAVIGATION_STATE_POSCTL || task_failure) { @@ -1062,6 +1104,27 @@ MulticopterPositionControl::start() return OK; } +void MulticopterPositionControl::send_command_ack(const vehicle_command_s &command, const uint8_t &cmd_result, + const int &switch_result) +{ + // send back acknowledgment + vehicle_command_ack_s command_ack = {}; + command_ack.command = command.command; + command_ack.result = cmd_result; + command_ack.result_param1 = switch_result; + command_ack.target_system = command.source_system; + command_ack.target_component = command.source_component; + + if (_pub_vehicle_command_ack == nullptr) { + _pub_vehicle_command_ack = orb_advertise_queue(ORB_ID(vehicle_command_ack), &command_ack, + vehicle_command_ack_s::ORB_QUEUE_LENGTH); + + } else { + orb_publish(ORB_ID(vehicle_command_ack), _pub_vehicle_command_ack, &command_ack); + + } +} + int mc_pos_control_main(int argc, char *argv[]) { if (argc < 2) {