@ -103,25 +103,38 @@ using namespace time_literals;
@@ -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
static constexpr hrt_abstime T_ALT_TIMEOUT = 1 _s ; // time after which we abort landing if terrain estimate is not valid
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
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]
// [m] initial distance of waypoint in front of plane in heading hold mode
static constexpr float HDG_HOLD_DIST_NEXT = 3000.0f ;
// [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 ;
// [m] distance by which previous waypoint is set behind the plane
static constexpr float HDG_HOLD_SET_BACK_DIST = 100.0f ;
// [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:
@@ -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 ) } ;
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
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 { } ;
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 start ed
position_setpoint_s _hdg_hold_curr_wp { } ; ///< position to which heading hold flies
bool _hdg_hold_enabled { false } ; // heading hold enabl ed
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:
@@ -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]
// indicates whether the plane was in the air in the previous interation
bool _was_in_air { false } ;
/* Takeoff launch detection and runway */
// [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:
@@ -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 } ;
// captures the number of times the estimator has reset the altitude state
uint8_t _alt_reset_counter { 0 } ;
// [.] 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 } ;
hrt_abstime _time_in_fixed_bank_loiter { 0 } ;
// [m/s] airspeed setpoint for manual modes commanded via MAV_CMD_DO_CHANGE_SPEED
float _commanded_airspeed_setpoint { NAN } ;
ECL_L1_Pos_Controller _l1_control ;
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:
@@ -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:
@@ -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 status_publish ( ) ;
void landing_status_publish ( ) ;
void tecs_status_publish ( ) ;
void publishLocalPositionSetpoint ( const position_setpoint_s & current_waypoint ) ;
void abort_landing ( bool abort ) ;
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 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:
@@ -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 c urrentposition setpoint
* @ param [ in , out ] current_sp C urrentposition 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 ) ;
/* automatic control methods */
/**
* @ 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 2 D local position vector of vehicle [ m ]
* @ param ground_speed Local 2 D 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 2 D local position vector of vehicle [ m ]
* @ param ground_speed Local 2 D 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 ) ;
void control_auto_fixed_bank_alt_hold ( const float control_interval ) ;
void control_auto_descend ( const float control_interval ) ;
/**
* @ brief Vehicle control for loiter waypoints .
*
* @ param control_interval Time since last position control call [ s ]
* @ param curr_pos Current 2 D local position vector of vehicle [ m ]
* @ param ground_speed Local 2 D 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 ) ;
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 ) ;
/**
* @ brief Controls a desired airspeed , bearing , and height rate .
*
* @ param control_interval Time since last position control call [ s ]
* @ param curr_pos Current 2 D local position vector of vehicle [ m ]
* @ param ground_speed Local 2 D 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 Vehicle control while in takeoff
* @ brief Controls automatic takeoff .
*
* @ param now Current system time [ us ]
* @ param control_interval Time since last position control call [ s ]
* @ param curr_pos Current 2 D local position vector of vehicle [ m ]
* @ param control_interval Time since the last position control update [ s ]
* @ param ground_speed Local 2 D 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:
@@ -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 2 D local position vector of vehicle [ m ]
* @ param control_interval Time since the last position control update [ s ]
* @ param ground_speed Local 2 D 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 ) ;
/* manual control methods */
float get_tecs_pitch ( ) ;
float get_tecs_thrust ( ) ;
/**
* @ brief Controls altitude and airspeed , user commands roll setpoint .
*
* @ param control_interval Time since last position control call [ s ]
* @ param curr_pos Current 2 D local position vector of vehicle [ m ]
* @ param ground_speed Local 2 D ground speed of vehicle [ m / s ]
*/
void control_manual_altitude ( const float control_interval , const Vector2d & curr_pos , const Vector2f & ground_speed ) ;
float get_manual_airspeed_setpoint ( ) ;
/**
* @ brief Controls user commanded altitude , airspeed , and bearing .
*
* @ param control_interval Time since last position control call [ s ]
* @ param curr_pos Current 2 D local position vector of vehicle [ m ]
* @ param ground_speed Local 2 D 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 indicated airspeed setpoint for auto modes .
* @ brief Returns an calibr atedairspeed 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 Calibr atedairspeed 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 ,
Thomas Stastny
3 years ago
committed by
Silvan Fuhrer