fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-05-23 11:07:36 +08:00
Code Issues Packages Projects Releases Wiki Activity

fw pos ctrl: some incremental cleanup of the class

Browse Source 
- make less ambiguous variable names
- fix some incorrect comments
- add units to variable descriptions
- start documenting methods
This commit is contained in:
Thomas Stastny
2022-05-05 23:18:17 +02:00
parent ccf0b7b342
commit 8f827ff6f3
2 changed files with 285 additions and 160 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp
+32 -33
View File
@@ -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<vtol_type>(vt_type) == vtol_type::TAILSITTER);
_is_vtol_tailsitter = (static_cast<vtol_type>(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;
}
}
}
src/modules/fw_pos_control_l1/FixedwingPositionControl.hpp
+253 -127
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 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<FixedwingPositionControl>, 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<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 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 &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
@@ -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 &current_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);