Mirrors_Framework/PX4-Autopilot
1
0
Fork 0
mirror of https://github.com/PX4/PX4-Autopilot.git synced 2026-05-21 13:02:25 +08:00
Code Issues Actions 4 Packages Projects Releases Wiki Activity

mc_pos_control: split out header and name consistently

Browse Source
This commit is contained in:
Daniel Agar
2020-10-22 10:34:08 -04:00
parent 1fb4f960cd
commit 6285e06fe2
3 changed files with 284 additions and 267 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/mc_pos_control/CMakeLists.txt
+3 -2
View File
@@ -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
src/modules/mc_pos_control/mc_pos_control_main.cpp → src/modules/mc_pos_control/MulticopterPositionControl.cpp
+14 -265
View File
@@ -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 <commander/px4_custom_mode.h>
#include <drivers/drv_hrt.h>
#include <lib/controllib/blocks.hpp>
#include <lib/flight_tasks/FlightTasks.hpp>
#include <lib/hysteresis/hysteresis.h>
#include <lib/mathlib/mathlib.h>
#include <lib/matrix/matrix/math.hpp>
#include <lib/perf/perf_counter.h>
#include <lib/systemlib/mavlink_log.h>
#include <lib/weather_vane/WeatherVane.hpp>
#include <px4_platform_common/px4_config.h>
#include <px4_platform_common/defines.h>
#include <px4_platform_common/module.h>
#include <px4_platform_common/module_params.h>
#include <px4_platform_common/px4_work_queue/ScheduledWorkItem.hpp>
#include <px4_platform_common/posix.h>
#include <px4_platform_common/tasks.h>
#include <uORB/Publication.hpp>
#include <uORB/Subscription.hpp>
#include <uORB/SubscriptionCallback.hpp>
#include <uORB/topics/home_position.h>
#include <uORB/topics/landing_gear.h>
#include <uORB/topics/parameter_update.h>
#include <uORB/topics/vehicle_attitude_setpoint.h>
#include <uORB/topics/vehicle_control_mode.h>
#include <uORB/topics/vehicle_land_detected.h>
#include <uORB/topics/vehicle_local_position.h>
#include <uORB/topics/vehicle_local_position_setpoint.h>
#include <uORB/topics/vehicle_status.h>
#include <uORB/topics/vehicle_trajectory_waypoint.h>
#include <uORB/topics/hover_thrust_estimate.h>
#include "PositionControl/PositionControl.hpp"
#include "Takeoff/Takeoff.hpp"
#include <float.h>
#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<MulticopterPositionControl>, 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_s> _vehicle_attitude_setpoint_pub;
uORB::PublicationQueued<vehicle_command_s> _pub_vehicle_command{ORB_ID(vehicle_command)}; /**< vehicle command publication */
orb_advert_t _mavlink_log_pub{nullptr};
uORB::Publication<landing_gear_s> _landing_gear_pub{ORB_ID(landing_gear)};
uORB::Publication<vehicle_local_position_setpoint_s> _local_pos_sp_pub{ORB_ID(vehicle_local_position_setpoint)}; /**< vehicle local position setpoint publication */
uORB::Publication<vehicle_local_position_setpoint_s> _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<px4::params::MPC_XY_P>) _param_mpc_xy_p,
(ParamFloat<px4::params::MPC_Z_P>) _param_mpc_z_p,
(ParamFloat<px4::params::MPC_XY_VEL_P_ACC>) _param_mpc_xy_vel_p_acc,
(ParamFloat<px4::params::MPC_XY_VEL_I_ACC>) _param_mpc_xy_vel_i_acc,
(ParamFloat<px4::params::MPC_XY_VEL_D_ACC>) _param_mpc_xy_vel_d_acc,
(ParamFloat<px4::params::MPC_Z_VEL_P_ACC>) _param_mpc_z_vel_p_acc,
(ParamFloat<px4::params::MPC_Z_VEL_I_ACC>) _param_mpc_z_vel_i_acc,
(ParamFloat<px4::params::MPC_Z_VEL_D_ACC>) _param_mpc_z_vel_d_acc,
(ParamFloat<px4::params::MPC_XY_VEL_MAX>) _param_mpc_xy_vel_max,
(ParamFloat<px4::params::MPC_Z_VEL_MAX_UP>) _param_mpc_z_vel_max_up,
(ParamFloat<px4::params::MPC_Z_VEL_MAX_DN>) _param_mpc_z_vel_max_dn,
(ParamFloat<px4::params::MPC_TILTMAX_AIR>) _param_mpc_tiltmax_air,
(ParamFloat<px4::params::MPC_THR_HOVER>) _param_mpc_thr_hover,
(ParamBool<px4::params::MPC_USE_HTE>) _param_mpc_use_hte,
// Takeoff / Land
(ParamFloat<px4::params::MPC_SPOOLUP_TIME>) _param_mpc_spoolup_time, /**< time to let motors spool up after arming */
(ParamFloat<px4::params::MPC_TKO_RAMP_T>) _param_mpc_tko_ramp_t, /**< time constant for smooth takeoff ramp */
(ParamFloat<px4::params::MPC_TKO_SPEED>) _param_mpc_tko_speed,
(ParamFloat<px4::params::MPC_LAND_SPEED>) _param_mpc_land_speed,
(ParamFloat<px4::params::MPC_VEL_MANUAL>) _param_mpc_vel_manual,
(ParamFloat<px4::params::MPC_XY_CRUISE>) _param_mpc_xy_cruise,
(ParamFloat<px4::params::MPC_LAND_ALT2>) _param_mpc_land_alt2, /**< downwards speed limited below this altitude */
(ParamInt<px4::params::MPC_POS_MODE>) _param_mpc_pos_mode,
(ParamInt<px4::params::MPC_ALT_MODE>) _param_mpc_alt_mode,
(ParamFloat<px4::params::MPC_TILTMAX_LND>) _param_mpc_tiltmax_lnd, /**< maximum tilt for landing and smooth takeoff */
(ParamFloat<px4::params::MPC_THR_MIN>) _param_mpc_thr_min,
(ParamFloat<px4::params::MPC_THR_MAX>) _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);
}
src/modules/mc_pos_control/MulticopterPositionControl.hpp
+267
View File
@@ -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 <commander/px4_custom_mode.h>
#include <drivers/drv_hrt.h>
#include <lib/controllib/blocks.hpp>
#include <lib/flight_tasks/FlightTasks.hpp>
#include <lib/hysteresis/hysteresis.h>
#include <lib/mathlib/mathlib.h>
#include <lib/matrix/matrix/math.hpp>
#include <lib/perf/perf_counter.h>
#include <lib/systemlib/mavlink_log.h>
#include <lib/weather_vane/WeatherVane.hpp>
#include <px4_platform_common/px4_config.h>
#include <px4_platform_common/defines.h>
#include <px4_platform_common/module.h>
#include <px4_platform_common/module_params.h>
#include <px4_platform_common/px4_work_queue/ScheduledWorkItem.hpp>
#include <px4_platform_common/posix.h>
#include <px4_platform_common/tasks.h>
#include <uORB/Publication.hpp>
#include <uORB/Subscription.hpp>
#include <uORB/SubscriptionCallback.hpp>
#include <uORB/topics/home_position.h>
#include <uORB/topics/landing_gear.h>
#include <uORB/topics/parameter_update.h>
#include <uORB/topics/vehicle_attitude_setpoint.h>
#include <uORB/topics/vehicle_control_mode.h>
#include <uORB/topics/vehicle_land_detected.h>
#include <uORB/topics/vehicle_local_position.h>
#include <uORB/topics/vehicle_local_position_setpoint.h>
#include <uORB/topics/vehicle_status.h>
#include <uORB/topics/vehicle_trajectory_waypoint.h>
#include <uORB/topics/hover_thrust_estimate.h>
#include "PositionControl/PositionControl.hpp"
#include "Takeoff/Takeoff.hpp"
#include <float.h>
class MulticopterPositionControl : public ModuleBase<MulticopterPositionControl>, 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_s> _vehicle_attitude_setpoint_pub;
uORB::PublicationQueued<vehicle_command_s> _pub_vehicle_command{ORB_ID(vehicle_command)}; /**< vehicle command publication */
orb_advert_t _mavlink_log_pub{nullptr};
uORB::Publication<landing_gear_s> _landing_gear_pub{ORB_ID(landing_gear)};
uORB::Publication<vehicle_local_position_setpoint_s> _local_pos_sp_pub{ORB_ID(vehicle_local_position_setpoint)}; /**< vehicle local position setpoint publication */
uORB::Publication<vehicle_local_position_setpoint_s> _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<px4::params::MPC_XY_P>) _param_mpc_xy_p,
(ParamFloat<px4::params::MPC_Z_P>) _param_mpc_z_p,
(ParamFloat<px4::params::MPC_XY_VEL_P_ACC>) _param_mpc_xy_vel_p_acc,
(ParamFloat<px4::params::MPC_XY_VEL_I_ACC>) _param_mpc_xy_vel_i_acc,
(ParamFloat<px4::params::MPC_XY_VEL_D_ACC>) _param_mpc_xy_vel_d_acc,
(ParamFloat<px4::params::MPC_Z_VEL_P_ACC>) _param_mpc_z_vel_p_acc,
(ParamFloat<px4::params::MPC_Z_VEL_I_ACC>) _param_mpc_z_vel_i_acc,
(ParamFloat<px4::params::MPC_Z_VEL_D_ACC>) _param_mpc_z_vel_d_acc,
(ParamFloat<px4::params::MPC_XY_VEL_MAX>) _param_mpc_xy_vel_max,
(ParamFloat<px4::params::MPC_Z_VEL_MAX_UP>) _param_mpc_z_vel_max_up,
(ParamFloat<px4::params::MPC_Z_VEL_MAX_DN>) _param_mpc_z_vel_max_dn,
(ParamFloat<px4::params::MPC_TILTMAX_AIR>) _param_mpc_tiltmax_air,
(ParamFloat<px4::params::MPC_THR_HOVER>) _param_mpc_thr_hover,
(ParamBool<px4::params::MPC_USE_HTE>) _param_mpc_use_hte,
// Takeoff / Land
(ParamFloat<px4::params::MPC_SPOOLUP_TIME>) _param_mpc_spoolup_time, /**< time to let motors spool up after arming */
(ParamFloat<px4::params::MPC_TKO_RAMP_T>) _param_mpc_tko_ramp_t, /**< time constant for smooth takeoff ramp */
(ParamFloat<px4::params::MPC_TKO_SPEED>) _param_mpc_tko_speed,
(ParamFloat<px4::params::MPC_LAND_SPEED>) _param_mpc_land_speed,
(ParamFloat<px4::params::MPC_VEL_MANUAL>) _param_mpc_vel_manual,
(ParamFloat<px4::params::MPC_XY_CRUISE>) _param_mpc_xy_cruise,
(ParamFloat<px4::params::MPC_LAND_ALT2>) _param_mpc_land_alt2, /**< downwards speed limited below this altitude */
(ParamInt<px4::params::MPC_POS_MODE>) _param_mpc_pos_mode,
(ParamInt<px4::params::MPC_ALT_MODE>) _param_mpc_alt_mode,
(ParamFloat<px4::params::MPC_TILTMAX_LND>) _param_mpc_tiltmax_lnd, /**< maximum tilt for landing and smooth takeoff */
(ParamFloat<px4::params::MPC_THR_MIN>) _param_mpc_thr_min,
(ParamFloat<px4::params::MPC_THR_MAX>) _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);
};