From 594a6d6e806c88025b595ba7cedfcd4ff6097f45 Mon Sep 17 00:00:00 2001 From: Thomas Stastny Date: Thu, 5 May 2022 23:18:17 +0200 Subject: [PATCH] fw pos ctrl: some incremental clean up of the class - 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 --- .../FixedwingPositionControl.cpp | 93 ++-- .../FixedwingPositionControl.hpp | 445 ++++++++++++------ 2 files changed, 358 insertions(+), 180 deletions(-) diff --git a/src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp b/src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp index ec0eb8d04a..5c099c8420 100644 --- a/src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp +++ b/src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp @@ -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; } } } diff --git a/src/modules/fw_pos_control_l1/FixedwingPositionControl.hpp b/src/modules/fw_pos_control_l1/FixedwingPositionControl.hpp index d5626963cd..68188ccbf9 100644 --- a/src/modules/fw_pos_control_l1/FixedwingPositionControl.hpp +++ b/src/modules/fw_pos_control_l1/FixedwingPositionControl.hpp @@ -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, 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 _attitude_sp_pub; - uORB::Publication _local_pos_sp_pub{ORB_ID(vehicle_local_position_setpoint)}; ///< vehicle local position setpoint publication - uORB::Publication _npfg_status_pub{ORB_ID(npfg_status)}; ///< NPFG status publication - uORB::Publication _pos_ctrl_status_pub{ORB_ID(position_controller_status)}; ///< navigation capabilities publication - uORB::Publication _pos_ctrl_landing_status_pub{ORB_ID(position_controller_landing_status)}; ///< landing status publication - uORB::Publication _tecs_status_pub{ORB_ID(tecs_status)}; ///< TECS status publication - uORB::PublicationMulti _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 _attitude_sp_pub; + uORB::Publication _local_pos_sp_pub{ORB_ID(vehicle_local_position_setpoint)}; + uORB::Publication _npfg_status_pub{ORB_ID(npfg_status)}; + uORB::Publication _pos_ctrl_status_pub{ORB_ID(position_controller_status)}; + uORB::Publication _pos_ctrl_landing_status_pub{ORB_ID(position_controller_landing_status)}; + uORB::Publication _tecs_status_pub{ORB_ID(tecs_status)}; + uORB::PublicationMulti _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 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] + + // 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: 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: 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 status_publish(); - void landing_status_publish(); - void tecs_status_publish(); - void publishLocalPositionSetpoint(const position_setpoint_s ¤t_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 ¤t_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: * @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 ¤t_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 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); - 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 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); - 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 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 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 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); + + /* 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 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); - 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 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 indicated airspeed setpoint for auto modes. + * @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 _slew_rate_airspeed; + SlewRate _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,