fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-03 08:38:53 +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.cpp
+46 -47
View File
@@ -67,7 +67,7 @@ FixedwingPositionControl::FixedwingPositionControl(bool vtol) :
_pos_ctrl_landing_status_pub.advertise();
_tecs_status_pub.advertise();
_slew_rate_airspeed.setSlewRate(ASPD_SP_SLEW_RATE);
_airspeed_slew_rate_controller.setSlewRate(ASPD_SP_SLEW_RATE);
/* fetch initial parameter values */
parameters_update();
@@ -283,7 +283,7 @@ FixedwingPositionControl::airspeed_poll()
airspeed_valid = true;
_airspeed_last_valid = airspeed_validated.timestamp;
_time_airspeed_last_valid = airspeed_validated.timestamp;
_airspeed = airspeed_validated.calibrated_airspeed_m_s;
_eas2tas = constrain(airspeed_validated.true_airspeed_m_s / airspeed_validated.calibrated_airspeed_m_s, 0.9f, 2.0f);
@@ -291,7 +291,7 @@ FixedwingPositionControl::airspeed_poll()
} else {
// no airspeed updates for one second
if (airspeed_valid && (hrt_elapsed_time(&_airspeed_last_valid) > 1_s)) {
if (airspeed_valid && (hrt_elapsed_time(&_time_airspeed_last_valid) > 1_s)) {
airspeed_valid = false;
}
}
@@ -321,7 +321,7 @@ FixedwingPositionControl::wind_poll()
} else {
// invalidate wind estimate usage (and correspondingly NPFG, if enabled) after subscription timeout
_wind_valid = _wind_valid && (hrt_absolute_time() - _time_wind_last_received) < T_WIND_EST_TIMEOUT;
_wind_valid = _wind_valid && (hrt_absolute_time() - _time_wind_last_received) < WIND_EST_TIMEOUT;
}
}
@@ -330,21 +330,21 @@ FixedwingPositionControl::manual_control_setpoint_poll()
{
_manual_control_setpoint_sub.update(&_manual_control_setpoint);
_manual_control_setpoint_altitude = _manual_control_setpoint.x;
_manual_control_setpoint_airspeed = math::constrain(_manual_control_setpoint.z, 0.0f, 1.0f);
_manual_control_setpoint_for_height_rate = _manual_control_setpoint.x;
_manual_control_setpoint_for_airspeed = math::constrain(_manual_control_setpoint.z, 0.0f, 1.0f);
if (_param_fw_pos_stk_conf.get() & STICK_CONFIG_SWAP_STICKS_BIT) {
/* Alternate stick allocation (similar concept as for multirotor systems:
* demanding up/down with the throttle stick, and move faster/break with the pitch one.
*/
_manual_control_setpoint_altitude = -(math::constrain(_manual_control_setpoint.z, 0.0f, 1.0f) * 2.f - 1.f);
_manual_control_setpoint_airspeed = math::constrain(_manual_control_setpoint.x, -1.0f, 1.0f) / 2.f + 0.5f;
_manual_control_setpoint_for_height_rate = -(math::constrain(_manual_control_setpoint.z, 0.0f, 1.0f) * 2.f - 1.f);
_manual_control_setpoint_for_airspeed = math::constrain(_manual_control_setpoint.x, -1.0f, 1.0f) / 2.f + 0.5f;
}
// send neutral setpoints if no update for 1 s
if (hrt_elapsed_time(&_manual_control_setpoint.timestamp) > 1_s) {
_manual_control_setpoint_altitude = 0.f;
_manual_control_setpoint_airspeed = 0.5f;
_manual_control_setpoint_for_height_rate = 0.f;
_manual_control_setpoint_for_airspeed = 0.5f;
}
}
@@ -393,17 +393,17 @@ FixedwingPositionControl::get_manual_airspeed_setpoint()
if (_param_fw_pos_stk_conf.get() & STICK_CONFIG_ENABLE_AIRSPEED_SP_MANUAL_BIT) {
// neutral throttle corresponds to trim airspeed
if (_manual_control_setpoint_airspeed < 0.5f) {
if (_manual_control_setpoint_for_airspeed < 0.5f) {
// lower half of throttle is min to trim airspeed
altctrl_airspeed = _param_fw_airspd_min.get() +
(_param_fw_airspd_trim.get() - _param_fw_airspd_min.get()) *
_manual_control_setpoint_airspeed * 2;
_manual_control_setpoint_for_airspeed * 2;
} else {
// upper half of throttle is trim to max airspeed
altctrl_airspeed = _param_fw_airspd_trim.get() +
(_param_fw_airspd_max.get() - _param_fw_airspd_trim.get()) *
(_manual_control_setpoint_airspeed * 2 - 1);
(_manual_control_setpoint_for_airspeed * 2 - 1);
}
} else if (PX4_ISFINITE(_commanded_airspeed_setpoint)) {
@@ -460,15 +460,15 @@ FixedwingPositionControl::get_auto_airspeed_setpoint(const float control_interva
airspeed_setpoint = constrain(airspeed_setpoint, airspeed_min_adjusted, _param_fw_airspd_max.get());
// initialize to current airspeed setpoint, also if previous setpoint is out of bounds to not apply slew rate in that case
const bool outside_of_limits = _slew_rate_airspeed.getState() < airspeed_min_adjusted
|| _slew_rate_airspeed.getState() > _param_fw_airspd_max.get();
const bool outside_of_limits = _airspeed_slew_rate_controller.getState() < airspeed_min_adjusted
|| _airspeed_slew_rate_controller.getState() > _param_fw_airspd_max.get();
if (!PX4_ISFINITE(_slew_rate_airspeed.getState()) || outside_of_limits) {
_slew_rate_airspeed.setForcedValue(airspeed_setpoint);
if (!PX4_ISFINITE(_airspeed_slew_rate_controller.getState()) || outside_of_limits) {
_airspeed_slew_rate_controller.setForcedValue(airspeed_setpoint);
} else if (control_interval > FLT_EPSILON) {
// constrain airspeed setpoint changes with slew rate of ASPD_SP_SLEW_RATE m/s/s
airspeed_setpoint = _slew_rate_airspeed.update(airspeed_setpoint, control_interval);
airspeed_setpoint = _airspeed_slew_rate_controller.update(airspeed_setpoint, control_interval);
}
return airspeed_setpoint;
@@ -707,14 +707,14 @@ FixedwingPositionControl::getManualHeightRateSetpoint()
* an axis. Positive X means to rotate positively around
* the X axis in NED frame, which is pitching down
*/
if (_manual_control_setpoint_altitude > deadBand) {
if (_manual_control_setpoint_for_height_rate > deadBand) {
/* pitching down */
float pitch = -(_manual_control_setpoint_altitude - deadBand) / factor;
float pitch = -(_manual_control_setpoint_for_height_rate - deadBand) / factor;
height_rate_setpoint = pitch * _param_sinkrate_target.get();
} else if (_manual_control_setpoint_altitude < - deadBand) {
} else if (_manual_control_setpoint_for_height_rate < - deadBand) {
/* pitching up */
float pitch = -(_manual_control_setpoint_altitude + deadBand) / factor;
float pitch = -(_manual_control_setpoint_for_height_rate + deadBand) / factor;
const float climb_rate_target = _param_climbrate_target.get();
height_rate_setpoint = pitch * climb_rate_target;
@@ -882,7 +882,7 @@ FixedwingPositionControl::control_auto(const float control_interval, const Vecto
position_setpoint_s current_sp = pos_sp_curr;
move_position_setpoint_for_vtol_transition(current_sp);
const uint8_t position_sp_type = handle_setpoint_type(current_sp.type, current_sp);
const uint8_t position_sp_type = handle_setpoint_type(current_sp);
_position_sp_type = position_sp_type;
@@ -908,7 +908,7 @@ FixedwingPositionControl::control_auto(const float control_interval, const Vecto
break;
case position_setpoint_s::SETPOINT_TYPE_VELOCITY:
control_auto_velocity(control_interval, curr_pos, ground_speed, pos_sp_prev, current_sp);
control_auto_velocity(control_interval, curr_pos, ground_speed, current_sp);
break;
case position_setpoint_s::SETPOINT_TYPE_LOITER:
@@ -974,8 +974,6 @@ FixedwingPositionControl::control_auto_fixed_bank_alt_hold(const float control_i
void
FixedwingPositionControl::control_auto_descend(const float control_interval)
{
// only control height rate
// Hard-code descend rate to 0.5m/s. This is a compromise to give the system to recover,
// but not letting it drift too far away.
const float descend_rate = -0.5f;
@@ -1004,8 +1002,10 @@ FixedwingPositionControl::control_auto_descend(const float control_interval)
}
uint8_t
FixedwingPositionControl::handle_setpoint_type(const uint8_t setpoint_type, const position_setpoint_s &pos_sp_curr)
FixedwingPositionControl::handle_setpoint_type(const position_setpoint_s &pos_sp_curr)
{
uint8_t position_sp_type = pos_sp_curr.type;
if (!_control_mode.flag_control_position_enabled && _control_mode.flag_control_velocity_enabled) {
return position_setpoint_s::SETPOINT_TYPE_VELOCITY;
}
@@ -1017,8 +1017,6 @@ FixedwingPositionControl::handle_setpoint_type(const uint8_t setpoint_type, cons
const float acc_rad = (_param_fw_use_npfg.get()) ? _npfg.switchDistance(500.0f) : _l1_control.switch_distance(500.0f);
uint8_t position_sp_type = setpoint_type;
if (pos_sp_curr.type == position_setpoint_s::SETPOINT_TYPE_POSITION
|| pos_sp_curr.type == position_setpoint_s::SETPOINT_TYPE_LOITER) {
@@ -1068,7 +1066,6 @@ FixedwingPositionControl::control_auto_position(const float control_interval, co
Vector2d curr_wp{0, 0};
Vector2d prev_wp{0, 0};
/* current waypoint (the one currently heading for) */
curr_wp = Vector2d(pos_sp_curr.lat, pos_sp_curr.lon);
@@ -1145,6 +1142,7 @@ FixedwingPositionControl::control_auto_position(const float control_interval, co
}
float target_airspeed = get_auto_airspeed_setpoint(control_interval, pos_sp_curr.cruising_speed, ground_speed);
Vector2f curr_pos_local{_local_pos.x, _local_pos.y};
Vector2f curr_wp_local = _global_local_proj_ref.project(curr_wp(0), curr_wp(1));
Vector2f prev_wp_local = _global_local_proj_ref.project(prev_wp(0), prev_wp(1));
@@ -1192,7 +1190,7 @@ FixedwingPositionControl::control_auto_position(const float control_interval, co
void
FixedwingPositionControl::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)
const Vector2f &ground_speed, const position_setpoint_s &pos_sp_curr)
{
float tecs_fw_thr_min;
float tecs_fw_thr_max;
@@ -1733,10 +1731,10 @@ FixedwingPositionControl::control_auto_landing(const hrt_abstime &now, const flo
// all good, have valid terrain altitude
float terrain_vpos = _local_pos.dist_bottom + _local_pos.z;
terrain_alt = (_local_pos.ref_alt - terrain_vpos);
_t_alt_prev_valid = terrain_alt;
_time_last_t_alt = now;
_last_valid_terrain_alt_estimate = terrain_alt;
_last_time_terrain_alt_was_valid = now;
} else if (_time_last_t_alt == 0) {
} else if (_last_time_terrain_alt_was_valid == 0) {
// we have started landing phase but don't have valid terrain
// wait for some time, maybe we will soon get a valid estimate
// until then just use the altitude of the landing waypoint
@@ -1749,16 +1747,16 @@ FixedwingPositionControl::control_auto_landing(const hrt_abstime &now, const flo
abort_landing(true);
}
} else if ((!_local_pos.dist_bottom_valid && (now - _time_last_t_alt) < T_ALT_TIMEOUT)
} else if ((!_local_pos.dist_bottom_valid && (now - _last_time_terrain_alt_was_valid) < TERRAIN_ALT_TIMEOUT)
|| _land_noreturn_vertical) {
// use previous terrain estimate for some time and hope to recover
// if we are already flaring (land_noreturn_vertical) then just
// go with the old estimate
terrain_alt = _t_alt_prev_valid;
terrain_alt = _last_valid_terrain_alt_estimate;
} else {
// terrain alt was not valid for long time, abort landing
terrain_alt = _t_alt_prev_valid;
terrain_alt = _last_valid_terrain_alt_estimate;
abort_landing(true);
}
}
@@ -2009,7 +2007,7 @@ FixedwingPositionControl::control_manual_altitude(const float control_interval,
/* throttle limiting */
float throttle_max = _param_fw_thr_max.get();
if (_landed && (fabsf(_manual_control_setpoint_airspeed) < THROTTLE_THRESH)) {
if (_landed && (fabsf(_manual_control_setpoint_for_airspeed) < THROTTLE_THRESH)) {
throttle_max = 0.0f;
}
@@ -2071,7 +2069,7 @@ FixedwingPositionControl::control_manual_position(const float control_interval,
/* throttle limiting */
float throttle_max = _param_fw_thr_max.get();
if (_landed && (fabsf(_manual_control_setpoint_airspeed) < THROTTLE_THRESH)) {
if (_landed && (fabsf(_manual_control_setpoint_for_airspeed) < THROTTLE_THRESH)) {
throttle_max = 0.0f;
}
@@ -2520,7 +2518,7 @@ FixedwingPositionControl::reset_landing_state()
_time_started_landing = 0;
// reset terrain estimation relevant values
_time_last_t_alt = 0;
_last_time_terrain_alt_was_valid = 0;
_land_noreturn_horizontal = false;
_land_noreturn_vertical = false;
@@ -2588,24 +2586,25 @@ FixedwingPositionControl::tecs_update_pitch_throttle(const float control_interva
// set this to transition airspeed to init tecs correctly
if (_param_fw_arsp_mode.get() == 1 && PX4_ISFINITE(_param_airspeed_trans)) {
// some vtols fly without airspeed sensor
_asp_after_transition = _param_airspeed_trans;
_airspeed_after_transition = _param_airspeed_trans;
} else {
_asp_after_transition = _airspeed;
_airspeed_after_transition = _airspeed;
}
_asp_after_transition = constrain(_asp_after_transition, _param_fw_airspd_min.get(), _param_fw_airspd_max.get());
_airspeed_after_transition = constrain(_airspeed_after_transition, _param_fw_airspd_min.get(),
_param_fw_airspd_max.get());
} else if (_was_in_transition) {
// after transition we ramp up desired airspeed from the speed we had coming out of the transition
_asp_after_transition += control_interval * 2.0f; // increase 2m/s
_airspeed_after_transition += control_interval * 2.0f; // increase 2m/s
if (_asp_after_transition < airspeed_sp && _airspeed < airspeed_sp) {
airspeed_sp = max(_asp_after_transition, _airspeed);
if (_airspeed_after_transition < airspeed_sp && _airspeed < airspeed_sp) {
airspeed_sp = max(_airspeed_after_transition, _airspeed);
} else {
_was_in_transition = false;
_asp_after_transition = 0.0f;
_airspeed_after_transition = 0.0f;
}
}
}
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,