diff --git a/src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp b/src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp index da781ae284..05785d0682 100644 --- a/src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp +++ b/src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp @@ -64,7 +64,7 @@ FixedwingPositionControl::FixedwingPositionControl(bool vtol) : int32_t vt_type = -1; param_get(param_find("VT_TYPE"), &vt_type); - _vtol_tailsitter = (static_cast(vt_type) == vtol_type::TAILSITTER); + _is_vtol_tailsitter = (static_cast(vt_type) == vtol_type::TAILSITTER); } // limit to 50 Hz @@ -290,7 +290,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); @@ -298,7 +298,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; } } @@ -370,7 +370,7 @@ FixedwingPositionControl::vehicle_attitude_poll() // if the vehicle is a tailsitter we have to rotate the attitude by the pitch offset // between multirotor and fixed wing flight - if (_vtol_tailsitter) { + if (_is_vtol_tailsitter) { const Dcmf R_offset{Eulerf{0.f, M_PI_2_F, 0.f}}; R = R * R_offset; @@ -915,7 +915,7 @@ FixedwingPositionControl::control_auto(const hrt_abstime &now, const float contr break; case position_setpoint_s::SETPOINT_TYPE_VELOCITY: - control_auto_velocity(now, control_interval, curr_pos, ground_speed, pos_sp_prev, current_sp); + control_auto_velocity(now, control_interval, curr_pos, ground_speed, pos_sp_prev); break; case position_setpoint_s::SETPOINT_TYPE_LOITER: @@ -949,8 +949,6 @@ FixedwingPositionControl::control_auto(const hrt_abstime &now, const float contr void FixedwingPositionControl::control_auto_fixed_bank_alt_hold(const hrt_abstime &now) { - // only control altitude and airspeed ("fixed-bank loiter") - tecs_update_pitch_throttle(now, _current_altitude, _param_fw_airspd_trim.get(), radians(_param_fw_p_lim_min.get()), @@ -980,8 +978,6 @@ FixedwingPositionControl::control_auto_fixed_bank_alt_hold(const hrt_abstime &no void FixedwingPositionControl::control_auto_descend(const hrt_abstime &now) { - // 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; @@ -1066,8 +1062,9 @@ FixedwingPositionControl::handle_setpoint_type(const uint8_t setpoint_type, cons } void -FixedwingPositionControl::control_auto_position(const hrt_abstime &now, const float dt, const Vector2d &curr_pos, - const Vector2f &ground_speed, const position_setpoint_s &pos_sp_prev, const position_setpoint_s &pos_sp_curr) +FixedwingPositionControl::control_auto_position(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) { const float acc_rad = (_param_fw_use_npfg.get()) ? _npfg.switchDistance(500.0f) : _l1_control.switch_distance(500.0f); Vector2d curr_wp{0, 0}; @@ -1149,7 +1146,7 @@ FixedwingPositionControl::control_auto_position(const hrt_abstime &now, const fl } } - float target_airspeed = get_auto_airspeed_setpoint(now, pos_sp_curr.cruising_speed, ground_speed, dt); + float target_airspeed = get_auto_airspeed_setpoint(now, pos_sp_curr.cruising_speed, ground_speed, control_interval); 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)); @@ -1195,8 +1192,8 @@ FixedwingPositionControl::control_auto_position(const hrt_abstime &now, const fl } void -FixedwingPositionControl::control_auto_velocity(const hrt_abstime &now, const float dt, const Vector2d &curr_pos, - const Vector2f &ground_speed, const position_setpoint_s &pos_sp_prev, const position_setpoint_s &pos_sp_curr) +FixedwingPositionControl::control_auto_velocity(const hrt_abstime &now, const float control_interval, + const Vector2d &curr_pos, const Vector2f &ground_speed, const position_setpoint_s &pos_sp_curr) { float tecs_fw_thr_min; float tecs_fw_thr_max; @@ -1230,7 +1227,7 @@ FixedwingPositionControl::control_auto_velocity(const hrt_abstime &now, const fl Vector2f target_velocity{pos_sp_curr.vx, pos_sp_curr.vy}; _target_bearing = wrap_pi(atan2f(target_velocity(1), target_velocity(0))); - float target_airspeed = get_auto_airspeed_setpoint(now, pos_sp_curr.cruising_speed, ground_speed, dt); + float target_airspeed = get_auto_airspeed_setpoint(now, pos_sp_curr.cruising_speed, ground_speed, control_interval); if (_param_fw_use_npfg.get()) { _npfg.setAirspeedNom(target_airspeed * _eas2tas); @@ -1263,7 +1260,8 @@ FixedwingPositionControl::control_auto_velocity(const hrt_abstime &now, const fl } void -FixedwingPositionControl::control_auto_loiter(const hrt_abstime &now, const float dt, const Vector2d &curr_pos, +FixedwingPositionControl::control_auto_loiter(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, const position_setpoint_s &pos_sp_next) { @@ -1344,7 +1342,7 @@ FixedwingPositionControl::control_auto_loiter(const hrt_abstime &now, const floa _att_sp.apply_spoilers = vehicle_attitude_setpoint_s::SPOILERS_OFF; } - float target_airspeed = get_auto_airspeed_setpoint(now, airspeed_sp, ground_speed, dt); + float target_airspeed = get_auto_airspeed_setpoint(now, airspeed_sp, ground_speed, control_interval); 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)); @@ -1734,10 +1732,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 @@ -1750,16 +1748,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) < T_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); } } @@ -2519,7 +2517,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; @@ -2570,8 +2568,8 @@ FixedwingPositionControl::tecs_update_pitch_throttle(const hrt_abstime &now, flo bool climbout_mode, float climbout_pitch_min_rad, bool disable_underspeed_detection, float hgt_rate_sp) { - const float dt = math::constrain((now - _last_tecs_update) * 1e-6f, MIN_AUTO_TIMESTEP, MAX_AUTO_TIMESTEP); - _last_tecs_update = now; + const float dt = math::constrain((now - _time_last_tecs_update) * 1e-6f, MIN_AUTO_TIMESTEP, MAX_AUTO_TIMESTEP); + _time_last_tecs_update = now; // do not run TECS if we are not in air bool run_tecs = !_landed; @@ -2590,24 +2588,25 @@ FixedwingPositionControl::tecs_update_pitch_throttle(const hrt_abstime &now, flo // 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 += dt * 2.0f; // increase 2m/s + _airspeed_after_transition += dt * 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 0af5f98962..e435ca3bb5 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 +// [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 T_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 T_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,39 +177,43 @@ 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)}; + 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 {}; ///< 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 at which 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 + + 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 /** * @brief Last absolute time position control has been called [us] @@ -216,38 +233,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 +295,40 @@ 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; + float _manual_control_setpoint_altitude{0.0f}; // [m] + float _manual_control_setpoint_airspeed{0.0f}; // [m/s] + + // [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] + + ECL_L1_Pos_Controller _l1_control; NPFG _npfg; - TECS _tecs; + TECS _tecs; uint8_t _position_sp_type{0}; + enum FW_POSCTRL_MODE { FW_POSCTRL_MODE_AUTO, FW_POSCTRL_MODE_AUTO_ALTITUDE, @@ -291,10 +338,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,27 +350,27 @@ 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 ¤t_waypoint); + 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 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 @@ -333,23 +381,23 @@ private: 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 */ - float get_terrain_altitude_takeoff(float takeoff_alt); + float get_terrain_altitude_takeoff(float takeoff_alt); 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. @@ -366,57 +414,135 @@ private: */ 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 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, const position_setpoint_s &pos_sp_next); - - void control_auto_fixed_bank_alt_hold(const hrt_abstime &now); - void control_auto_descend(const hrt_abstime &now); - - void control_auto_position(const hrt_abstime &now, const float dt, 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 hrt_abstime &now, const float dt, 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 hrt_abstime &now, const float dt, const Vector2d &curr_pos, - const Vector2f &ground_speed, - const position_setpoint_s &pos_sp_prev, const position_setpoint_s &pos_sp_curr); + uint8_t handle_setpoint_type(const uint8_t setpoint_type, const position_setpoint_s &pos_sp_curr); /** - * @brief Vehicle control while in takeoff + * @brief Position control for all automatic modes except takeoff and 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 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 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, + const position_setpoint_s &pos_sp_next); + + /** + * @brief Controls altitude and airspeed for a fixed-bank loiter. + * + * @param now Current system time [us] + */ + void control_auto_fixed_bank_alt_hold(const hrt_abstime &now); + + /** + * @brief Control airspeed with a fixed descent rate and roll angle. + * + * @param now Current system time [us] + */ + void control_auto_descend(const hrt_abstime &now); + + /** + * @brief Vehicle control for position waypoints. + * + * @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 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_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 hrt_abstime &now, const Vector2d &curr_pos, const Vector2f &ground_speed); - void control_manual_position(const hrt_abstime &now, const float control_interval, const Vector2d &curr_pos, - const Vector2f &ground_speed); + void control_auto_position(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(); + /** + * @brief Vehicle control for loiter waypoints. + * + * @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 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 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, + const position_setpoint_s &pos_sp_next); - float get_manual_airspeed_setpoint(); - float get_auto_airspeed_setpoint(const hrt_abstime &now, const float pos_sp_cru_airspeed, const Vector2f &ground_speed, - float dt); + /** + * @brief Controls a desired airspeed, bearing, and height rate. + * + * @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 ground_speed Local 2D ground speed of vehicle [m/s] + * @param pos_sp_curr current position setpoint + */ + void control_auto_velocity(const hrt_abstime &now, const float control_interval, const Vector2d &curr_pos, + const Vector2f &ground_speed, const position_setpoint_s &pos_sp_curr); - 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); + /** + * @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 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_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); + + /** + * @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 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); + + /** + * @brief Controls altitude and airspeed, user commands roll setpoint. + * + * @param now Current system time [us] + * @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 hrt_abstime &now, const Vector2d &curr_pos, const Vector2f &ground_speed); + + /** + * @brief Controls user commanded altitude, airspeed, and bearing. + * + * @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 ground_speed Local 2D ground speed of vehicle [m/s] + */ + void control_manual_position(const hrt_abstime &now, 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(); + float get_auto_airspeed_setpoint(const hrt_abstime &now, const float pos_sp_cru_airspeed, const Vector2f &ground_speed, + float dt); + + 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 publishOrbitStatus(const position_setpoint_s pos_sp);