fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-11 16:43:34 +08:00
Code Issues Packages Projects Releases Wiki Activity

Remove current mode

Browse Source 
Remove landing logic

Remove waypoint navigtion logic
This commit is contained in:
JaeyoungLim
2026-01-10 07:10:05 -08:00
parent d656326e79
commit 87574986d5
2 changed files with 14 additions and 1052 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/fw_guidance_control/FixedWingGuidanceControl.cpp
+3 -797
View File
@@ -60,13 +60,8 @@ FixedWingGuidanceControl::FixedWingGuidanceControl() :
// limit to 50 Hz
_local_pos_sub.set_interval_ms(20);
_pos_ctrl_landing_status_pub.advertise();
_launch_detection_status_pub.advertise();
_landing_gear_pub.advertise();
_flaps_setpoint_pub.advertise();
_spoilers_setpoint_pub.advertise();
_fixed_wing_lateral_guidance_status_pub.advertise();
_fixed_wing_runway_control_pub.advertise();
parameters_update();
}
@@ -104,18 +99,6 @@ FixedWingGuidanceControl::parameters_update()
void
FixedWingGuidanceControl::vehicle_control_mode_poll()
{
if (_control_mode_sub.updated()) {
const bool was_armed = _control_mode.flag_armed;
if (_control_mode_sub.copy(&_control_mode)) {
// reset state when arming
if (!was_armed && _control_mode.flag_armed) {
reset_takeoff_state();
reset_landing_state();
}
}
}
}
void
@@ -124,16 +107,7 @@ FixedWingGuidanceControl::vehicle_command_poll()
vehicle_command_s vehicle_command;
while (_vehicle_command_sub.update(&vehicle_command)) {
if (vehicle_command.command == vehicle_command_s::VEHICLE_CMD_DO_GO_AROUND) {
// only abort landing before point of no return (horizontal and vertical)
if (_control_mode.flag_control_auto_enabled &&
_position_setpoint_current_valid &&
(_pos_sp_triplet.current.type == position_setpoint_s::SETPOINT_TYPE_LAND)) {
updateLandingAbortStatus(position_controller_landing_status_s::ABORTED_BY_OPERATOR);
}
} else if (vehicle_command.command == vehicle_command_s::VEHICLE_CMD_DO_CHANGE_SPEED) {
if (vehicle_command.command == vehicle_command_s::VEHICLE_CMD_DO_CHANGE_SPEED) {
if ((static_cast<uint8_t>(vehicle_command.param1 + .5f) == vehicle_command_s::SPEED_TYPE_AIRSPEED)) {
if (vehicle_command.param2 > FLT_EPSILON) { // param2 is an equivalent airspeed setpoint
@@ -252,237 +226,6 @@ FixedWingGuidanceControl::vehicle_attitude_poll()
}
}
float
FixedWingGuidanceControl::get_manual_airspeed_setpoint()
{
float manual_airspeed_setpoint = NAN;
if (_param_fw_pos_stk_conf.get() & STICK_CONFIG_ENABLE_AIRSPEED_SP_MANUAL_BIT) {
// neutral throttle corresponds to trim airspeed
manual_airspeed_setpoint = math::interpolateNXY(_manual_control_setpoint_for_airspeed,
{-1.f, 0.f, 1.f},
{_param_fw_airspd_min.get(), _param_fw_airspd_trim.get(), _param_fw_airspd_max.get()});
} else if (PX4_ISFINITE(_commanded_manual_airspeed_setpoint)) {
// override stick by commanded airspeed
manual_airspeed_setpoint = _commanded_manual_airspeed_setpoint;
}
return manual_airspeed_setpoint;
}
void
FixedWingGuidanceControl::landing_status_publish()
{
position_controller_landing_status_s pos_ctrl_landing_status = {};
pos_ctrl_landing_status.lateral_touchdown_offset = _lateral_touchdown_position_offset;
pos_ctrl_landing_status.flaring = _flare_states.flaring;
pos_ctrl_landing_status.abort_status = _landing_abort_status;
pos_ctrl_landing_status.timestamp = hrt_absolute_time();
_pos_ctrl_landing_status_pub.publish(pos_ctrl_landing_status);
}
void
FixedWingGuidanceControl::updateLandingAbortStatus(const uint8_t new_abort_status)
{
// prevent automatic aborts if already flaring, but allow manual aborts
if (!_flare_states.flaring || new_abort_status == position_controller_landing_status_s::ABORTED_BY_OPERATOR) {
// only announce changes
// if (new_abort_status > 0 && _landing_abort_status != new_abort_status) {
// switch (new_abort_status) {
// case (position_controller_landing_status_s::ABORTED_BY_OPERATOR): {
// events::send(events::ID("fixedwing_position_control_landing_abort_status_operator_abort"), events::Log::Critical,
// "Landing aborted by operator");
// break;
// }
// case (position_controller_landing_status_s::TERRAIN_NOT_FOUND): {
// events::send(events::ID("fixedwing_position_control_landing_abort_status_terrain_not_found"), events::Log::Critical,
// "Landing aborted: terrain measurement not found");
// break;
// }
// case (position_controller_landing_status_s::TERRAIN_TIMEOUT): {
// events::send(events::ID("fixedwing_position_control_landing_abort_status_terrain_timeout"), events::Log::Critical,
// "Landing aborted: terrain estimate timed out");
// break;
// }
// default: {
// events::send(events::ID("fixedwing_position_control_landing_abort_status_unknown_criterion"), events::Log::Critical,
// "Landing aborted: unknown criterion");
// }
// }
// }
_landing_abort_status = (new_abort_status >= position_controller_landing_status_s::UNKNOWN_ABORT_CRITERION) ?
position_controller_landing_status_s::UNKNOWN_ABORT_CRITERION : new_abort_status;
landing_status_publish();
}
}
float
FixedWingGuidanceControl::getManualHeightRateSetpoint()
{
float height_rate_setpoint = 0.f;
if (_manual_control_setpoint_for_height_rate >= FLT_EPSILON) {
height_rate_setpoint = math::interpolate<float>(math::deadzone(_manual_control_setpoint_for_height_rate,
kStickDeadBand), 0, 1.f, 0.f, -_param_sinkrate_target.get());
} else {
height_rate_setpoint = math::interpolate<float>(math::deadzone(_manual_control_setpoint_for_height_rate,
kStickDeadBand), -1., 0.f, _param_climbrate_target.get(), 0.f);
}
return height_rate_setpoint;
}
void
FixedWingGuidanceControl::updateManualTakeoffStatus()
{
if (!_completed_manual_takeoff) {
const bool at_controllable_airspeed = _airspeed_eas > _param_fw_airspd_min.get()
|| !_airspeed_valid;
const bool is_hovering = _vehicle_status.vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING
&& _control_mode.flag_armed;
_completed_manual_takeoff = (!_landed && at_controllable_airspeed) || is_hovering;
}
}
// void
// FixedWingGuidanceControl::set_control_mode_current(const hrt_abstime &now)
// {
// /* only run position controller in fixed-wing mode and during transitions for VTOL */
// if (_vehicle_status.vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING && !_vehicle_status.in_transition_mode) {
// _control_mode_current = FW_POSCTRL_MODE_OTHER;
// return; // do not publish the setpoint
// }
// const FW_POSCTRL_MODE previous_position_control_mode = _control_mode_current;
// _skipping_takeoff_detection = false;
// const bool doing_backtransition = _vehicle_status.in_transition_mode && !_vehicle_status.in_transition_to_fw;
// if (_control_mode.flag_control_offboard_enabled && _position_setpoint_current_valid
// && _control_mode.flag_control_position_enabled) {
// if (PX4_ISFINITE(_pos_sp_triplet.current.vx) && PX4_ISFINITE(_pos_sp_triplet.current.vy)
// && PX4_ISFINITE(_pos_sp_triplet.current.vz)) {
// // Offboard position with velocity setpoints
// _control_mode_current = FW_POSCTRL_MODE_AUTO_PATH;
// return;
// } else {
// // Offboard position setpoint only
// _control_mode_current = FW_POSCTRL_MODE_AUTO;
// return;
// }
// } else if ((_control_mode.flag_control_auto_enabled && _control_mode.flag_control_position_enabled)
// && (_position_setpoint_current_valid
// || _pos_sp_triplet.current.type == position_setpoint_s::SETPOINT_TYPE_IDLE)) {
// // Enter this mode only if the current waypoint has valid 3D position setpoints or is of type IDLE.
// // A setpoint of type IDLE can be published by Navigator without a valid position, and is handled here in FW_POSCTRL_MODE_AUTO.
// if (doing_backtransition) {
// _control_mode_current = FW_POSCTRL_MODE_TRANSITION_TO_HOVER_LINE_FOLLOW;
// } else if (_pos_sp_triplet.current.type == position_setpoint_s::SETPOINT_TYPE_TAKEOFF) {
// if (_vehicle_status.is_vtol && _vehicle_status.in_transition_mode) {
// _control_mode_current = FW_POSCTRL_MODE_AUTO;
// // in this case we want the waypoint handled as a position setpoint -- a submode in control_auto()
// _pos_sp_triplet.current.type = position_setpoint_s::SETPOINT_TYPE_POSITION;
// } else {
// _control_mode_current = _local_pos.xy_valid ? FW_POSCTRL_MODE_AUTO_TAKEOFF : FW_POSCTRL_MODE_AUTO_TAKEOFF_NO_NAV;
// if (previous_position_control_mode != FW_POSCTRL_MODE_AUTO_TAKEOFF_NO_NAV
// && previous_position_control_mode != FW_POSCTRL_MODE_AUTO_TAKEOFF && !_landed) {
// // skip takeoff detection when switching from any other mode, auto or manual,
// // while already in air.
// // TODO: find a better place for / way of doing this
// _skipping_takeoff_detection = true;
// }
// }
// } else if (_pos_sp_triplet.current.type == position_setpoint_s::SETPOINT_TYPE_LAND) {
// // Use _position_setpoint_previous_valid to determine if landing should be straight or circular.
// // Straight landings are currently only possible in Missions, and there the previous WP
// // is valid, and circular ones are used outside of Missions, as the land mode sets prev_valid=false.
// if (_position_setpoint_previous_valid) {
// _control_mode_current = FW_POSCTRL_MODE_AUTO_LANDING_STRAIGHT;
// } else {
// _control_mode_current = FW_POSCTRL_MODE_AUTO_LANDING_CIRCULAR;
// }
// } else {
// _control_mode_current = FW_POSCTRL_MODE_AUTO;
// }
// } else if (_control_mode.flag_control_auto_enabled
// && _control_mode.flag_control_climb_rate_enabled
// && _control_mode.flag_armed // only enter this modes if armed, as pure failsafe modes
// && !_control_mode.flag_control_position_enabled) {
// // failsafe modes engaged if position estimate is invalidated
// if (previous_position_control_mode != FW_POSCTRL_MODE_AUTO_ALTITUDE
// && previous_position_control_mode != FW_POSCTRL_MODE_AUTO_CLIMBRATE) {
// // reset timer the first time we switch into this mode
// _time_in_fixed_bank_loiter = now;
// }
// if (doing_backtransition) {
// // we handle loss of position control during backtransition as a special case
// _control_mode_current = FW_POSCTRL_MODE_TRANSITION_TO_HOVER_HEADING_HOLD;
// } else if (hrt_elapsed_time(&_time_in_fixed_bank_loiter) < (_param_nav_gpsf_lt.get() * 1_s)
// && !_vehicle_status.in_transition_mode) {
// if (previous_position_control_mode != FW_POSCTRL_MODE_AUTO_ALTITUDE) {
// // Need to init because last loop iteration was in a different mode
// events::send(events::ID("fixedwing_position_control_fb_loiter"), events::Log::Critical,
// "Start loiter with fixed bank angle");
// }
// _control_mode_current = FW_POSCTRL_MODE_AUTO_ALTITUDE;
// } else {
// if (previous_position_control_mode != FW_POSCTRL_MODE_AUTO_CLIMBRATE && !_vehicle_status.in_transition_mode) {
// events::send(events::ID("fixedwing_position_control_descend"), events::Log::Critical, "Start descending");
// }
// _control_mode_current = FW_POSCTRL_MODE_AUTO_CLIMBRATE;
// }
// } else if (_control_mode.flag_control_manual_enabled && _control_mode.flag_control_position_enabled) {
// if (previous_position_control_mode != FW_POSCTRL_MODE_MANUAL_POSITION) {
// /* Need to init because last loop iteration was in a different mode */
// _hdg_hold_yaw = _yaw; // yaw is not controlled, so set setpoint to current yaw
// _hdg_hold_enabled = false; // this makes sure the waypoints are reset below
// _yaw_lock_engaged = false;
// }
// _control_mode_current = FW_POSCTRL_MODE_MANUAL_POSITION;
// } else if (_control_mode.flag_control_manual_enabled && _control_mode.flag_control_altitude_enabled) {
// _control_mode_current = FW_POSCTRL_MODE_MANUAL_ALTITUDE;
// } else {
// _control_mode_current = FW_POSCTRL_MODE_OTHER;
// }
// }
void
FixedWingGuidanceControl::update_in_air_states(const hrt_abstime now)
{
@@ -492,108 +235,6 @@ FixedWingGuidanceControl::update_in_air_states(const hrt_abstime now)
}
}
void
FixedWingGuidanceControl::move_position_setpoint_for_vtol_transition(position_setpoint_s &current_sp)
{
// TODO: velocity, altitude, or just a heading hold position mode should be used for this, not position
// shifting hacks
if (_vehicle_status.in_transition_to_fw) {
if (!PX4_ISFINITE(_transition_waypoint(0))) {
double lat_transition, lon_transition;
// Create a virtual waypoint HDG_HOLD_DIST_NEXT meters in front of the vehicle which the path navigation controller can track
// during the transition. Use the current yaw setpoint to determine the transition heading, as that one in turn
// is set to the transition heading by Navigator, or current yaw if setpoint is not valid.
const float transition_heading = PX4_ISFINITE(current_sp.yaw) ? current_sp.yaw : _yaw;
waypoint_from_heading_and_distance(_current_latitude, _current_longitude, transition_heading, HDG_HOLD_DIST_NEXT,
&lat_transition,
&lon_transition);
_transition_waypoint(0) = lat_transition;
_transition_waypoint(1) = lon_transition;
}
current_sp.lat = _transition_waypoint(0);
current_sp.lon = _transition_waypoint(1);
} else {
/* reset transition waypoint, will be set upon entering front transition */
_transition_waypoint(0) = static_cast<double>(NAN);
_transition_waypoint(1) = static_cast<double>(NAN);
}
}
void FixedWingGuidanceControl::control_idle()
{
const hrt_abstime now = hrt_absolute_time();
fixed_wing_lateral_setpoint_s lateral_ctrl_sp {empty_lateral_control_setpoint};
lateral_ctrl_sp.timestamp = now;
lateral_ctrl_sp.lateral_acceleration = 0.0f;
_lateral_ctrl_sp_pub.publish(lateral_ctrl_sp);
fixed_wing_longitudinal_setpoint_s long_contrl_sp {empty_longitudinal_control_setpoint};
long_contrl_sp.timestamp = now;
long_contrl_sp.pitch_direct = 0.f;
long_contrl_sp.throttle_direct = 0.0f;
_longitudinal_ctrl_sp_pub.publish(long_contrl_sp);
_ctrl_configuration_handler.setThrottleMax(0.0f);
_ctrl_configuration_handler.setThrottleMin(0.0f);
}
uint8_t
FixedWingGuidanceControl::handle_setpoint_type(const position_setpoint_s &pos_sp_curr,
const position_setpoint_s &pos_sp_next)
{
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;
}
Vector2d curr_wp{0, 0};
/* current waypoint (the one currently heading for) */
curr_wp = Vector2d(pos_sp_curr.lat, pos_sp_curr.lon);
const float acc_rad = _directional_guidance.switchDistance(500.0f);
const bool approaching_vtol_backtransition = _vehicle_status.is_vtol
&& pos_sp_curr.type == position_setpoint_s::SETPOINT_TYPE_POSITION && _position_setpoint_current_valid
&& pos_sp_next.type == position_setpoint_s::SETPOINT_TYPE_LAND && _position_setpoint_next_valid;
// check if we should switch to loiter but only if we are not expecting a backtransition to happen
if (pos_sp_curr.type == position_setpoint_s::SETPOINT_TYPE_POSITION && !approaching_vtol_backtransition) {
float dist_xy = -1.f;
float dist_z = -1.f;
const float dist = get_distance_to_point_global_wgs84(
(double)curr_wp(0), (double)curr_wp(1), pos_sp_curr.alt,
_current_latitude, _current_longitude, _current_altitude,
&dist_xy, &dist_z);
const float acc_rad_z = (PX4_ISFINITE(pos_sp_curr.alt_acceptance_radius)
&& pos_sp_curr.alt_acceptance_radius > FLT_EPSILON) ? pos_sp_curr.alt_acceptance_radius :
_param_nav_fw_alt_rad.get();
// Achieve position setpoint altitude via loiter when laterally close to WP.
// Detect if system has switchted into a Loiter before (check _position_sp_type), and in that
// case remove the dist_xy check (not switch out of Loiter until altitude is reached).
if ((!_vehicle_status.in_transition_mode) && (dist >= 0.f)
&& (dist_z > acc_rad_z)
&& (dist_xy < acc_rad || _position_sp_type == position_setpoint_s::SETPOINT_TYPE_LOITER)) {
// SETPOINT_TYPE_POSITION -> SETPOINT_TYPE_LOITER
position_sp_type = position_setpoint_s::SETPOINT_TYPE_LOITER;
}
}
return position_sp_type;
}
void
FixedWingGuidanceControl::control_auto_path(const float control_interval, const Vector2d &curr_pos,
const Vector2f &ground_speed, const position_setpoint_s &pos_sp_curr)
@@ -635,11 +276,6 @@ FixedWingGuidanceControl::control_auto_path(const float control_interval, const
}
}
float FixedWingGuidanceControl::rollAngleToLateralAccel(float roll_body) const
{
return tanf(roll_body) * CONSTANTS_ONE_G;
}
void
FixedWingGuidanceControl::Run()
{
@@ -808,8 +444,6 @@ FixedWingGuidanceControl::Run()
Vector2d curr_pos(_current_latitude, _current_longitude);
Vector2f ground_speed(_local_pos.vx, _local_pos.vy);
// set_control_mode_current(now);
update_in_air_states(now);
// restore nominal TECS parameters in case changed intermittently (e.g. in landing handling)
@@ -817,33 +451,13 @@ FixedWingGuidanceControl::Run()
// restore lateral-directional guidance parameters (changed in takeoff mode)
_directional_guidance.setPeriod(_param_npfg_period.get());
// by default no flaps/spoilers, is overwritten below in certain modes
_flaps_setpoint = 0.f;
_spoilers_setpoint = 0.f;
// by default set speed weight to the param value, can be overwritten inside the methods below
_ctrl_configuration_handler.setSpeedWeight(_param_t_spdweight.get());
if (_control_mode_current != FW_POSCTRL_MODE_AUTO_LANDING_STRAIGHT
&& _control_mode_current != FW_POSCTRL_MODE_AUTO_LANDING_CIRCULAR) {
reset_landing_state();
}
if (_control_mode_current != FW_POSCTRL_MODE_AUTO_TAKEOFF
&& _control_mode_current != FW_POSCTRL_MODE_AUTO_TAKEOFF_NO_NAV) {
reset_takeoff_state();
}
control_auto_path(control_interval, curr_pos, ground_speed, _pos_sp_triplet.current);
int8_t old_landing_gear_position = _new_landing_gear_position;
_new_landing_gear_position = landing_gear_s::GEAR_KEEP; // is overwritten in Takeoff and Land
if (_control_mode_current == FW_POSCTRL_MODE_AUTO_PATH) {
control_auto_path(control_interval, curr_pos, ground_speed, _pos_sp_triplet.current);
}
if (_control_mode_current != FW_POSCTRL_MODE_OTHER) {
_ctrl_configuration_handler.update(now);
}
_ctrl_configuration_handler.update(now);
// only publish status in full FW mode
if (_vehicle_status.vehicle_type == vehicle_status_s::VEHICLE_TYPE_FIXED_WING
@@ -852,412 +466,12 @@ FixedWingGuidanceControl::Run()
}
// if there's any change in landing gear setpoint publish it
if (_new_landing_gear_position != old_landing_gear_position
&& _new_landing_gear_position != landing_gear_s::GEAR_KEEP) {
landing_gear_s landing_gear = {};
landing_gear.landing_gear = _new_landing_gear_position;
landing_gear.timestamp = now;
_landing_gear_pub.publish(landing_gear);
}
// In Manual modes flaps and spoilers are directly controlled in the Attitude controller and not published here
if (_control_mode.flag_control_auto_enabled
&& _vehicle_status.vehicle_type == vehicle_status_s::VEHICLE_TYPE_FIXED_WING) {
normalized_unsigned_setpoint_s flaps_setpoint;
flaps_setpoint.normalized_setpoint = _flaps_setpoint;
flaps_setpoint.timestamp = now;
_flaps_setpoint_pub.publish(flaps_setpoint);
normalized_unsigned_setpoint_s spoilers_setpoint;
spoilers_setpoint.normalized_setpoint = _spoilers_setpoint;
spoilers_setpoint.timestamp = now;
_spoilers_setpoint_pub.publish(spoilers_setpoint);
}
_xy_reset_counter = _local_pos.xy_reset_counter;
perf_end(_loop_perf);
}
}
void
FixedWingGuidanceControl::reset_takeoff_state()
{
_launch_detected = false;
_takeoff_ground_alt = _current_altitude;
}
void
FixedWingGuidanceControl::reset_landing_state()
{
_time_started_landing = 0;
_flare_states = FlareStates{};
_lateral_touchdown_position_offset = 0.0f;
_last_time_terrain_alt_was_valid = 0;
// reset abort land, unless loitering after an abort
if ((_landing_abort_status && (_pos_sp_triplet.current.type != position_setpoint_s::SETPOINT_TYPE_LOITER)) ||
(_landing_abort_status && _param_fw_lnd_abort.get() == 0)) {
updateLandingAbortStatus(position_controller_landing_status_s::NOT_ABORTED);
}
}
float FixedWingGuidanceControl::getMaxRollAngleNearGround(const float altitude, const float terrain_altitude) const
{
// we want the wings level when at the wing height above ground
const float height_above_ground = math::max(altitude - (terrain_altitude + _param_fw_wing_height.get()), 0.0f);
// this is a conservative (linear) approximation of the roll angle that would cause wing tip strike
// roll strike = arcsin( 2 * height / span )
// d(roll strike)/d(height) = 2 / span / cos(2 * height / span)
// d(roll strike)/d(height) (@height=0) = 2 / span
// roll strike ~= 2 * height / span
return math::constrain(2.f * height_above_ground / _param_fw_wing_span.get(), 0.f,
math::radians(_param_fw_r_lim.get()));
}
void
FixedWingGuidanceControl::initializeAutoLanding(const hrt_abstime &now, const position_setpoint_s &pos_sp_prev,
const float land_point_altitude, const Vector2f &local_position, const Vector2f &local_land_point)
{
if (_time_started_landing == 0) {
float height_above_land_point;
Vector2f local_approach_entrance;
// set the landing approach entrance location when we have just started the landing and store it
// NOTE: the landing approach vector is relative to the land point. ekf resets may cause a local frame
// jump, so we reference to the land point, which is globally referenced and will update
if (_position_setpoint_previous_valid) {
height_above_land_point = pos_sp_prev.alt - land_point_altitude;
local_approach_entrance = _global_local_proj_ref.project(pos_sp_prev.lat, pos_sp_prev.lon);
} else {
// no valid previous waypoint, construct one from the glide slope and direction from current
// position to land point
// NOTE: this is not really a supported use case at the moment, this is just bandaiding any
// ill-advised usage of the current implementation
// TODO: proper handling of on-the-fly landing points would need to involve some more sophisticated
// landing pattern generation and corresponding logic
height_above_land_point = _current_altitude - land_point_altitude;
local_approach_entrance = local_position;
}
_landing_approach_entrance_rel_alt = math::max(height_above_land_point, FLT_EPSILON);
const Vector2f landing_approach_vector = local_land_point - local_approach_entrance;
float landing_approach_distance = landing_approach_vector.norm();
const float max_glide_slope = tanf(math::radians(_param_fw_lnd_ang.get()));
const float glide_slope = _landing_approach_entrance_rel_alt / landing_approach_distance;
if (glide_slope > max_glide_slope) {
// rescale the landing distance - this will have the same effect as dropping down the approach
// entrance altitude on the vehicle's behavior. if we reach here.. it means the navigator checks
// didn't work, or something is using the control_auto_landing_straight() method inappropriately
landing_approach_distance = _landing_approach_entrance_rel_alt / max_glide_slope;
}
if (landing_approach_vector.norm_squared() > FLT_EPSILON) {
_landing_approach_entrance_offset_vector = -landing_approach_vector.unit_or_zero() * landing_approach_distance;
} else {
// land in direction of airframe
_landing_approach_entrance_offset_vector = Vector2f({cosf(_yaw), sinf(_yaw)}) * landing_approach_distance;
}
// save time at which we started landing and reset landing abort status
reset_landing_state();
_time_started_landing = now;
}
}
Vector2f
FixedWingGuidanceControl::calculateTouchdownPosition(const float control_interval, const Vector2f &local_land_position)
{
if (fabsf(_sticks.getYaw()) > MANUAL_TOUCHDOWN_NUDGE_INPUT_DEADZONE
&& _param_fw_lnd_nudge.get() > LandingNudgingOption::kNudgingDisabled
&& !_flare_states.flaring) {
// laterally nudge touchdown location with yaw stick
// positive is defined in the direction of a right hand turn starting from the approach vector direction
const float signed_deadzone_threshold = MANUAL_TOUCHDOWN_NUDGE_INPUT_DEADZONE * math::signNoZero(
_sticks.getYaw());
_lateral_touchdown_position_offset += (_sticks.getYaw() - signed_deadzone_threshold) *
MAX_TOUCHDOWN_POSITION_NUDGE_RATE * control_interval;
_lateral_touchdown_position_offset = math::constrain(_lateral_touchdown_position_offset, -_param_fw_lnd_td_off.get(),
_param_fw_lnd_td_off.get());
}
const Vector2f approach_unit_vector = -_landing_approach_entrance_offset_vector.unit_or_zero();
const Vector2f approach_unit_normal_vector{-approach_unit_vector(1), approach_unit_vector(0)};
return local_land_position + approach_unit_normal_vector * _lateral_touchdown_position_offset;
}
Vector2f
FixedWingGuidanceControl::calculateLandingApproachVector() const
{
Vector2f landing_approach_vector = -_landing_approach_entrance_offset_vector;
const Vector2f approach_unit_vector = landing_approach_vector.unit_or_zero();
const Vector2f approach_unit_normal_vector{-approach_unit_vector(1), approach_unit_vector(0)};
if (_param_fw_lnd_nudge.get() == LandingNudgingOption::kNudgeApproachAngle) {
// nudge the approach angle -- i.e. we adjust the approach vector to reach from the original approach
// entrance position to the newly nudged touchdown point
// NOTE: this lengthens the landing distance.. which will adjust the glideslope height slightly
landing_approach_vector += approach_unit_normal_vector * _lateral_touchdown_position_offset;
}
// if _param_fw_lnd_nudge.get() == LandingNudgingOption::kNudgingDisabled, no nudging
// if _param_fw_lnd_nudge.get() == LandingNudgingOption::kNudgeApproachPath, the full path (including approach
// entrance point) is nudged with the touchdown point, which does not require any additions to the approach vector
return landing_approach_vector;
}
float
FixedWingGuidanceControl::getLandingTerrainAltitudeEstimate(const hrt_abstime &now, const float land_point_altitude,
const bool abort_on_terrain_measurement_timeout, const bool abort_on_terrain_timeout)
{
if (_param_fw_lnd_useter.get() > TerrainEstimateUseOnLanding::kDisableTerrainEstimation) {
if (_local_pos.dist_bottom_valid) {
const float terrain_estimate = _local_pos.ref_alt + -_local_pos.z - _local_pos.dist_bottom;
_last_valid_terrain_alt_estimate = terrain_estimate;
_last_time_terrain_alt_was_valid = now;
return terrain_estimate;
}
if (_last_time_terrain_alt_was_valid == 0) {
const bool terrain_first_measurement_timed_out = (now - _time_started_landing) > TERRAIN_ALT_FIRST_MEASUREMENT_TIMEOUT;
if (terrain_first_measurement_timed_out && abort_on_terrain_measurement_timeout) {
updateLandingAbortStatus(position_controller_landing_status_s::TERRAIN_NOT_FOUND);
}
return land_point_altitude;
}
if (!_local_pos.dist_bottom_valid) {
const bool terrain_timed_out = (now - _last_time_terrain_alt_was_valid) > TERRAIN_ALT_TIMEOUT;
if (terrain_timed_out && abort_on_terrain_timeout) {
updateLandingAbortStatus(position_controller_landing_status_s::TERRAIN_TIMEOUT);
}
return _last_valid_terrain_alt_estimate;
}
}
return land_point_altitude;
}
bool FixedWingGuidanceControl::checkLandingAbortBitMask(const uint8_t automatic_abort_criteria_bitmask,
uint8_t landing_abort_criterion)
{
// landing abort status contains a manual criterion at abort_status==1, need to subtract 2 to directly compare
// to automatic criteria bits from the parameter FW_LND_ABORT
if (landing_abort_criterion <= 1) {
return false;
}
landing_abort_criterion -= 2;
return ((1 << landing_abort_criterion) & automatic_abort_criteria_bitmask) == (1 << landing_abort_criterion);
}
void FixedWingGuidanceControl::publishLocalPositionSetpoint(const position_setpoint_s &current_waypoint)
{
vehicle_local_position_setpoint_s local_position_setpoint{};
local_position_setpoint.timestamp = hrt_absolute_time();
Vector2f current_setpoint;
current_setpoint = _closest_point_on_path;
local_position_setpoint.x = current_setpoint(0);
local_position_setpoint.y = current_setpoint(1);
local_position_setpoint.z = _reference_altitude - current_waypoint.alt;
local_position_setpoint.yaw = NAN;
local_position_setpoint.yawspeed = NAN;
local_position_setpoint.vx = NAN;
local_position_setpoint.vy = NAN;
local_position_setpoint.vz = NAN;
local_position_setpoint.acceleration[0] = NAN;
local_position_setpoint.acceleration[1] = NAN;
local_position_setpoint.acceleration[2] = NAN;
_local_pos_sp_pub.publish(local_position_setpoint);
}
void FixedWingGuidanceControl::publishOrbitStatus(const position_setpoint_s pos_sp)
{
orbit_status_s orbit_status{};
orbit_status.timestamp = hrt_absolute_time();
float loiter_radius = pos_sp.loiter_radius * (pos_sp.loiter_direction_counter_clockwise ? -1.f : 1.f);
if (fabsf(loiter_radius) < FLT_EPSILON) {
loiter_radius = _param_nav_loiter_rad.get();
}
orbit_status.radius = loiter_radius;
orbit_status.frame = 0; // MAV_FRAME::MAV_FRAME_GLOBAL
orbit_status.x = static_cast<double>(pos_sp.lat);
orbit_status.y = static_cast<double>(pos_sp.lon);
orbit_status.z = pos_sp.alt;
orbit_status.yaw_behaviour = orbit_status_s::ORBIT_YAW_BEHAVIOUR_HOLD_FRONT_TANGENT_TO_CIRCLE;
_orbit_status_pub.publish(orbit_status);
}
DirectionalGuidanceOutput FixedWingGuidanceControl::navigateWaypoints(const Vector2f &start_waypoint,
const Vector2f &end_waypoint,
const Vector2f &vehicle_pos, const Vector2f &ground_vel, const Vector2f &wind_vel)
{
const Vector2f start_waypoint_to_end_waypoint = end_waypoint - start_waypoint;
const Vector2f start_waypoint_to_vehicle = vehicle_pos - start_waypoint;
const Vector2f end_waypoint_to_vehicle = vehicle_pos - end_waypoint;
if (start_waypoint_to_end_waypoint.norm() < FLT_EPSILON) {
// degenerate case: the waypoints are on top of each other, this should only happen when someone uses this
// method incorrectly. just as a safe guard, call the singular waypoint navigation method.
return navigateWaypoint(end_waypoint, vehicle_pos, ground_vel, wind_vel);
}
if ((start_waypoint_to_end_waypoint.dot(start_waypoint_to_vehicle) < -FLT_EPSILON)
&& (start_waypoint_to_vehicle.norm() > _directional_guidance.switchDistance(500.0f))) {
// we are in front of the start waypoint, fly directly to it until we are within switch distance
return navigateWaypoint(start_waypoint, vehicle_pos, ground_vel, wind_vel);
}
if (start_waypoint_to_end_waypoint.dot(end_waypoint_to_vehicle) > FLT_EPSILON) {
// we are beyond the end waypoint, fly back to it
// NOTE: this logic ideally never gets executed, as a waypoint switch should happen before passing the
// end waypoint. however this included here as a safety precaution if any navigator (module) switch condition
// is missed for any reason. in the future this logic should all be handled in one place in a dedicated
// flight mode state machine.
return navigateWaypoint(end_waypoint, vehicle_pos, ground_vel, wind_vel);
}
// follow the line segment between the start and end waypoints
return navigateLine(start_waypoint, end_waypoint, vehicle_pos, ground_vel, wind_vel);
}
DirectionalGuidanceOutput FixedWingGuidanceControl::navigateWaypoint(const Vector2f &waypoint_pos,
const Vector2f &vehicle_pos,
const Vector2f &ground_vel, const Vector2f &wind_vel)
{
const Vector2f vehicle_to_waypoint = waypoint_pos - vehicle_pos;
if (vehicle_to_waypoint.norm() < FLT_EPSILON) {
// degenerate case: the vehicle is on top of the single waypoint. (can happen). maintain the last npfg command.
return DirectionalGuidanceOutput{};
}
const Vector2f unit_path_tangent = vehicle_to_waypoint.normalized();
_closest_point_on_path = waypoint_pos;
const float path_curvature = 0.f;
DirectionalGuidanceOutput sp = _directional_guidance.guideToPath(vehicle_pos, ground_vel, wind_vel, unit_path_tangent,
_closest_point_on_path, path_curvature);
return sp;
}
DirectionalGuidanceOutput FixedWingGuidanceControl::navigateLine(const Vector2f &point_on_line_1,
const Vector2f &point_on_line_2,
const Vector2f &vehicle_pos, const Vector2f &ground_vel, const Vector2f &wind_vel)
{
const Vector2f line_segment = point_on_line_2 - point_on_line_1;
if (line_segment.norm() <= FLT_EPSILON) {
// degenerate case: line segment has zero length. maintain the last npfg command.
return DirectionalGuidanceOutput{};
}
const Vector2f unit_path_tangent = line_segment.normalized();
const Vector2f point_1_to_vehicle = vehicle_pos - point_on_line_1;
_closest_point_on_path = point_on_line_1 + point_1_to_vehicle.dot(unit_path_tangent) * unit_path_tangent;
const float path_curvature = 0.f;
const DirectionalGuidanceOutput sp = _directional_guidance.guideToPath(vehicle_pos, ground_vel, wind_vel,
unit_path_tangent,
_closest_point_on_path, path_curvature);
return sp;
}
DirectionalGuidanceOutput FixedWingGuidanceControl::navigateLine(const Vector2f &point_on_line,
const float line_bearing,
const Vector2f &vehicle_pos, const Vector2f &ground_vel, const Vector2f &wind_vel)
{
const Vector2f unit_path_tangent{cosf(line_bearing), sinf(line_bearing)};
const Vector2f point_on_line_to_vehicle = vehicle_pos - point_on_line;
_closest_point_on_path = point_on_line + point_on_line_to_vehicle.dot(unit_path_tangent) * unit_path_tangent;
const float path_curvature = 0.f;
const DirectionalGuidanceOutput sp = _directional_guidance.guideToPath(vehicle_pos, ground_vel, wind_vel,
unit_path_tangent,
_closest_point_on_path, path_curvature);
return sp;
}
DirectionalGuidanceOutput FixedWingGuidanceControl::navigateLoiter(const Vector2f &loiter_center,
const Vector2f &vehicle_pos,
float radius, bool loiter_direction_counter_clockwise, const Vector2f &ground_vel, const Vector2f &wind_vel)
{
const float loiter_direction_multiplier = loiter_direction_counter_clockwise ? -1.f : 1.f;
Vector2f vector_center_to_vehicle = vehicle_pos - loiter_center;
const float dist_to_center = vector_center_to_vehicle.norm();
// find the direction from the circle center to the closest point on its perimeter
// from the vehicle position
Vector2f unit_vec_center_to_closest_pt;
if (dist_to_center < 0.1f) {
// the logic breaks down at the circle center, employ some mitigation strategies
// until we exit this region
if (ground_vel.norm() < 0.1f) {
// arbitrarily set the point in the northern top of the circle
unit_vec_center_to_closest_pt = Vector2f{1.0f, 0.0f};
} else {
// set the point in the direction we are moving
unit_vec_center_to_closest_pt = ground_vel.normalized();
}
} else {
// set the point in the direction of the aircraft
unit_vec_center_to_closest_pt = vector_center_to_vehicle.normalized();
}
// 90 deg clockwise rotation * loiter direction
const Vector2f unit_path_tangent = loiter_direction_multiplier * Vector2f{-unit_vec_center_to_closest_pt(1), unit_vec_center_to_closest_pt(0)};
const float path_curvature = loiter_direction_multiplier / radius;
_closest_point_on_path = unit_vec_center_to_closest_pt * radius + loiter_center;
return _directional_guidance.guideToPath(vehicle_pos, ground_vel, wind_vel, unit_path_tangent,
loiter_center + unit_vec_center_to_closest_pt * radius, path_curvature);
}
DirectionalGuidanceOutput FixedWingGuidanceControl::navigatePathTangent(const matrix::Vector2f &vehicle_pos,
const matrix::Vector2f &position_setpoint,
@@ -1276,14 +490,6 @@ DirectionalGuidanceOutput FixedWingGuidanceControl::navigatePathTangent(const ma
curvature);
}
DirectionalGuidanceOutput FixedWingGuidanceControl::navigateBearing(const matrix::Vector2f &vehicle_pos, float bearing,
const Vector2f &ground_vel, const Vector2f &wind_vel)
{
const Vector2f unit_path_tangent = Vector2f{cosf(bearing), sinf(bearing)};
_closest_point_on_path = vehicle_pos;
return _directional_guidance.guideToPath(vehicle_pos, ground_vel, wind_vel, unit_path_tangent, vehicle_pos, 0.0f);
}
void FixedWingGuidanceControl::publish_lateral_guidance_status(const hrt_abstime now)
{
fixed_wing_lateral_guidance_status_s fixed_wing_lateral_guidance_status{};
src/modules/fw_guidance_control/FixedWingGuidanceControl.hpp
+11 -255
View File
@@ -68,11 +68,9 @@
#include <uORB/topics/fixed_wing_lateral_guidance_status.h>
#include <uORB/topics/fixed_wing_longitudinal_setpoint.h>
#include <uORB/topics/fixed_wing_runway_control.h>
#include <uORB/topics/landing_gear.h>
#include <uORB/topics/launch_detection_status.h>
#include <uORB/topics/normalized_unsigned_setpoint.h>
#include <uORB/topics/parameter_update.h>
#include <uORB/topics/position_controller_landing_status.h>
#include <uORB/topics/position_controller_status.h>
#include <uORB/topics/position_setpoint_triplet.h>
#include <uORB/topics/trajectory_setpoint.h>
@@ -180,16 +178,11 @@ private:
uORB::Subscription _vehicle_status_sub{ORB_ID(vehicle_status)};
uORB::Publication<vehicle_local_position_setpoint_s> _local_pos_sp_pub{ORB_ID(vehicle_local_position_setpoint)};
uORB::Publication<position_controller_landing_status_s> _pos_ctrl_landing_status_pub{ORB_ID(position_controller_landing_status)};
uORB::Publication<launch_detection_status_s> _launch_detection_status_pub{ORB_ID(launch_detection_status)};
uORB::PublicationMulti<orbit_status_s> _orbit_status_pub{ORB_ID(orbit_status)};
uORB::Publication<landing_gear_s> _landing_gear_pub {ORB_ID(landing_gear)};
uORB::Publication<normalized_unsigned_setpoint_s> _flaps_setpoint_pub{ORB_ID(flaps_setpoint)};
uORB::Publication<normalized_unsigned_setpoint_s> _spoilers_setpoint_pub{ORB_ID(spoilers_setpoint)};
uORB::PublicationData<fixed_wing_lateral_setpoint_s> _lateral_ctrl_sp_pub{ORB_ID(fixed_wing_lateral_setpoint)};
uORB::PublicationData<fixed_wing_longitudinal_setpoint_s> _longitudinal_ctrl_sp_pub{ORB_ID(fixed_wing_longitudinal_setpoint)};
uORB::Publication<fixed_wing_lateral_guidance_status_s> _fixed_wing_lateral_guidance_status_pub{ORB_ID(fixed_wing_lateral_guidance_status)};
uORB::Publication<fixed_wing_runway_control_s> _fixed_wing_runway_control_pub{ORB_ID(fixed_wing_runway_control)};
position_setpoint_triplet_s _pos_sp_triplet{};
vehicle_control_mode_s _control_mode{};
@@ -292,48 +285,15 @@ private:
bool _skipping_takeoff_detection{false};
// AUTO LANDING
// corresponds to param FW_LND_NUDGE
enum LandingNudgingOption {
kNudgingDisabled = 0,
kNudgeApproachAngle,
kNudgeApproachPath
};
// [us] Start time of the landing approach. If a fixed-wing landing pattern is used, this timer starts *after any
// orbit to altitude only when the aircraft has entered the final *straight approach.
hrt_abstime _time_started_landing{0};
// [m] lateral touchdown position offset manually commanded during landing
float _lateral_touchdown_position_offset{0.0f};
// [m] relative vector from land point to approach entrance (NE)
Vector2f _landing_approach_entrance_offset_vector{};
// [m] relative height above land point
float _landing_approach_entrance_rel_alt{0.0f};
uint8_t _landing_abort_status{position_controller_landing_status_s::NOT_ABORTED};
// organize flare states XXX: need to split into a separate class at some point!
struct FlareStates {
bool flaring{false};
hrt_abstime start_time{0}; // [us]
float initial_height_rate_setpoint{0.0f}; // [m/s]
} _flare_states;
// [m] last terrain estimate which was valid
float _last_valid_terrain_alt_estimate{0.0f};
// [us] time at which we had last valid terrain alt
hrt_abstime _last_time_terrain_alt_was_valid{0};
enum TerrainEstimateUseOnLanding {
kDisableTerrainEstimation = 0,
kTriggerFlareWithTerrainEstimate,
kFollowTerrainRelativeLandingGlideSlope
};
// AIRSPEED
float _airspeed_eas{0.f};
@@ -367,13 +327,6 @@ private:
// nonlinear path following guidance - lateral-directional position control
DirectionalGuidance _directional_guidance;
// LANDING GEAR
int8_t _new_landing_gear_position{landing_gear_s::GEAR_KEEP};
// FLAPS/SPOILERS
float _flaps_setpoint{0.f};
float _spoilers_setpoint{0.f};
hrt_abstime _time_in_fixed_bank_loiter{0}; // [us]
float _min_current_sp_distance_xy{FLT_MAX};
@@ -390,40 +343,20 @@ private:
void wind_poll(const hrt_abstime now);
void landing_status_publish();
/**
* @brief Vehicle control for following a path.
*
* @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_path(const float control_interval, const Vector2d &curr_pos, const Vector2f &ground_speed,
const position_setpoint_s &pos_sp_curr);
void publishLocalPositionSetpoint(const position_setpoint_s &current_waypoint);
/**
* @brief Sets the landing abort status and publishes landing status.
*
* @param new_abort_status Either 0 (not aborted) or the singular bit >0 which triggered the abort
*/
void updateLandingAbortStatus(const uint8_t new_abort_status = position_controller_landing_status_s::NOT_ABORTED);
/**
* @brief Checks if the automatic abort bitmask (from FW_LND_ABORT) contains the given abort criterion.
*
* @param automatic_abort_criteria_bitmask Bitmask containing all active abort criteria
* @param landing_abort_criterion The specifc criterion we are checking for
* @return true if the bitmask contains the criterion
*/
bool checkLandingAbortBitMask(const uint8_t automatic_abort_criteria_bitmask, uint8_t landing_abort_criterion);
/**
* @brief Maps the manual control setpoint (pilot sticks) to height rate commands
*
* @return Manual height rate setpoint [m/s]
*/
float getManualHeightRateSetpoint();
/**
* @brief Updates a state indicating whether a manual takeoff has been completed.
*
* Criteria include passing an airspeed threshold and not being in a landed state. VTOL airframes always pass.
*/
void updateManualTakeoffStatus();
/**
* @brief Updates timing information for landed and in-air states.
*
@@ -431,165 +364,6 @@ private:
*/
void update_in_air_states(const hrt_abstime now);
/**
* @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
*/
void move_position_setpoint_for_vtol_transition(position_setpoint_s &current_sp);
/**
* @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,
const position_setpoint_s &pos_sp_next);
/* automatic control methods */
float get_manual_airspeed_setpoint();
void reset_takeoff_state();
void reset_landing_state();
/**
* @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]
*/
// void set_control_mode_current(const hrt_abstime &now);
void publishOrbitStatus(const position_setpoint_s pos_sp);
float getMaxRollAngleNearGround(const float altitude, const float terrain_altitude) const;
/**
* @brief Calculates the touchdown position for landing with optional manual lateral adjustments.
*
* Manual inputs (from the remote) are used to command a rate at which the position moves and the integrated
* position is bounded. This is useful for manually adjusting the landing point in real time when map or GNSS
* errors cause an offset from the desired landing vector.
*
* @param control_interval Time since the last position control update [s]
* @param local_land_position Originally commanded local land position (NE) [m]
* @return (Nudged) Local touchdown position (NE) [m]
*/
Vector2f calculateTouchdownPosition(const float control_interval, const Vector2f &local_land_position);
/**
* @brief Calculates the vector from landing approach entrance to touchdown point
*
* NOTE: calculateTouchdownPosition() MUST be called before this method
*
* @return Landing approach vector [m]
*/
Vector2f calculateLandingApproachVector() const;
/**
* @brief Returns a terrain altitude estimate with consideration of altimeter measurements.
*
* @param now Current system time [us]
* @param land_point_altitude Altitude (AMSL) of the land point [m]
* @param abort_on_terrain_measurement_timeout Abort if distance to ground estimation doesn't get valid when we expect it to
* @param abort_on_terrain_timeout Abort if distance to ground estimation is invalid after being valid before
* @return Terrain altitude (AMSL) [m]
*/
float getLandingTerrainAltitudeEstimate(const hrt_abstime &now, const float land_point_altitude,
const bool abort_on_terrain_measurement_timeout, const bool abort_on_terrain_timeout);
/**
* @brief Initializes landing states
*
* @param now Current system time [us]
* @param pos_sp_prev Previous position setpoint
* @param land_point_alt Landing point altitude setpoint AMSL [m]
* @param local_position Local aircraft position (NE) [m]
* @param local_land_point Local land point (NE) [m]
*/
void initializeAutoLanding(const hrt_abstime &now, const position_setpoint_s &pos_sp_prev,
const float land_point_alt, const Vector2f &local_position, const Vector2f &local_land_point);
/*
* Waypoint handling logic following closely to the ECL_L1_Pos_Controller
* method of the same name. Takes two waypoints, steering the vehicle to track
* the line segment between them.
*
* @param[in] start_waypoint Segment starting position in local coordinates. (N,E) [m]
* @param[in] end_waypoint Segment end position in local coordinates. (N,E) [m]
* @param[in] vehicle_pos Vehicle position in local coordinates. (N,E) [m]
* @param[in] ground_vel Vehicle ground velocity vector [m/s]
* @param[in] wind_vel Wind velocity vector [m/s]
*/
DirectionalGuidanceOutput navigateWaypoints(const matrix::Vector2f &start_waypoint,
const matrix::Vector2f &end_waypoint,
const matrix::Vector2f &vehicle_pos, const matrix::Vector2f &ground_vel,
const matrix::Vector2f &wind_vel);
/*
* Takes one waypoint and steers the vehicle towards this.
*
* NOTE: this *will lead to "flowering" behavior if no higher level state machine or
* switching condition changes the waypoint.
*
* @param[in] waypoint_pos Waypoint position in local coordinates. (N,E) [m]
* @param[in] vehicle_pos Vehicle position in local coordinates. (N,E) [m]
* @param[in] ground_vel Vehicle ground velocity vector [m/s]
* @param[in] wind_vel Wind velocity vector [m/s]
*/
DirectionalGuidanceOutput navigateWaypoint(const matrix::Vector2f &waypoint_pos, const matrix::Vector2f &vehicle_pos,
const matrix::Vector2f &ground_vel, const matrix::Vector2f &wind_vel);
/*
* Line (infinite) following logic. Two points on the line are used to define the
* line in 2D space (first to second point determines the direction). Determines the
* relevant parameters for evaluating the NPFG guidance law, then updates control setpoints.
*
* @param[in] point_on_line_1 Arbitrary first position on line in local coordinates. (N,E) [m]
* @param[in] point_on_line_2 Arbitrary second position on line in local coordinates. (N,E) [m]
* @param[in] vehicle_pos Vehicle position in local coordinates. (N,E) [m]
* @param[in] ground_vel Vehicle ground velocity vector [m/s]
* @param[in] wind_vel Wind velocity vector [m/s]
*/
DirectionalGuidanceOutput navigateLine(const Vector2f &point_on_line_1, const Vector2f &point_on_line_2,
const Vector2f &vehicle_pos,
const Vector2f &ground_vel, const Vector2f &wind_vel);
/*
* Line (infinite) following logic. One point on the line and a line bearing are used to define
* the line in 2D space. Determines the relevant parameters for evaluating the NPFG guidance law,
* then updates control setpoints.
*
* @param[in] point_on_line Arbitrary position on line in local coordinates. (N,E) [m]
* @param[in] line_bearing Line bearing [rad] (from north)
* @param[in] vehicle_pos Vehicle position in local coordinates. (N,E) [m]
* @param[in] ground_vel Vehicle ground velocity vector [m/s]
* @param[in] wind_vel Wind velocity vector [m/s]
*/
DirectionalGuidanceOutput navigateLine(const Vector2f &point_on_line, const float line_bearing,
const Vector2f &vehicle_pos,
const Vector2f &ground_vel, const Vector2f &wind_vel);
/*
* Loitering (unlimited) logic. Takes loiter center, radius, and direction and
* determines the relevant parameters for evaluating the NPFG guidance law,
* then updates control setpoints.
*
* @param[in] loiter_center The position of the center of the loiter circle [m]
* @param[in] vehicle_pos Vehicle position in local coordinates. (N,E) [m]
* @param[in] radius Loiter radius [m]
* @param[in] loiter_direction_counter_clockwise Specifies loiter direction
* @param[in] ground_vel Vehicle ground velocity vector [m/s]
* @param[in] wind_vel Wind velocity vector [m/s]
*/
DirectionalGuidanceOutput navigateLoiter(const matrix::Vector2f &loiter_center, const matrix::Vector2f &vehicle_pos,
float radius, bool loiter_direction_counter_clockwise, const matrix::Vector2f &ground_vel,
const matrix::Vector2f &wind_vel);
/*
* Path following logic. Takes poisiton, path tangent, curvature and
* then updates control setpoints to follow a path setpoint.
@@ -608,26 +382,8 @@ private:
const matrix::Vector2f &tangent_setpoint,
const matrix::Vector2f &ground_vel, const matrix::Vector2f &wind_vel, const float &curvature);
/*
* Navigate on a fixed bearing.
*
* This only holds a certain (ground relative) direction and does not perform
* cross track correction. Helpful for semi-autonomous modes.
*
* @param[in] vehicle_pos vehicle_pos Vehicle position in local coordinates. (N,E) [m]
* @param[in] bearing Bearing angle [rad]
* @param[in] ground_vel Vehicle ground velocity vector [m/s]
* @param[in] wind_vel Wind velocity vector [m/s]
*/
DirectionalGuidanceOutput navigateBearing(const matrix::Vector2f &vehicle_pos, float bearing,
const matrix::Vector2f &ground_vel,
const matrix::Vector2f &wind_vel);
void control_idle();
void publish_lateral_guidance_status(const hrt_abstime now);
float rollAngleToLateralAccel(float roll_body) const;
DEFINE_PARAMETERS(
(ParamFloat<px4::params::FW_R_LIM>) _param_fw_r_lim,