diff --git a/src/modules/mc_pos_control/CMakeLists.txt b/src/modules/mc_pos_control/CMakeLists.txt index a6394aced8..552b65c23a 100644 --- a/src/modules/mc_pos_control/CMakeLists.txt +++ b/src/modules/mc_pos_control/CMakeLists.txt @@ -1,6 +1,6 @@ ############################################################################ # -# Copyright (c) 2015 PX4 Development Team. All rights reserved. +# Copyright (c) 2015-2020 PX4 Development Team. All rights reserved. # # Redistribution and use in source and binary forms, with or without # modification, are permitted provided that the following conditions @@ -40,7 +40,8 @@ px4_add_module( COMPILE_FLAGS -Wno-implicit-fallthrough # TODO: fix and remove SRCS - mc_pos_control_main.cpp + MulticopterPositionControl.cpp + MulticopterPositionControl.hpp DEPENDS PositionControl controllib diff --git a/src/modules/mc_pos_control/mc_pos_control_main.cpp b/src/modules/mc_pos_control/MulticopterPositionControl.cpp similarity index 71% rename from src/modules/mc_pos_control/mc_pos_control_main.cpp rename to src/modules/mc_pos_control/MulticopterPositionControl.cpp index 1874282032..7abc8be07f 100644 --- a/src/modules/mc_pos_control/mc_pos_control_main.cpp +++ b/src/modules/mc_pos_control/MulticopterPositionControl.cpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2013 - 2019 PX4 Development Team. All rights reserved. + * Copyright (c) 2013-2020 PX4 Development Team. All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -31,248 +31,11 @@ * ****************************************************************************/ -/** - * @file mc_pos_control_main.cpp - * Multicopter position controller. - */ - -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include - -#include "PositionControl/PositionControl.hpp" -#include "Takeoff/Takeoff.hpp" - -#include +#include "MulticopterPositionControl.hpp" using namespace time_literals; using namespace matrix; -/** - * Multicopter position control app start / stop handling function - */ -extern "C" __EXPORT int mc_pos_control_main(int argc, char *argv[]); - -class MulticopterPositionControl : public ModuleBase, public control::SuperBlock, - public ModuleParams, public px4::WorkItem -{ -public: - MulticopterPositionControl(bool vtol = false); - ~MulticopterPositionControl() override; - - /** @see ModuleBase */ - static int task_spawn(int argc, char *argv[]); - - /** @see ModuleBase */ - static int custom_command(int argc, char *argv[]); - - /** @see ModuleBase */ - static int print_usage(const char *reason = nullptr); - - bool init(); - - /** @see ModuleBase::print_status() */ - int print_status() override; - -private: - void Run() override; - - Takeoff _takeoff; /**< state machine and ramp to bring the vehicle off the ground without jumps */ - - uORB::Publication _vehicle_attitude_setpoint_pub; - uORB::PublicationQueued _pub_vehicle_command{ORB_ID(vehicle_command)}; /**< vehicle command publication */ - orb_advert_t _mavlink_log_pub{nullptr}; - - uORB::Publication _landing_gear_pub{ORB_ID(landing_gear)}; - uORB::Publication _local_pos_sp_pub{ORB_ID(vehicle_local_position_setpoint)}; /**< vehicle local position setpoint publication */ - uORB::Publication _traj_sp_pub{ORB_ID(trajectory_setpoint)}; /**< trajectory setpoints publication */ - - uORB::SubscriptionCallbackWorkItem _local_pos_sub{this, ORB_ID(vehicle_local_position)}; /**< vehicle local position */ - - uORB::Subscription _vehicle_status_sub{ORB_ID(vehicle_status)}; /**< vehicle status subscription */ - uORB::Subscription _vehicle_land_detected_sub{ORB_ID(vehicle_land_detected)}; /**< vehicle land detected subscription */ - uORB::Subscription _control_mode_sub{ORB_ID(vehicle_control_mode)}; /**< vehicle control mode subscription */ - uORB::Subscription _parameter_update_sub{ORB_ID(parameter_update)}; /**< notification of parameter updates */ - uORB::Subscription _home_pos_sub{ORB_ID(home_position)}; /**< home position */ - uORB::Subscription _hover_thrust_estimate_sub{ORB_ID(hover_thrust_estimate)}; - - hrt_abstime _time_stamp_last_loop{0}; /**< time stamp of last loop iteration */ - - int _task_failure_count{0}; /**< counter for task failures */ - - vehicle_status_s _vehicle_status{}; /**< vehicle status */ - /**< vehicle-land-detection: initialze to landed */ - vehicle_land_detected_s _vehicle_land_detected = { - .timestamp = 0, - .alt_max = -1.0f, - .freefall = false, - .ground_contact = true, - .maybe_landed = true, - .landed = true, - }; - - vehicle_control_mode_s _control_mode{}; /**< vehicle control mode */ - vehicle_local_position_s _local_pos{}; /**< vehicle local position */ - home_position_s _home_pos{}; /**< home position */ - landing_gear_s _landing_gear{}; - int8_t _old_landing_gear_position{landing_gear_s::GEAR_KEEP}; - - DEFINE_PARAMETERS( - // Position Control - (ParamFloat) _param_mpc_xy_p, - (ParamFloat) _param_mpc_z_p, - (ParamFloat) _param_mpc_xy_vel_p_acc, - (ParamFloat) _param_mpc_xy_vel_i_acc, - (ParamFloat) _param_mpc_xy_vel_d_acc, - (ParamFloat) _param_mpc_z_vel_p_acc, - (ParamFloat) _param_mpc_z_vel_i_acc, - (ParamFloat) _param_mpc_z_vel_d_acc, - (ParamFloat) _param_mpc_xy_vel_max, - (ParamFloat) _param_mpc_z_vel_max_up, - (ParamFloat) _param_mpc_z_vel_max_dn, - (ParamFloat) _param_mpc_tiltmax_air, - (ParamFloat) _param_mpc_thr_hover, - (ParamBool) _param_mpc_use_hte, - - // Takeoff / Land - (ParamFloat) _param_mpc_spoolup_time, /**< time to let motors spool up after arming */ - (ParamFloat) _param_mpc_tko_ramp_t, /**< time constant for smooth takeoff ramp */ - (ParamFloat) _param_mpc_tko_speed, - (ParamFloat) _param_mpc_land_speed, - - (ParamFloat) _param_mpc_vel_manual, - (ParamFloat) _param_mpc_xy_cruise, - (ParamFloat) _param_mpc_land_alt2, /**< downwards speed limited below this altitude */ - (ParamInt) _param_mpc_pos_mode, - (ParamInt) _param_mpc_alt_mode, - (ParamFloat) _param_mpc_tiltmax_lnd, /**< maximum tilt for landing and smooth takeoff */ - (ParamFloat) _param_mpc_thr_min, - (ParamFloat) _param_mpc_thr_max - ); - - control::BlockDerivative _vel_x_deriv; /**< velocity derivative in x */ - control::BlockDerivative _vel_y_deriv; /**< velocity derivative in y */ - control::BlockDerivative _vel_z_deriv; /**< velocity derivative in z */ - - FlightTasks _flight_tasks; /**< class generating position controller setpoints depending on vehicle task */ - PositionControl _control; /**< class for core PID position control */ - PositionControlStates _states{}; /**< structure containing vehicle state information for position control */ - - hrt_abstime _last_warn = 0; /**< timer when the last warn message was sent out */ - - bool _in_failsafe = false; /**< true if failsafe was entered within current cycle */ - - bool _hover_thrust_initialized{false}; - - /** Timeout in us for trajectory data to get considered invalid */ - static constexpr uint64_t TRAJECTORY_STREAM_TIMEOUT_US = 500_ms; - /** number of tries before switching to a failsafe flight task */ - static constexpr int NUM_FAILURE_TRIES = 10; - /** If Flighttask fails, keep 0.2 seconds the current setpoint before going into failsafe land */ - static constexpr uint64_t LOITER_TIME_BEFORE_DESCEND = 200_ms; - /** During smooth-takeoff, below ALTITUDE_THRESHOLD the yaw-control is turned off ant tilt is limited */ - static constexpr float ALTITUDE_THRESHOLD = 0.3f; - - static constexpr float MAX_SAFE_TILT_DEG = 89.f; // Numerical issues above this value due to tanf - - systemlib::Hysteresis _failsafe_land_hysteresis{false}; /**< becomes true if task did not update correctly for LOITER_TIME_BEFORE_DESCEND */ - - WeatherVane *_wv_controller{nullptr}; - Vector3f _wv_dcm_z_sp_prev{0, 0, 1}; - - perf_counter_t _cycle_perf; - - /** - * Update our local parameter cache. - * Parameter update can be forced when argument is true. - * @param force forces parameter update. - */ - int parameters_update(bool force); - - /** - * Check for changes in subscribed topics. - */ - void poll_subscriptions(); - - /** - * Check for validity of positon/velocity states. - * @param vel_sp_z velocity setpoint in z-direction - */ - void set_vehicle_states(const float &vel_sp_z); - - /** - * Limit altitude based on land-detector. - * @param setpoint needed to detect vehicle intention. - */ - void limit_altitude(vehicle_local_position_setpoint_s &setpoint); - - /** - * Adjust the setpoint during landing. - * Thrust is adjusted to support the land-detector during detection. - * @param setpoint gets adjusted based on land-detector state - */ - void limit_thrust_during_landing(vehicle_attitude_setpoint_s &setpoint); - - /** - * Start flightasks based on navigation state. - * This methods activates a task based on the navigation state. - */ - void start_flight_task(); - - /** - * Failsafe. - * If flighttask fails for whatever reason, then do failsafe. This could - * occur if the commander fails to switch to a mode in case of invalid states or - * setpoints. The failsafe will occur after LOITER_TIME_BEFORE_DESCEND. If force is set - * to true, the failsafe will be initiated immediately. - */ - void failsafe(const hrt_abstime &now, vehicle_local_position_setpoint_s &setpoint, const PositionControlStates &states, - const bool force, bool warn); - - /** - * Reset setpoints to NAN - */ - void reset_setpoint_to_nan(vehicle_local_position_setpoint_s &setpoint); - - /** - * check if task should be switched because of failsafe - */ - void check_failure(bool task_failure, uint8_t nav_state); - - /** - * send vehicle command to inform commander about failsafe - */ - void send_vehicle_cmd_do(uint8_t nav_state); -}; - MulticopterPositionControl::MulticopterPositionControl(bool vtol) : SuperBlock(nullptr, "MPC"), ModuleParams(nullptr), @@ -302,8 +65,7 @@ MulticopterPositionControl::~MulticopterPositionControl() perf_free(_cycle_perf); } -bool -MulticopterPositionControl::init() +bool MulticopterPositionControl::init() { if (!_local_pos_sub.registerCallback()) { PX4_ERR("vehicle_local_position callback registration failed!"); @@ -318,8 +80,7 @@ MulticopterPositionControl::init() return true; } -int -MulticopterPositionControl::parameters_update(bool force) +int MulticopterPositionControl::parameters_update(bool force) { // check for parameter updates if (_parameter_update_sub.updated() || force) { @@ -397,8 +158,7 @@ MulticopterPositionControl::parameters_update(bool force) return OK; } -void -MulticopterPositionControl::poll_subscriptions() +void MulticopterPositionControl::poll_subscriptions() { _vehicle_status_sub.update(&_vehicle_status); _vehicle_land_detected_sub.update(&_vehicle_land_detected); @@ -416,8 +176,7 @@ MulticopterPositionControl::poll_subscriptions() } } -void -MulticopterPositionControl::limit_altitude(vehicle_local_position_setpoint_s &setpoint) +void MulticopterPositionControl::limit_altitude(vehicle_local_position_setpoint_s &setpoint) { if (_vehicle_land_detected.alt_max < 0.0f || !_home_pos.valid_alt || !_local_pos.v_z_valid) { // there is no altitude limitation present or the required information not available @@ -434,13 +193,8 @@ MulticopterPositionControl::limit_altitude(vehicle_local_position_setpoint_s &se } } -void -MulticopterPositionControl::set_vehicle_states(const float &vel_sp_z) +void MulticopterPositionControl::set_vehicle_states(const float &vel_sp_z) { - if (_local_pos.timestamp == 0) { - return; - } - // only set position states if valid and finite if (PX4_ISFINITE(_local_pos.x) && PX4_ISFINITE(_local_pos.y) && _local_pos.xy_valid) { _states.position(0) = _local_pos.x; @@ -494,8 +248,7 @@ MulticopterPositionControl::set_vehicle_states(const float &vel_sp_z) } } -int -MulticopterPositionControl::print_status() +int MulticopterPositionControl::print_status() { if (_flight_tasks.isAnyTaskActive()) { PX4_INFO("Running, active flight task: %i", _flight_tasks.getActiveTask()); @@ -509,8 +262,7 @@ MulticopterPositionControl::print_status() return 0; } -void -MulticopterPositionControl::Run() +void MulticopterPositionControl::Run() { if (should_exit()) { _local_pos_sub.unregisterCallback(); @@ -713,8 +465,7 @@ MulticopterPositionControl::Run() perf_end(_cycle_perf); } -void -MulticopterPositionControl::start_flight_task() +void MulticopterPositionControl::start_flight_task() { bool task_failure = false; bool should_disable_task = true; @@ -919,9 +670,8 @@ MulticopterPositionControl::start_flight_task() } } -void -MulticopterPositionControl::failsafe(const hrt_abstime &now, vehicle_local_position_setpoint_s &setpoint, - const PositionControlStates &states, const bool force, bool warn) +void MulticopterPositionControl::failsafe(const hrt_abstime &now, vehicle_local_position_setpoint_s &setpoint, + const PositionControlStates &states, const bool force, bool warn) { // do not warn while we are disarmed, as we might not have valid setpoints yet if (!_control_mode.flag_armed) { @@ -974,8 +724,7 @@ MulticopterPositionControl::failsafe(const hrt_abstime &now, vehicle_local_posit } } -void -MulticopterPositionControl::reset_setpoint_to_nan(vehicle_local_position_setpoint_s &setpoint) +void MulticopterPositionControl::reset_setpoint_to_nan(vehicle_local_position_setpoint_s &setpoint) { setpoint.x = setpoint.y = setpoint.z = NAN; setpoint.vx = setpoint.vy = setpoint.vz = NAN; @@ -1097,7 +846,7 @@ logging. return 0; } -int mc_pos_control_main(int argc, char *argv[]) +extern "C" __EXPORT int mc_pos_control_main(int argc, char *argv[]) { return MulticopterPositionControl::main(argc, argv); } diff --git a/src/modules/mc_pos_control/MulticopterPositionControl.hpp b/src/modules/mc_pos_control/MulticopterPositionControl.hpp new file mode 100644 index 0000000000..a1fca96374 --- /dev/null +++ b/src/modules/mc_pos_control/MulticopterPositionControl.hpp @@ -0,0 +1,267 @@ +/**************************************************************************** + * + * Copyright (c) 2013-2020 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * Multicopter position controller. + */ + +#pragma once + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include "PositionControl/PositionControl.hpp" +#include "Takeoff/Takeoff.hpp" + +#include + +class MulticopterPositionControl : public ModuleBase, public control::SuperBlock, + public ModuleParams, public px4::WorkItem +{ +public: + MulticopterPositionControl(bool vtol = false); + ~MulticopterPositionControl() override; + + /** @see ModuleBase */ + static int task_spawn(int argc, char *argv[]); + + /** @see ModuleBase */ + static int custom_command(int argc, char *argv[]); + + /** @see ModuleBase */ + static int print_usage(const char *reason = nullptr); + + bool init(); + + /** @see ModuleBase::print_status() */ + int print_status() override; + +private: + void Run() override; + + Takeoff _takeoff; /**< state machine and ramp to bring the vehicle off the ground without jumps */ + + uORB::Publication _vehicle_attitude_setpoint_pub; + uORB::PublicationQueued _pub_vehicle_command{ORB_ID(vehicle_command)}; /**< vehicle command publication */ + orb_advert_t _mavlink_log_pub{nullptr}; + + uORB::Publication _landing_gear_pub{ORB_ID(landing_gear)}; + uORB::Publication _local_pos_sp_pub{ORB_ID(vehicle_local_position_setpoint)}; /**< vehicle local position setpoint publication */ + uORB::Publication _traj_sp_pub{ORB_ID(trajectory_setpoint)}; /**< trajectory setpoints publication */ + + uORB::SubscriptionCallbackWorkItem _local_pos_sub{this, ORB_ID(vehicle_local_position)}; /**< vehicle local position */ + + uORB::Subscription _vehicle_status_sub{ORB_ID(vehicle_status)}; /**< vehicle status subscription */ + uORB::Subscription _vehicle_land_detected_sub{ORB_ID(vehicle_land_detected)}; /**< vehicle land detected subscription */ + uORB::Subscription _control_mode_sub{ORB_ID(vehicle_control_mode)}; /**< vehicle control mode subscription */ + uORB::Subscription _parameter_update_sub{ORB_ID(parameter_update)}; /**< notification of parameter updates */ + uORB::Subscription _home_pos_sub{ORB_ID(home_position)}; /**< home position */ + uORB::Subscription _hover_thrust_estimate_sub{ORB_ID(hover_thrust_estimate)}; + + hrt_abstime _time_stamp_last_loop{0}; /**< time stamp of last loop iteration */ + + int _task_failure_count{0}; /**< counter for task failures */ + + vehicle_status_s _vehicle_status{}; /**< vehicle status */ + /**< vehicle-land-detection: initialze to landed */ + vehicle_land_detected_s _vehicle_land_detected = { + .timestamp = 0, + .alt_max = -1.0f, + .freefall = false, + .ground_contact = true, + .maybe_landed = true, + .landed = true, + }; + + vehicle_control_mode_s _control_mode{}; /**< vehicle control mode */ + vehicle_local_position_s _local_pos{}; /**< vehicle local position */ + home_position_s _home_pos{}; /**< home position */ + landing_gear_s _landing_gear{}; + int8_t _old_landing_gear_position{landing_gear_s::GEAR_KEEP}; + + DEFINE_PARAMETERS( + // Position Control + (ParamFloat) _param_mpc_xy_p, + (ParamFloat) _param_mpc_z_p, + (ParamFloat) _param_mpc_xy_vel_p_acc, + (ParamFloat) _param_mpc_xy_vel_i_acc, + (ParamFloat) _param_mpc_xy_vel_d_acc, + (ParamFloat) _param_mpc_z_vel_p_acc, + (ParamFloat) _param_mpc_z_vel_i_acc, + (ParamFloat) _param_mpc_z_vel_d_acc, + (ParamFloat) _param_mpc_xy_vel_max, + (ParamFloat) _param_mpc_z_vel_max_up, + (ParamFloat) _param_mpc_z_vel_max_dn, + (ParamFloat) _param_mpc_tiltmax_air, + (ParamFloat) _param_mpc_thr_hover, + (ParamBool) _param_mpc_use_hte, + + // Takeoff / Land + (ParamFloat) _param_mpc_spoolup_time, /**< time to let motors spool up after arming */ + (ParamFloat) _param_mpc_tko_ramp_t, /**< time constant for smooth takeoff ramp */ + (ParamFloat) _param_mpc_tko_speed, + (ParamFloat) _param_mpc_land_speed, + + (ParamFloat) _param_mpc_vel_manual, + (ParamFloat) _param_mpc_xy_cruise, + (ParamFloat) _param_mpc_land_alt2, /**< downwards speed limited below this altitude */ + (ParamInt) _param_mpc_pos_mode, + (ParamInt) _param_mpc_alt_mode, + (ParamFloat) _param_mpc_tiltmax_lnd, /**< maximum tilt for landing and smooth takeoff */ + (ParamFloat) _param_mpc_thr_min, + (ParamFloat) _param_mpc_thr_max + ); + + control::BlockDerivative _vel_x_deriv; /**< velocity derivative in x */ + control::BlockDerivative _vel_y_deriv; /**< velocity derivative in y */ + control::BlockDerivative _vel_z_deriv; /**< velocity derivative in z */ + + FlightTasks _flight_tasks; /**< class generating position controller setpoints depending on vehicle task */ + PositionControl _control; /**< class for core PID position control */ + PositionControlStates _states{}; /**< structure containing vehicle state information for position control */ + + hrt_abstime _last_warn = 0; /**< timer when the last warn message was sent out */ + + bool _in_failsafe = false; /**< true if failsafe was entered within current cycle */ + + bool _hover_thrust_initialized{false}; + + /** Timeout in us for trajectory data to get considered invalid */ + static constexpr uint64_t TRAJECTORY_STREAM_TIMEOUT_US = 500_ms; + /** number of tries before switching to a failsafe flight task */ + static constexpr int NUM_FAILURE_TRIES = 10; + /** If Flighttask fails, keep 0.2 seconds the current setpoint before going into failsafe land */ + static constexpr uint64_t LOITER_TIME_BEFORE_DESCEND = 200_ms; + /** During smooth-takeoff, below ALTITUDE_THRESHOLD the yaw-control is turned off ant tilt is limited */ + static constexpr float ALTITUDE_THRESHOLD = 0.3f; + + static constexpr float MAX_SAFE_TILT_DEG = 89.f; // Numerical issues above this value due to tanf + + systemlib::Hysteresis _failsafe_land_hysteresis{false}; /**< becomes true if task did not update correctly for LOITER_TIME_BEFORE_DESCEND */ + + WeatherVane *_wv_controller{nullptr}; + Vector3f _wv_dcm_z_sp_prev{0, 0, 1}; + + perf_counter_t _cycle_perf; + + /** + * Update our local parameter cache. + * Parameter update can be forced when argument is true. + * @param force forces parameter update. + */ + int parameters_update(bool force); + + /** + * Check for changes in subscribed topics. + */ + void poll_subscriptions(); + + /** + * Check for validity of positon/velocity states. + * @param vel_sp_z velocity setpoint in z-direction + */ + void set_vehicle_states(const float &vel_sp_z); + + /** + * Limit altitude based on land-detector. + * @param setpoint needed to detect vehicle intention. + */ + void limit_altitude(vehicle_local_position_setpoint_s &setpoint); + + /** + * Adjust the setpoint during landing. + * Thrust is adjusted to support the land-detector during detection. + * @param setpoint gets adjusted based on land-detector state + */ + void limit_thrust_during_landing(vehicle_attitude_setpoint_s &setpoint); + + /** + * Start flightasks based on navigation state. + * This methods activates a task based on the navigation state. + */ + void start_flight_task(); + + /** + * Failsafe. + * If flighttask fails for whatever reason, then do failsafe. This could + * occur if the commander fails to switch to a mode in case of invalid states or + * setpoints. The failsafe will occur after LOITER_TIME_BEFORE_DESCEND. If force is set + * to true, the failsafe will be initiated immediately. + */ + void failsafe(const hrt_abstime &now, vehicle_local_position_setpoint_s &setpoint, const PositionControlStates &states, + const bool force, bool warn); + + /** + * Reset setpoints to NAN + */ + void reset_setpoint_to_nan(vehicle_local_position_setpoint_s &setpoint); + + /** + * check if task should be switched because of failsafe + */ + void check_failure(bool task_failure, uint8_t nav_state); + + /** + * send vehicle command to inform commander about failsafe + */ + void send_vehicle_cmd_do(uint8_t nav_state); +};