fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-22 20:53:35 +08:00
Code Issues Packages Projects Releases Wiki Activity

fw pos ctrl: some incremental clean up of the class

Browse Source 
- documentation of units, params, returns, and descriptions for variables and methods
- rename ambiguous or erroneous state variables
- remove unused or unecessary input arguments to functions
- remove ugly header white space
This commit is contained in:
Thomas Stastny
2022-06-09 09:23:02 +02:00
committed by Silvan Fuhrer
parent 9863c24b40
commit 594a6d6e80
2 changed files with 354 additions and 176 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/fw_pos_control_l1/FixedwingPositionControl.hpp
+308 -129
View File
@@ -103,25 +103,38 @@ using namespace time_literals;
using matrix::Vector2d;
using matrix::Vector2f;
static constexpr float HDG_HOLD_DIST_NEXT =
3000.0f; // initial distance of waypoint in front of plane in heading hold mode
static constexpr float HDG_HOLD_REACHED_DIST =
1000.0f; // distance (plane to waypoint in front) at which waypoints are reset in heading hold mode
static constexpr float HDG_HOLD_SET_BACK_DIST = 100.0f; // distance by which previous waypoint is set behind the plane
static constexpr float HDG_HOLD_YAWRATE_THRESH = 0.15f; // max yawrate at which plane locks yaw for heading hold mode
static constexpr float HDG_HOLD_MAN_INPUT_THRESH =
0.01f; // max manual roll/yaw input from user which does not change the locked heading
// [m] initial distance of waypoint in front of plane in heading hold mode
static constexpr float HDG_HOLD_DIST_NEXT = 3000.0f;
static constexpr hrt_abstime T_ALT_TIMEOUT = 1_s; // time after which we abort landing if terrain estimate is not valid
// [m] distance (plane to waypoint in front) at which waypoints are reset in heading hold mode
static constexpr float HDG_HOLD_REACHED_DIST = 1000.0f;
static constexpr float THROTTLE_THRESH =
0.05f; ///< max throttle from user which will not lead to motors spinning up in altitude controlled modes
static constexpr float ASPD_SP_SLEW_RATE = 1.f; // slew rate limit for airspeed setpoint changes [m/s/S]
static constexpr hrt_abstime T_WIND_EST_TIMEOUT =
10_s; // time after which the wind estimate is disabled if no longer updating
// [m] distance by which previous waypoint is set behind the plane
static constexpr float HDG_HOLD_SET_BACK_DIST = 100.0f;
static constexpr float MIN_AUTO_TIMESTEP = 0.01f; // minimum time step between auto control updates [s]
static constexpr float MAX_AUTO_TIMESTEP = 0.05f; // maximum time step between auto control updates [s]
// [rad/s] max yawrate at which plane locks yaw for heading hold mode
static constexpr float HDG_HOLD_YAWRATE_THRESH = 0.15f;
// [.] max manual roll/yaw normalized input from user which does not change the locked heading
static constexpr float HDG_HOLD_MAN_INPUT_THRESH = 0.01f;
// [us] time after which we abort landing if terrain estimate is not valid
static constexpr hrt_abstime TERRAIN_ALT_TIMEOUT = 1_s;
// [.] max throttle from user which will not lead to motors spinning up in altitude controlled modes
static constexpr float THROTTLE_THRESH = 0.05f;
// [m/s/s] slew rate limit for airspeed setpoint changes
static constexpr float ASPD_SP_SLEW_RATE = 1.f;
// [us] time after which the wind estimate is disabled if no longer updating
static constexpr hrt_abstime WIND_EST_TIMEOUT = 10_s;
// [s] minimum time step between auto control updates
static constexpr float MIN_AUTO_TIMESTEP = 0.01f;
// [s] maximum time step between auto control updates
static constexpr float MAX_AUTO_TIMESTEP = 0.05f;
class FixedwingPositionControl final : public ModuleBase<FixedwingPositionControl>, public ModuleParams,
public px4::WorkItem
@@ -164,44 +177,45 @@ private:
uORB::Subscription _vehicle_land_detected_sub{ORB_ID(vehicle_land_detected)};
uORB::Subscription _vehicle_status_sub{ORB_ID(vehicle_status)};
uORB::Publication<vehicle_attitude_setpoint_s> _attitude_sp_pub;
uORB::Publication<vehicle_local_position_setpoint_s> _local_pos_sp_pub{ORB_ID(vehicle_local_position_setpoint)}; ///< vehicle local position setpoint publication
uORB::Publication<npfg_status_s> _npfg_status_pub{ORB_ID(npfg_status)}; ///< NPFG status publication
uORB::Publication<position_controller_status_s> _pos_ctrl_status_pub{ORB_ID(position_controller_status)}; ///< navigation capabilities publication
uORB::Publication<position_controller_landing_status_s> _pos_ctrl_landing_status_pub{ORB_ID(position_controller_landing_status)}; ///< landing status publication
uORB::Publication<tecs_status_s> _tecs_status_pub{ORB_ID(tecs_status)}; ///< TECS status publication
uORB::PublicationMulti<orbit_status_s> _orbit_status_pub{ORB_ID(orbit_status)};
uORB::Publication<vehicle_attitude_setpoint_s> _attitude_sp_pub;
uORB::Publication<vehicle_local_position_setpoint_s> _local_pos_sp_pub{ORB_ID(vehicle_local_position_setpoint)};
uORB::Publication<npfg_status_s> _npfg_status_pub{ORB_ID(npfg_status)};
uORB::Publication<position_controller_status_s> _pos_ctrl_status_pub{ORB_ID(position_controller_status)};
uORB::Publication<position_controller_landing_status_s> _pos_ctrl_landing_status_pub{ORB_ID(position_controller_landing_status)};
uORB::Publication<tecs_status_s> _tecs_status_pub{ORB_ID(tecs_status)};
uORB::PublicationMulti<orbit_status_s> _orbit_status_pub{ORB_ID(orbit_status)};
manual_control_setpoint_s _manual_control_setpoint {}; ///< r/c channel data
position_setpoint_triplet_s _pos_sp_triplet {}; ///< triplet of mission items
vehicle_attitude_setpoint_s _att_sp {}; ///< vehicle attitude setpoint
vehicle_control_mode_s _control_mode {}; ///< control mode
vehicle_local_position_s _local_pos {}; ///< vehicle local position
vehicle_status_s _vehicle_status {}; ///< vehicle status
manual_control_setpoint_s _manual_control_setpoint {}; // r/c channel data
position_setpoint_triplet_s _pos_sp_triplet {}; // triplet of mission items
vehicle_attitude_setpoint_s _att_sp {}; // vehicle attitude setpoint
vehicle_control_mode_s _control_mode {};
vehicle_local_position_s _local_pos {}; // vehicle local position
vehicle_status_s _vehicle_status {}; // vehicle status
double _current_latitude{0};
double _current_longitude{0};
float _current_altitude{0.f};
perf_counter_t _loop_perf; ///< loop performance counter
perf_counter_t _loop_perf; // loop performance counter
MapProjection _global_local_proj_ref{};
float _global_local_alt0{NAN};
float _global_local_alt0{NAN};
float _takeoff_ground_alt{0.0f}; ///< ground altitude at which plane was launched
float _hdg_hold_yaw{0.0f}; ///< hold heading for velocity mode
bool _hdg_hold_enabled{false}; ///< heading hold enabled
bool _yaw_lock_engaged{false}; ///< yaw is locked for heading hold
// [m] ground altitude where the plane was launched
float _takeoff_ground_alt{0.0f};
float _min_current_sp_distance_xy{FLT_MAX};
// [rad] yaw setpoint for manual position mode heading hold
float _hdg_hold_yaw{0.0f};
position_setpoint_s _hdg_hold_prev_wp {}; ///< position where heading hold started
position_setpoint_s _hdg_hold_curr_wp {}; ///< position to which heading hold flies
bool _hdg_hold_enabled{false}; // heading hold enabled
bool _yaw_lock_engaged{false}; // yaw is locked for heading hold
/**
* @brief Last absolute time position control has been called [us]
*
*/
float _min_current_sp_distance_xy{FLT_MAX};
position_setpoint_s _hdg_hold_prev_wp {}; // position where heading hold started
position_setpoint_s _hdg_hold_curr_wp {}; // position to which heading hold flies
// [us] Last absolute time position control has been called
hrt_abstime _last_time_position_control_called{0};
bool _landed{true};
@@ -216,38 +230,60 @@ private:
Landingslope _landingslope;
hrt_abstime _time_started_landing{0}; ///< time at which landing started
// [us] time at which landing started
hrt_abstime _time_started_landing{0};
float _t_alt_prev_valid{0}; ///< last terrain estimate which was valid
hrt_abstime _time_last_t_alt{0}; ///< time at which we had last valid terrain alt
// [m] last terrain estimate which was valid
float _last_valid_terrain_alt_estimate{0.0f};
float _flare_height{0.0f}; ///< estimated height to ground at which flare started
float _flare_pitch_sp{0.0f}; ///< Current forced (i.e. not determined using TECS) flare pitch setpoint
// [us] time at which we had last valid terrain alt
hrt_abstime _last_time_terrain_alt_was_valid{0};
// [m] estimated height to ground at which flare started
float _flare_height{0.0f};
// [m] current forced (i.e. not determined using TECS) flare pitch setpoint
float _flare_pitch_sp{0.0f};
// [m] estimated height to ground at which flare started
float _flare_curve_alt_rel_last{0.0f};
float _target_bearing{0.0f}; ///< estimated height to ground at which flare started
bool _was_in_air{false}; ///< indicated wether the plane was in the air in the previous interation*/
hrt_abstime _time_went_in_air{0}; ///< time at which the plane went in the air
float _target_bearing{0.0f}; // [rad]
/* Takeoff launch detection and runway */
// indicates whether the plane was in the air in the previous interation
bool _was_in_air{false};
// [us] time at which the plane went in the air
hrt_abstime _time_went_in_air{0};
// Takeoff launch detection and runway
LaunchDetector _launchDetector;
LaunchDetectionResult _launch_detection_state{LAUNCHDETECTION_RES_NONE};
hrt_abstime _launch_detection_notify{0};
RunwayTakeoff _runway_takeoff;
bool _last_manual{false}; ///< true if the last iteration was in manual mode (used to determine when a reset is needed)
// true if the last iteration was in manual mode (used to determine when a reset is needed)
bool _last_manual{false};
/* throttle and airspeed states */
bool _airspeed_valid{false}; ///< flag if a valid airspeed estimate exists
hrt_abstime _airspeed_last_valid{0}; ///< last time airspeed was received. Used to detect timeouts.
bool _airspeed_valid{false};
// [us] last time airspeed was received. used to detect timeouts.
hrt_abstime _time_airspeed_last_valid{0};
float _airspeed{0.0f};
float _eas2tas{1.0f};
/* wind estimates */
Vector2f _wind_vel{0.0f, 0.0f}; ///< wind velocity vector [m/s]
bool _wind_valid{false}; ///< flag if a valid wind estimate exists
hrt_abstime _time_wind_last_received{0}; ///< last time wind estimate was received in microseconds. Used to detect timeouts.
// [m/s] wind velocity vector
Vector2f _wind_vel{0.0f, 0.0f};
bool _wind_valid{false};
hrt_abstime _time_wind_last_received{0}; // [us]
float _pitch{0.0f};
float _yaw{0.0f};
@@ -256,32 +292,48 @@ private:
matrix::Vector3f _body_acceleration{};
matrix::Vector3f _body_velocity{};
bool _reinitialize_tecs{true}; ///< indicates if the TECS states should be reinitialized (used for VTOL)
bool _reinitialize_tecs{true};
bool _is_tecs_running{false};
hrt_abstime _last_tecs_update{0};
float _asp_after_transition{0.0f};
hrt_abstime _time_last_tecs_update{0}; // [us]
float _airspeed_after_transition{0.0f};
bool _was_in_transition{false};
bool _vtol_tailsitter{false};
bool _is_vtol_tailsitter{false};
matrix::Vector2d _transition_waypoint{(double)NAN, (double)NAN};
// estimator reset counters
uint8_t _pos_reset_counter{0}; ///< captures the number of times the estimator has reset the horizontal position
uint8_t _alt_reset_counter{0}; ///< captures the number of times the estimator has reset the altitude state
float _manual_control_setpoint_altitude{0.0f};
float _manual_control_setpoint_airspeed{0.0f};
float _commanded_airspeed_setpoint{NAN}; ///< airspeed setpoint for manual modes commanded via MAV_CMD_DO_CHANGE_SPEED
// captures the number of times the estimator has reset the horizontal position
uint8_t _pos_reset_counter{0};
hrt_abstime _time_in_fixed_bank_loiter{0};
// captures the number of times the estimator has reset the altitude state
uint8_t _alt_reset_counter{0};
ECL_L1_Pos_Controller _l1_control;
// [.] normalized setpoint for manual altitude control [-1,1]; -1,0,1 maps to min,zero,max height rate commands
float _manual_control_setpoint_for_height_rate{0.0f};
// [.] normalized setpoint for manual airspeed control [0,1]; 0,0.5,1 maps to min,cruise,max airspeed commands
float _manual_control_setpoint_for_airspeed{0.0f};
// [m/s] airspeed setpoint for manual modes commanded via MAV_CMD_DO_CHANGE_SPEED
float _commanded_airspeed_setpoint{NAN};
hrt_abstime _time_in_fixed_bank_loiter{0}; // [us]
// L1 guidance - lateral-directional position control
ECL_L1_Pos_Controller _l1_control;
// nonlinear path following guidance - lateral-directional position control
NPFG _npfg;
TECS _tecs;
// total energy control system - airspeed / altitude control
TECS _tecs;
uint8_t _position_sp_type{0};
enum FW_POSCTRL_MODE {
FW_POSCTRL_MODE_AUTO,
FW_POSCTRL_MODE_AUTO_ALTITUDE,
@@ -291,10 +343,11 @@ private:
FW_POSCTRL_MODE_MANUAL_POSITION,
FW_POSCTRL_MODE_MANUAL_ALTITUDE,
FW_POSCTRL_MODE_OTHER
} _control_mode_current{FW_POSCTRL_MODE_OTHER}; ///< used to check the mode in the last control loop iteration. Use to check if the last iteration was in the same mode.
} _control_mode_current{FW_POSCTRL_MODE_OTHER}; // used to check if the mode has changed
param_t _param_handle_airspeed_trans{PARAM_INVALID};
float _param_airspeed_trans{NAN};
float _param_airspeed_trans{NAN}; // [m/s]
enum StickConfig {
STICK_CONFIG_SWAP_STICKS_BIT = (1 << 0),
@@ -302,54 +355,61 @@ private:
};
// Update our local parameter cache.
int parameters_update();
int parameters_update();
// Update subscriptions
void airspeed_poll();
void control_update();
void manual_control_setpoint_poll();
void vehicle_attitude_poll();
void vehicle_command_poll();
void vehicle_control_mode_poll();
void vehicle_status_poll();
void wind_poll();
void airspeed_poll();
void control_update();
void manual_control_setpoint_poll();
void vehicle_attitude_poll();
void vehicle_command_poll();
void vehicle_control_mode_poll();
void vehicle_status_poll();
void wind_poll();
void status_publish();
void landing_status_publish();
void tecs_status_publish();
void publishLocalPositionSetpoint(const position_setpoint_s &current_waypoint);
void status_publish();
void landing_status_publish();
void tecs_status_publish();
void publishLocalPositionSetpoint(const position_setpoint_s &current_waypoint);
void abort_landing(bool abort);
void abort_landing(bool abort);
/**
* Get a new waypoint based on heading and distance from current position
* @brief Get a new waypoint based on heading and distance from current position
*
* @param heading the heading to fly to
* @param distance the distance of the generated waypoint
* @param waypoint_prev the waypoint at the current position
* @param waypoint_next the waypoint in the heading direction
*/
void get_waypoint_heading_distance(float heading, position_setpoint_s &waypoint_prev,
position_setpoint_s &waypoint_next, bool flag_init);
void get_waypoint_heading_distance(float heading, position_setpoint_s &waypoint_prev,
position_setpoint_s &waypoint_next, bool flag_init);
/**
* Return the terrain estimate during takeoff or takeoff_alt if terrain estimate is not available
* @brief Return the terrain estimate during takeoff or takeoff_alt if terrain estimate is not available
*
* @param takeoff_alt Altitude AMSL at launch or when runway takeoff is detected [m]
*/
float get_terrain_altitude_takeoff(float takeoff_alt);
float get_terrain_altitude_takeoff(float takeoff_alt);
/**
* @brief Maps the manual control setpoint (pilot sticks) to height rate commands
*
* @return Manual height rate setpoint [m/s]
*/
float getManualHeightRateSetpoint();
/**
* Check if we are in a takeoff situation
* @brief Check if we are in a takeoff situation
*/
bool in_takeoff_situation();
bool in_takeoff_situation();
/**
* Update desired altitude base on user pitch stick input
* @brief Update desired altitude base on user pitch stick input
*
* @param dt Time step
*/
void update_desired_altitude(float dt);
void update_desired_altitude(float dt);
/**
* @brief Updates timing information for landed and in-air states.
@@ -362,30 +422,89 @@ private:
* @brief Moves the current position setpoint to a value far ahead of the current vehicle yaw when in a VTOL
* transition.
*
* @param[in,out] current_sp current position setpoint
* @param[in,out] current_sp Current position setpoint
*/
void move_position_setpoint_for_vtol_transition(position_setpoint_s &current_sp);
uint8_t handle_setpoint_type(const uint8_t setpoint_type, const position_setpoint_s &pos_sp_curr);
void control_auto(const float control_interval, const Vector2d &curr_pos, const Vector2f &ground_speed,
const position_setpoint_s &pos_sp_prev, const position_setpoint_s &pos_sp_curr, const position_setpoint_s &pos_sp_next);
/**
* @brief Changes the position setpoint type to achieve the desired behavior in some instances.
*
* @param pos_sp_curr Current position setpoint
* @return Adjusted position setpoint type
*/
uint8_t handle_setpoint_type(const position_setpoint_s &pos_sp_curr);
void control_auto_fixed_bank_alt_hold(const float control_interval);
void control_auto_descend(const float control_interval);
void control_auto_position(const float control_interval, const Vector2d &curr_pos, const Vector2f &ground_speed,
const position_setpoint_s &pos_sp_prev, const position_setpoint_s &pos_sp_curr);
void control_auto_loiter(const float control_interval, const Vector2d &curr_pos, const Vector2f &ground_speed,
const position_setpoint_s &pos_sp_prev, const position_setpoint_s &pos_sp_curr, const position_setpoint_s &pos_sp_next);
void control_auto_velocity(const float control_interval, const Vector2d &curr_pos, const Vector2f &ground_speed,
const position_setpoint_s &pos_sp_prev, const position_setpoint_s &pos_sp_curr);
/* automatic control methods */
/**
* @brief Vehicle control while in takeoff
* @brief Position control for all automatic modes except takeoff and landing
*
* @param control_interval Time since last position control call [s]
* @param curr_pos Current 2D local position vector of vehicle [m]
* @param ground_speed Local 2D ground speed of vehicle [m/s]
* @param pos_sp_prev previous position setpoint
* @param pos_sp_curr current position setpoint
* @param pos_sp_next next position setpoint
*/
void control_auto(const float control_interval, const Vector2d &curr_pos, const Vector2f &ground_speed,
const position_setpoint_s &pos_sp_prev, const position_setpoint_s &pos_sp_curr, const position_setpoint_s &pos_sp_next);
/**
* @brief Controls altitude and airspeed for a fixed-bank loiter.
*
* @param control_interval Time since last position control call [s]
*/
void control_auto_fixed_bank_alt_hold(const float control_interval);
/**
* @brief Control airspeed with a fixed descent rate and roll angle.
*
* @param control_interval Time since last position control call [s]
*/
void control_auto_descend(const float control_interval);
/**
* @brief Vehicle control for position waypoints.
*
* @param control_interval Time since last position control call [s]
* @param curr_pos Current 2D local position vector of vehicle [m]
* @param ground_speed Local 2D ground speed of vehicle [m/s]
* @param pos_sp_prev previous position setpoint
* @param pos_sp_curr current position setpoint
*/
void control_auto_position(const float control_interval, const Vector2d &curr_pos, const Vector2f &ground_speed,
const position_setpoint_s &pos_sp_prev, const position_setpoint_s &pos_sp_curr);
/**
* @brief Vehicle control for loiter waypoints.
*
* @param control_interval Time since last position control call [s]
* @param curr_pos Current 2D local position vector of vehicle [m]
* @param ground_speed Local 2D ground speed of vehicle [m/s]
* @param pos_sp_prev previous position setpoint
* @param pos_sp_curr current position setpoint
* @param pos_sp_next next position setpoint
*/
void control_auto_loiter(const float control_interval, const Vector2d &curr_pos, const Vector2f &ground_speed,
const position_setpoint_s &pos_sp_prev, const position_setpoint_s &pos_sp_curr, const position_setpoint_s &pos_sp_next);
/**
* @brief Controls a desired airspeed, bearing, and height rate.
*
* @param control_interval Time since last position control call [s]
* @param curr_pos Current 2D local position vector of vehicle [m]
* @param ground_speed Local 2D ground speed of vehicle [m/s]
* @param pos_sp_curr current position setpoint
*/
void control_auto_velocity(const float control_interval, const Vector2d &curr_pos, const Vector2f &ground_speed,
const position_setpoint_s &pos_sp_curr);
/**
* @brief Controls automatic takeoff.
*
* @param now Current system time [us]
* @param control_interval Time since last position control call [s]
* @param curr_pos Current 2D local position vector of vehicle [m]
* @param control_interval Time since the last position control update [s]
* @param ground_speed Local 2D ground speed of vehicle [m/s]
* @param pos_sp_prev previous position setpoint
* @param pos_sp_curr current position setpoint
@@ -393,43 +512,103 @@ private:
void control_auto_takeoff(const hrt_abstime &now, const float control_interval, const Vector2d &curr_pos,
const Vector2f &ground_speed, const position_setpoint_s &pos_sp_prev, const position_setpoint_s &pos_sp_curr);
void control_auto_landing(const hrt_abstime &now, const float control_interval, const Vector2d &curr_pos,
const Vector2f &ground_speed,
const position_setpoint_s &pos_sp_prev,
const position_setpoint_s &pos_sp_curr);
void control_manual_altitude(const float control_interval, const Vector2d &curr_pos, const Vector2f &ground_speed);
void control_manual_position(const float control_interval, const Vector2d &curr_pos, const Vector2f &ground_speed);
/**
* @brief Controls automatic landing.
*
* @param now Current system time [us]
* @param control_interval Time since last position control call [s]
* @param curr_pos Current 2D local position vector of vehicle [m]
* @param control_interval Time since the last position control update [s]
* @param ground_speed Local 2D ground speed of vehicle [m/s]
* @param pos_sp_prev previous position setpoint
* @param pos_sp_curr current position setpoint
*/
void control_auto_landing(const hrt_abstime &now, const float control_interval, const Vector2d &curr_pos,
const Vector2f &ground_speed, const position_setpoint_s &pos_sp_prev, const position_setpoint_s &pos_sp_curr);
float get_tecs_pitch();
float get_tecs_thrust();
float get_manual_airspeed_setpoint();
/* manual control methods */
/**
* @brief Returns an indicated airspeed setpoint for auto modes.
* @brief Controls altitude and airspeed, user commands roll setpoint.
*
* @param control_interval Time since last position control call [s]
* @param curr_pos Current 2D local position vector of vehicle [m]
* @param ground_speed Local 2D ground speed of vehicle [m/s]
*/
void control_manual_altitude(const float control_interval, const Vector2d &curr_pos, const Vector2f &ground_speed);
/**
* @brief Controls user commanded altitude, airspeed, and bearing.
*
* @param control_interval Time since last position control call [s]
* @param curr_pos Current 2D local position vector of vehicle [m]
* @param ground_speed Local 2D ground speed of vehicle [m/s]
*/
void control_manual_position(const float control_interval, const Vector2d &curr_pos, const Vector2f &ground_speed);
float get_tecs_pitch();
float get_tecs_thrust();
float get_manual_airspeed_setpoint();
/**
* @brief Returns an calibrated airspeed setpoint for auto modes.
*
* Adjusts the setpoint for wind, accelerated stall, and slew rates.
*
* @param control_interval Time since the last position control update [s]
* @param pos_sp_cru_airspeed The commanded cruise airspeed from the position setpoint [s]
* @param ground_speed Vehicle ground velocity vector [m/s]
* @return Indicated airspeed setpoint [m/s]
* @return Calibrated airspeed setpoint [m/s]
*/
float get_auto_airspeed_setpoint(const float control_interval, const float pos_sp_cru_airspeed,
const Vector2f &ground_speed);
void reset_takeoff_state(bool force = false);
void reset_landing_state();
bool using_npfg_with_wind_estimate() const;
Vector2f get_nav_speed_2d(const Vector2f &ground_speed);
void set_control_mode_current(const hrt_abstime &now, bool pos_sp_curr_valid);
void reset_takeoff_state(bool force = false);
void reset_landing_state();
bool using_npfg_with_wind_estimate() const;
/**
* @brief Returns the velocity vector to be input in the lateral-directional guidance laws.
*
* Replaces the ground velocity vector with an air velocity vector depending on the wind condition and whether
* NPFG or L1 are being used for horizontal position control.
*
* @param ground_speed Vehicle ground velocity vector [m/s]
* @return Velocity vector to control with lateral-directional guidance [m/s]
*/
Vector2f get_nav_speed_2d(const Vector2f &ground_speed);
/**
* @brief Decides which control mode to execute.
*
* May also change the position setpoint type depending on the desired behavior.
*
* @param now Current system time [us]
* @param pos_sp_curr_valid True if the current position setpoint is valid
*/
void set_control_mode_current(const hrt_abstime &now, bool pos_sp_curr_valid);
void publishOrbitStatus(const position_setpoint_s pos_sp);
SlewRate<float> _slew_rate_airspeed;
SlewRate<float> _airspeed_slew_rate_controller;
/*
* Call TECS : a wrapper function to call the TECS implementation
/**
* @brief A wrapper function to call the TECS implementation
*
* @param control_interval Time since the last position control update [s]
* @param alt_sp Altitude setpoint, AMSL [m]
* @param airspeed_sp Calibrated airspeed setpoint [m/s]
* @param pitch_min_rad Nominal pitch angle command minimum [rad]
* @param pitch_max_rad Nominal pitch angle command maximum [rad]
* @param throttle_min Minimum throttle command [0,1]
* @param throttle_max Maximum throttle command [0,1]
* @param throttle_cruise Cruise throttle command [0,1]
* @param climbout_mode True if TECS should engage climbout mode
* @param climbout_pitch_min_rad Minimum pitch angle command in climbout mode [rad]
* @param disable_underspeed_detection True if underspeed detection should be disabled
* @param hgt_rate_sp Height rate setpoint [m/s]
*/
void tecs_update_pitch_throttle(const float control_interval, float alt_sp, float airspeed_sp,
float pitch_min_rad, float pitch_max_rad,