|
|
|
|
@ -100,12 +100,6 @@ FixedwingPositionControl::parameters_update()
|
|
|
|
|
param_get(_param_handle_airspeed_trans, &_param_airspeed_trans);
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
// L1 control parameters
|
|
|
|
|
_l1_control.set_l1_damping(_param_fw_l1_damping.get());
|
|
|
|
|
_l1_control.set_l1_period(_param_fw_l1_period.get());
|
|
|
|
|
_l1_control.set_l1_roll_limit(radians(_param_fw_r_lim.get()));
|
|
|
|
|
_l1_control.set_roll_slew_rate(radians(_param_fw_l1_r_slew_max.get()));
|
|
|
|
|
|
|
|
|
|
// NPFG parameters
|
|
|
|
|
_npfg.setPeriod(_param_npfg_period.get());
|
|
|
|
|
_npfg.setDamping(_param_npfg_damping.get());
|
|
|
|
|
@ -399,7 +393,7 @@ FixedwingPositionControl::adapt_airspeed_setpoint(const float control_interval,
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
// Adapt cruise airspeed when otherwise the min groundspeed couldn't be maintained
|
|
|
|
|
if (!_l1_control.circle_mode() && !using_npfg_with_wind_estimate()) {
|
|
|
|
|
if (!_wind_valid) {
|
|
|
|
|
/*
|
|
|
|
|
* This error value ensures that a plane (as long as its throttle capability is
|
|
|
|
|
* not exceeded) travels towards a waypoint (and is not pushed more and more away
|
|
|
|
|
@ -516,50 +510,33 @@ FixedwingPositionControl::status_publish()
|
|
|
|
|
pos_ctrl_status.nav_roll = _att_sp.roll_body;
|
|
|
|
|
pos_ctrl_status.nav_pitch = _att_sp.pitch_body;
|
|
|
|
|
|
|
|
|
|
if (_l1_control.has_guidance_updated()) {
|
|
|
|
|
pos_ctrl_status.nav_bearing = _l1_control.nav_bearing();
|
|
|
|
|
npfg_status_s npfg_status = {};
|
|
|
|
|
|
|
|
|
|
pos_ctrl_status.target_bearing = _l1_control.target_bearing();
|
|
|
|
|
pos_ctrl_status.xtrack_error = _l1_control.crosstrack_error();
|
|
|
|
|
npfg_status.wind_est_valid = _wind_valid;
|
|
|
|
|
|
|
|
|
|
} else {
|
|
|
|
|
pos_ctrl_status.nav_bearing = NAN;
|
|
|
|
|
pos_ctrl_status.target_bearing = NAN;
|
|
|
|
|
pos_ctrl_status.xtrack_error = NAN;
|
|
|
|
|
}
|
|
|
|
|
const float bearing = _npfg.getBearing(); // dont repeat atan2 calc
|
|
|
|
|
|
|
|
|
|
if (_param_fw_use_npfg.get()) {
|
|
|
|
|
npfg_status_s npfg_status = {};
|
|
|
|
|
pos_ctrl_status.nav_bearing = bearing;
|
|
|
|
|
pos_ctrl_status.target_bearing = _target_bearing;
|
|
|
|
|
pos_ctrl_status.xtrack_error = _npfg.getTrackError();
|
|
|
|
|
pos_ctrl_status.acceptance_radius = _npfg.switchDistance(500.0f);
|
|
|
|
|
|
|
|
|
|
npfg_status.wind_est_valid = _wind_valid;
|
|
|
|
|
npfg_status.lat_accel = _npfg.getLateralAccel();
|
|
|
|
|
npfg_status.lat_accel_ff = _npfg.getLateralAccelFF();
|
|
|
|
|
npfg_status.heading_ref = _npfg.getHeadingRef();
|
|
|
|
|
npfg_status.bearing = bearing;
|
|
|
|
|
npfg_status.bearing_feas = _npfg.getBearingFeas();
|
|
|
|
|
npfg_status.bearing_feas_on_track = _npfg.getOnTrackBearingFeas();
|
|
|
|
|
npfg_status.signed_track_error = _npfg.getTrackError();
|
|
|
|
|
npfg_status.track_error_bound = _npfg.getTrackErrorBound();
|
|
|
|
|
npfg_status.airspeed_ref = _npfg.getAirspeedRef();
|
|
|
|
|
npfg_status.min_ground_speed_ref = _npfg.getMinGroundSpeedRef();
|
|
|
|
|
npfg_status.adapted_period = _npfg.getAdaptedPeriod();
|
|
|
|
|
npfg_status.p_gain = _npfg.getPGain();
|
|
|
|
|
npfg_status.time_const = _npfg.getTimeConst();
|
|
|
|
|
npfg_status.timestamp = hrt_absolute_time();
|
|
|
|
|
|
|
|
|
|
const float bearing = _npfg.getBearing(); // dont repeat atan2 calc
|
|
|
|
|
|
|
|
|
|
pos_ctrl_status.nav_bearing = bearing;
|
|
|
|
|
pos_ctrl_status.target_bearing = _target_bearing;
|
|
|
|
|
pos_ctrl_status.xtrack_error = _npfg.getTrackError();
|
|
|
|
|
pos_ctrl_status.acceptance_radius = _npfg.switchDistance(500.0f);
|
|
|
|
|
|
|
|
|
|
npfg_status.lat_accel = _npfg.getLateralAccel();
|
|
|
|
|
npfg_status.lat_accel_ff = _npfg.getLateralAccelFF();
|
|
|
|
|
npfg_status.heading_ref = _npfg.getHeadingRef();
|
|
|
|
|
npfg_status.bearing = bearing;
|
|
|
|
|
npfg_status.bearing_feas = _npfg.getBearingFeas();
|
|
|
|
|
npfg_status.bearing_feas_on_track = _npfg.getOnTrackBearingFeas();
|
|
|
|
|
npfg_status.signed_track_error = _npfg.getTrackError();
|
|
|
|
|
npfg_status.track_error_bound = _npfg.getTrackErrorBound();
|
|
|
|
|
npfg_status.airspeed_ref = _npfg.getAirspeedRef();
|
|
|
|
|
npfg_status.min_ground_speed_ref = _npfg.getMinGroundSpeedRef();
|
|
|
|
|
npfg_status.adapted_period = _npfg.getAdaptedPeriod();
|
|
|
|
|
npfg_status.p_gain = _npfg.getPGain();
|
|
|
|
|
npfg_status.time_const = _npfg.getTimeConst();
|
|
|
|
|
npfg_status.timestamp = hrt_absolute_time();
|
|
|
|
|
|
|
|
|
|
_npfg_status_pub.publish(npfg_status);
|
|
|
|
|
|
|
|
|
|
} else {
|
|
|
|
|
pos_ctrl_status.acceptance_radius = _l1_control.switch_distance(500.0f);
|
|
|
|
|
}
|
|
|
|
|
_npfg_status_pub.publish(npfg_status);
|
|
|
|
|
|
|
|
|
|
pos_ctrl_status.wp_dist = get_distance_to_next_waypoint(_current_latitude, _current_longitude,
|
|
|
|
|
_pos_sp_triplet.current.lat, _pos_sp_triplet.current.lon);
|
|
|
|
|
@ -996,7 +973,7 @@ FixedwingPositionControl::handle_setpoint_type(const position_setpoint_s &pos_sp
|
|
|
|
|
/* current waypoint (the one currently heading for) */
|
|
|
|
|
curr_wp = Vector2d(pos_sp_curr.lat, pos_sp_curr.lon);
|
|
|
|
|
|
|
|
|
|
const float acc_rad = (_param_fw_use_npfg.get()) ? _npfg.switchDistance(500.0f) : _l1_control.switch_distance(500.0f);
|
|
|
|
|
const float acc_rad = _npfg.switchDistance(500.0f);
|
|
|
|
|
|
|
|
|
|
if (pos_sp_curr.type == position_setpoint_s::SETPOINT_TYPE_POSITION
|
|
|
|
|
|| pos_sp_curr.type == position_setpoint_s::SETPOINT_TYPE_LOITER) {
|
|
|
|
|
@ -1027,15 +1004,6 @@ FixedwingPositionControl::handle_setpoint_type(const position_setpoint_s &pos_sp
|
|
|
|
|
// SETPOINT_TYPE_POSITION -> SETPOINT_TYPE_LOITER
|
|
|
|
|
position_sp_type = position_setpoint_s::SETPOINT_TYPE_LOITER;
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
} else if (pos_sp_curr.type == position_setpoint_s::SETPOINT_TYPE_LOITER) {
|
|
|
|
|
// LOITER: use SETPOINT_TYPE_POSITION to get to SETPOINT_TYPE_LOITER (NPFG handles this internally)
|
|
|
|
|
if ((dist >= 0.f)
|
|
|
|
|
&& (dist_xy > 2.f * math::max(acc_rad, loiter_radius_abs))
|
|
|
|
|
&& !_param_fw_use_npfg.get()) {
|
|
|
|
|
// SETPOINT_TYPE_LOITER -> SETPOINT_TYPE_POSITION
|
|
|
|
|
position_sp_type = position_setpoint_s::SETPOINT_TYPE_POSITION;
|
|
|
|
|
}
|
|
|
|
|
}
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
@ -1046,7 +1014,7 @@ void
|
|
|
|
|
FixedwingPositionControl::control_auto_position(const float control_interval, const Vector2d &curr_pos,
|
|
|
|
|
const Vector2f &ground_speed, const position_setpoint_s &pos_sp_prev, const position_setpoint_s &pos_sp_curr)
|
|
|
|
|
{
|
|
|
|
|
const float acc_rad = (_param_fw_use_npfg.get()) ? _npfg.switchDistance(500.0f) : _l1_control.switch_distance(500.0f);
|
|
|
|
|
const float acc_rad = _npfg.switchDistance(500.0f);
|
|
|
|
|
Vector2d curr_wp{0, 0};
|
|
|
|
|
Vector2d prev_wp{0, 0};
|
|
|
|
|
|
|
|
|
|
@ -1120,30 +1088,24 @@ FixedwingPositionControl::control_auto_position(const float control_interval, co
|
|
|
|
|
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));
|
|
|
|
|
|
|
|
|
|
if (_param_fw_use_npfg.get()) {
|
|
|
|
|
_npfg.setAirspeedNom(target_airspeed * _eas2tas);
|
|
|
|
|
_npfg.setAirspeedMax(_param_fw_airspd_max.get() * _eas2tas);
|
|
|
|
|
_npfg.setAirspeedNom(target_airspeed * _eas2tas);
|
|
|
|
|
_npfg.setAirspeedMax(_param_fw_airspd_max.get() * _eas2tas);
|
|
|
|
|
|
|
|
|
|
if (_control_mode.flag_control_offboard_enabled && PX4_ISFINITE(pos_sp_curr.vx) && PX4_ISFINITE(pos_sp_curr.vy)) {
|
|
|
|
|
// Navigate directly on position setpoint and path tangent
|
|
|
|
|
matrix::Vector2f velocity_2d(pos_sp_curr.vx, pos_sp_curr.vy);
|
|
|
|
|
float curvature = PX4_ISFINITE(_pos_sp_triplet.current.loiter_radius) ? 1 / _pos_sp_triplet.current.loiter_radius :
|
|
|
|
|
0.0f;
|
|
|
|
|
navigatePathTangent(curr_pos_local, curr_wp_local, velocity_2d.normalized(), get_nav_speed_2d(ground_speed),
|
|
|
|
|
_wind_vel, curvature);
|
|
|
|
|
|
|
|
|
|
} else {
|
|
|
|
|
navigateWaypoints(prev_wp_local, curr_wp_local, curr_pos_local, get_nav_speed_2d(ground_speed), _wind_vel);
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
_att_sp.roll_body = _npfg.getRollSetpoint();
|
|
|
|
|
target_airspeed = _npfg.getAirspeedRef() / _eas2tas;
|
|
|
|
|
if (_control_mode.flag_control_offboard_enabled && PX4_ISFINITE(pos_sp_curr.vx) && PX4_ISFINITE(pos_sp_curr.vy)) {
|
|
|
|
|
// Navigate directly on position setpoint and path tangent
|
|
|
|
|
matrix::Vector2f velocity_2d(pos_sp_curr.vx, pos_sp_curr.vy);
|
|
|
|
|
float curvature = PX4_ISFINITE(_pos_sp_triplet.current.loiter_radius) ? 1 / _pos_sp_triplet.current.loiter_radius :
|
|
|
|
|
0.0f;
|
|
|
|
|
navigatePathTangent(curr_pos_local, curr_wp_local, velocity_2d.normalized(), get_nav_speed_2d(ground_speed),
|
|
|
|
|
_wind_vel, curvature);
|
|
|
|
|
|
|
|
|
|
} else {
|
|
|
|
|
_l1_control.navigate_waypoints(prev_wp_local, curr_wp_local, curr_pos_local, get_nav_speed_2d(ground_speed));
|
|
|
|
|
_att_sp.roll_body = _l1_control.get_roll_setpoint();
|
|
|
|
|
navigateWaypoints(prev_wp_local, curr_wp_local, curr_pos_local, get_nav_speed_2d(ground_speed), _wind_vel);
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
_att_sp.roll_body = _npfg.getRollSetpoint();
|
|
|
|
|
target_airspeed = _npfg.getAirspeedRef() / _eas2tas;
|
|
|
|
|
|
|
|
|
|
_att_sp.yaw_body = _yaw; // yaw is not controlled, so set setpoint to current yaw
|
|
|
|
|
|
|
|
|
|
tecs_update_pitch_throttle(control_interval,
|
|
|
|
|
@ -1186,18 +1148,12 @@ FixedwingPositionControl::control_auto_velocity(const float control_interval, co
|
|
|
|
|
float target_airspeed = adapt_airspeed_setpoint(control_interval, pos_sp_curr.cruising_speed,
|
|
|
|
|
_param_fw_airspd_min.get(), ground_speed);
|
|
|
|
|
|
|
|
|
|
if (_param_fw_use_npfg.get()) {
|
|
|
|
|
Vector2f curr_pos_local{_local_pos.x, _local_pos.y};
|
|
|
|
|
_npfg.setAirspeedNom(target_airspeed * _eas2tas);
|
|
|
|
|
_npfg.setAirspeedMax(_param_fw_airspd_max.get() * _eas2tas);
|
|
|
|
|
navigateBearing(curr_pos_local, _target_bearing, ground_speed, _wind_vel);
|
|
|
|
|
_att_sp.roll_body = _npfg.getRollSetpoint();
|
|
|
|
|
target_airspeed = _npfg.getAirspeedRef() / _eas2tas;
|
|
|
|
|
|
|
|
|
|
} else {
|
|
|
|
|
_l1_control.navigate_heading(_target_bearing, _yaw, ground_speed);
|
|
|
|
|
_att_sp.roll_body = _l1_control.get_roll_setpoint();
|
|
|
|
|
}
|
|
|
|
|
Vector2f curr_pos_local{_local_pos.x, _local_pos.y};
|
|
|
|
|
_npfg.setAirspeedNom(target_airspeed * _eas2tas);
|
|
|
|
|
_npfg.setAirspeedMax(_param_fw_airspd_max.get() * _eas2tas);
|
|
|
|
|
navigateBearing(curr_pos_local, _target_bearing, ground_speed, _wind_vel);
|
|
|
|
|
_att_sp.roll_body = _npfg.getRollSetpoint();
|
|
|
|
|
target_airspeed = _npfg.getAirspeedRef() / _eas2tas;
|
|
|
|
|
|
|
|
|
|
_att_sp.yaw_body = _yaw;
|
|
|
|
|
|
|
|
|
|
@ -1286,21 +1242,13 @@ FixedwingPositionControl::control_auto_loiter(const float control_interval, cons
|
|
|
|
|
float target_airspeed = adapt_airspeed_setpoint(control_interval, airspeed_sp, _param_fw_airspd_min.get(),
|
|
|
|
|
ground_speed);
|
|
|
|
|
|
|
|
|
|
if (_param_fw_use_npfg.get()) {
|
|
|
|
|
_npfg.setAirspeedNom(target_airspeed * _eas2tas);
|
|
|
|
|
_npfg.setAirspeedMax(_param_fw_airspd_max.get() * _eas2tas);
|
|
|
|
|
navigateLoiter(curr_wp_local, curr_pos_local, loiter_radius, pos_sp_curr.loiter_direction_counter_clockwise,
|
|
|
|
|
get_nav_speed_2d(ground_speed),
|
|
|
|
|
_wind_vel);
|
|
|
|
|
_att_sp.roll_body = _npfg.getRollSetpoint();
|
|
|
|
|
target_airspeed = _npfg.getAirspeedRef() / _eas2tas;
|
|
|
|
|
|
|
|
|
|
} else {
|
|
|
|
|
_l1_control.navigate_loiter(curr_wp_local, curr_pos_local, loiter_radius,
|
|
|
|
|
pos_sp_curr.loiter_direction_counter_clockwise,
|
|
|
|
|
get_nav_speed_2d(ground_speed));
|
|
|
|
|
_att_sp.roll_body = _l1_control.get_roll_setpoint();
|
|
|
|
|
}
|
|
|
|
|
_npfg.setAirspeedNom(target_airspeed * _eas2tas);
|
|
|
|
|
_npfg.setAirspeedMax(_param_fw_airspd_max.get() * _eas2tas);
|
|
|
|
|
navigateLoiter(curr_wp_local, curr_pos_local, loiter_radius, pos_sp_curr.loiter_direction_counter_clockwise,
|
|
|
|
|
get_nav_speed_2d(ground_speed),
|
|
|
|
|
_wind_vel);
|
|
|
|
|
_att_sp.roll_body = _npfg.getRollSetpoint();
|
|
|
|
|
target_airspeed = _npfg.getAirspeedRef() / _eas2tas;
|
|
|
|
|
|
|
|
|
|
_att_sp.yaw_body = _yaw; // yaw is not controlled, so set setpoint to current yaw
|
|
|
|
|
|
|
|
|
|
@ -1391,8 +1339,6 @@ FixedwingPositionControl::control_auto_takeoff(const hrt_abstime &now, const flo
|
|
|
|
|
// tune up the lateral position control guidance when on the ground
|
|
|
|
|
if (_att_sp.fw_control_yaw_wheel) {
|
|
|
|
|
_npfg.setPeriod(_param_rwto_l1_period.get());
|
|
|
|
|
_l1_control.set_l1_period(_param_rwto_l1_period.get());
|
|
|
|
|
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
const Vector2f start_pos_local = _global_local_proj_ref.project(_runway_takeoff.getStartPosition()(0),
|
|
|
|
|
@ -1407,37 +1353,20 @@ FixedwingPositionControl::control_auto_takeoff(const hrt_abstime &now, const flo
|
|
|
|
|
takeoff_bearing_vector = calculateTakeoffBearingVector(start_pos_local, takeoff_waypoint_local);
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
if (_param_fw_use_npfg.get()) {
|
|
|
|
|
_npfg.setAirspeedNom(target_airspeed * _eas2tas);
|
|
|
|
|
_npfg.setAirspeedMax(_param_fw_airspd_max.get() * _eas2tas);
|
|
|
|
|
navigatePathTangent(local_2D_position, start_pos_local, takeoff_bearing_vector, ground_speed,
|
|
|
|
|
_wind_vel, 0.0f);
|
|
|
|
|
_npfg.setAirspeedNom(target_airspeed * _eas2tas);
|
|
|
|
|
_npfg.setAirspeedMax(_param_fw_airspd_max.get() * _eas2tas);
|
|
|
|
|
navigatePathTangent(local_2D_position, start_pos_local, takeoff_bearing_vector, ground_speed,
|
|
|
|
|
_wind_vel, 0.0f);
|
|
|
|
|
|
|
|
|
|
_att_sp.roll_body = _runway_takeoff.getRoll(_npfg.getRollSetpoint());
|
|
|
|
|
_att_sp.roll_body = _runway_takeoff.getRoll(_npfg.getRollSetpoint());
|
|
|
|
|
|
|
|
|
|
target_airspeed = _npfg.getAirspeedRef() / _eas2tas;
|
|
|
|
|
target_airspeed = _npfg.getAirspeedRef() / _eas2tas;
|
|
|
|
|
|
|
|
|
|
// use npfg's bearing to commanded course, controlled via yaw angle while on runway
|
|
|
|
|
const float bearing = _npfg.getBearing();
|
|
|
|
|
// use npfg's bearing to commanded course, controlled via yaw angle while on runway
|
|
|
|
|
const float bearing = _npfg.getBearing();
|
|
|
|
|
|
|
|
|
|
// heading hold mode will override this bearing setpoint
|
|
|
|
|
_att_sp.yaw_body = _runway_takeoff.getYaw(bearing);
|
|
|
|
|
|
|
|
|
|
} else {
|
|
|
|
|
// make a fake waypoint in the direction of the takeoff bearing
|
|
|
|
|
const Vector2f virtual_waypoint = start_pos_local + takeoff_bearing_vector * HDG_HOLD_DIST_NEXT;
|
|
|
|
|
|
|
|
|
|
_l1_control.navigate_waypoints(start_pos_local, virtual_waypoint, local_2D_position, ground_speed);
|
|
|
|
|
|
|
|
|
|
const float l1_roll_setpoint = _l1_control.get_roll_setpoint();
|
|
|
|
|
_att_sp.roll_body = _runway_takeoff.getRoll(l1_roll_setpoint);
|
|
|
|
|
|
|
|
|
|
// use l1's nav bearing to command course, controlled via yaw angle while on runway
|
|
|
|
|
const float bearing = _l1_control.nav_bearing();
|
|
|
|
|
|
|
|
|
|
// heading hold mode will override this bearing setpoint
|
|
|
|
|
_att_sp.yaw_body = _runway_takeoff.getYaw(bearing);
|
|
|
|
|
}
|
|
|
|
|
// heading hold mode will override this bearing setpoint
|
|
|
|
|
_att_sp.yaw_body = _runway_takeoff.getYaw(bearing);
|
|
|
|
|
|
|
|
|
|
// update tecs
|
|
|
|
|
const float pitch_max = _runway_takeoff.getMaxPitch(math::radians(_param_fw_p_lim_max.get()));
|
|
|
|
|
@ -1512,20 +1441,13 @@ FixedwingPositionControl::control_auto_takeoff(const hrt_abstime &now, const flo
|
|
|
|
|
&& _param_fw_laun_detcn_on.get()) {
|
|
|
|
|
/* Launch has been detected, hence we have to control the plane. */
|
|
|
|
|
|
|
|
|
|
if (_param_fw_use_npfg.get()) {
|
|
|
|
|
_npfg.setAirspeedNom(target_airspeed * _eas2tas);
|
|
|
|
|
_npfg.setAirspeedMax(_param_fw_airspd_max.get() * _eas2tas);
|
|
|
|
|
navigatePathTangent(local_2D_position, launch_local_position, takeoff_bearing_vector, ground_speed, _wind_vel,
|
|
|
|
|
0.0f);
|
|
|
|
|
_att_sp.roll_body = _npfg.getRollSetpoint();
|
|
|
|
|
target_airspeed = _npfg.getAirspeedRef() / _eas2tas;
|
|
|
|
|
_npfg.setAirspeedNom(target_airspeed * _eas2tas);
|
|
|
|
|
_npfg.setAirspeedMax(_param_fw_airspd_max.get() * _eas2tas);
|
|
|
|
|
navigatePathTangent(local_2D_position, launch_local_position, takeoff_bearing_vector, ground_speed, _wind_vel,
|
|
|
|
|
0.0f);
|
|
|
|
|
_att_sp.roll_body = _npfg.getRollSetpoint();
|
|
|
|
|
target_airspeed = _npfg.getAirspeedRef() / _eas2tas;
|
|
|
|
|
|
|
|
|
|
} else {
|
|
|
|
|
// make a fake waypoint in the direction of the takeoff bearing
|
|
|
|
|
const Vector2f virtual_waypoint = launch_local_position + takeoff_bearing_vector * HDG_HOLD_DIST_NEXT;
|
|
|
|
|
_l1_control.navigate_waypoints(launch_local_position, virtual_waypoint, local_2D_position, ground_speed);
|
|
|
|
|
_att_sp.roll_body = _l1_control.get_roll_setpoint();
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
const float max_takeoff_throttle = (_launchDetector.getLaunchDetected() < launch_detection_status_s::STATE_FLYING) ?
|
|
|
|
|
_param_fw_thr_idle.get() : _param_fw_thr_max.get();
|
|
|
|
|
@ -1658,39 +1580,18 @@ FixedwingPositionControl::control_auto_landing(const hrt_abstime &now, const flo
|
|
|
|
|
// tune up the lateral position control guidance when on the ground
|
|
|
|
|
const float ground_roll_npfg_period = flare_ramp_interpolator * _param_rwto_l1_period.get() +
|
|
|
|
|
(1.0f - flare_ramp_interpolator) * _param_npfg_period.get();
|
|
|
|
|
const float ground_roll_l1_period = flare_ramp_interpolator * _param_rwto_l1_period.get() +
|
|
|
|
|
(1.0f - flare_ramp_interpolator) * _param_fw_l1_period.get();
|
|
|
|
|
_npfg.setPeriod(ground_roll_npfg_period);
|
|
|
|
|
_l1_control.set_l1_period(ground_roll_l1_period);
|
|
|
|
|
|
|
|
|
|
const Vector2f local_approach_entrance = local_land_point - landing_approach_vector;
|
|
|
|
|
|
|
|
|
|
if (_param_fw_use_npfg.get()) {
|
|
|
|
|
_npfg.setAirspeedNom(target_airspeed * _eas2tas);
|
|
|
|
|
_npfg.setAirspeedMax(_param_fw_airspd_max.get() * _eas2tas);
|
|
|
|
|
navigateWaypoints(local_approach_entrance, local_land_point, local_position, ground_speed, _wind_vel);
|
|
|
|
|
target_airspeed = _npfg.getAirspeedRef() / _eas2tas;
|
|
|
|
|
_att_sp.roll_body = _npfg.getRollSetpoint();
|
|
|
|
|
_npfg.setAirspeedNom(target_airspeed * _eas2tas);
|
|
|
|
|
_npfg.setAirspeedMax(_param_fw_airspd_max.get() * _eas2tas);
|
|
|
|
|
navigateWaypoints(local_approach_entrance, local_land_point, local_position, ground_speed, _wind_vel);
|
|
|
|
|
target_airspeed = _npfg.getAirspeedRef() / _eas2tas;
|
|
|
|
|
_att_sp.roll_body = _npfg.getRollSetpoint();
|
|
|
|
|
|
|
|
|
|
// use npfg's bearing to commanded course, controlled via yaw angle while on runway
|
|
|
|
|
_att_sp.yaw_body = _npfg.getBearing();
|
|
|
|
|
|
|
|
|
|
} else {
|
|
|
|
|
// make a fake waypoint beyond the land point in the direction of the landing approach bearing
|
|
|
|
|
// (always HDG_HOLD_DIST_NEXT meters in front of the aircraft's progress along the landing approach vector)
|
|
|
|
|
|
|
|
|
|
const float along_track_distance_from_entrance = landing_approach_vector.unit_or_zero().dot(
|
|
|
|
|
local_position - local_approach_entrance);
|
|
|
|
|
|
|
|
|
|
const Vector2f virtual_waypoint = local_approach_entrance + (along_track_distance_from_entrance + HDG_HOLD_DIST_NEXT) *
|
|
|
|
|
landing_approach_vector.unit_or_zero();
|
|
|
|
|
|
|
|
|
|
_l1_control.navigate_waypoints(local_approach_entrance, virtual_waypoint, local_position, ground_speed);
|
|
|
|
|
_att_sp.roll_body = _l1_control.get_roll_setpoint();
|
|
|
|
|
|
|
|
|
|
// use l1's nav bearing to command course, controlled via yaw angle while on runway
|
|
|
|
|
_att_sp.yaw_body = _l1_control.nav_bearing();
|
|
|
|
|
}
|
|
|
|
|
// use npfg's bearing to commanded course, controlled via yaw angle while on runway
|
|
|
|
|
_att_sp.yaw_body = _npfg.getBearing();
|
|
|
|
|
|
|
|
|
|
/* longitudinal guidance */
|
|
|
|
|
|
|
|
|
|
@ -1762,26 +1663,11 @@ FixedwingPositionControl::control_auto_landing(const hrt_abstime &now, const flo
|
|
|
|
|
|
|
|
|
|
const Vector2f local_approach_entrance = local_land_point - landing_approach_vector;
|
|
|
|
|
|
|
|
|
|
if (_param_fw_use_npfg.get()) {
|
|
|
|
|
_npfg.setAirspeedNom(target_airspeed * _eas2tas);
|
|
|
|
|
_npfg.setAirspeedMax(_param_fw_airspd_max.get() * _eas2tas);
|
|
|
|
|
navigateWaypoints(local_approach_entrance, local_land_point, local_position, ground_speed, _wind_vel);
|
|
|
|
|
target_airspeed = _npfg.getAirspeedRef() / _eas2tas;
|
|
|
|
|
_att_sp.roll_body = _npfg.getRollSetpoint();
|
|
|
|
|
|
|
|
|
|
} else {
|
|
|
|
|
// make a fake waypoint beyond the land point in the direction of the landing approach bearing
|
|
|
|
|
// (always HDG_HOLD_DIST_NEXT meters in front of the aircraft's progress along the landing approach vector)
|
|
|
|
|
|
|
|
|
|
const float along_track_distance_from_entrance = landing_approach_vector.unit_or_zero().dot(
|
|
|
|
|
local_position - local_approach_entrance);
|
|
|
|
|
|
|
|
|
|
const Vector2f virtual_waypoint = local_approach_entrance + (along_track_distance_from_entrance + HDG_HOLD_DIST_NEXT) *
|
|
|
|
|
landing_approach_vector.unit_or_zero();
|
|
|
|
|
|
|
|
|
|
_l1_control.navigate_waypoints(local_approach_entrance, virtual_waypoint, local_position, ground_speed);
|
|
|
|
|
_att_sp.roll_body = _l1_control.get_roll_setpoint();
|
|
|
|
|
}
|
|
|
|
|
_npfg.setAirspeedNom(target_airspeed * _eas2tas);
|
|
|
|
|
_npfg.setAirspeedMax(_param_fw_airspd_max.get() * _eas2tas);
|
|
|
|
|
navigateWaypoints(local_approach_entrance, local_land_point, local_position, ground_speed, _wind_vel);
|
|
|
|
|
target_airspeed = _npfg.getAirspeedRef() / _eas2tas;
|
|
|
|
|
_att_sp.roll_body = _npfg.getRollSetpoint();
|
|
|
|
|
|
|
|
|
|
/* longitudinal guidance */
|
|
|
|
|
|
|
|
|
|
@ -1937,18 +1823,11 @@ FixedwingPositionControl::control_manual_position(const float control_interval,
|
|
|
|
|
Vector2f prev_wp_local = _global_local_proj_ref.project(prev_wp(0),
|
|
|
|
|
prev_wp(1));
|
|
|
|
|
|
|
|
|
|
if (_param_fw_use_npfg.get()) {
|
|
|
|
|
_npfg.setAirspeedNom(calibrated_airspeed_sp * _eas2tas);
|
|
|
|
|
_npfg.setAirspeedMax(_param_fw_airspd_max.get() * _eas2tas);
|
|
|
|
|
navigateWaypoints(prev_wp_local, curr_wp_local, curr_pos_local, ground_speed, _wind_vel);
|
|
|
|
|
_att_sp.roll_body = _npfg.getRollSetpoint();
|
|
|
|
|
calibrated_airspeed_sp = _npfg.getAirspeedRef() / _eas2tas;
|
|
|
|
|
|
|
|
|
|
} else {
|
|
|
|
|
/* populate l1 control setpoint */
|
|
|
|
|
_l1_control.navigate_waypoints(prev_wp_local, curr_wp_local, curr_pos_local, ground_speed);
|
|
|
|
|
_att_sp.roll_body = _l1_control.get_roll_setpoint();
|
|
|
|
|
}
|
|
|
|
|
_npfg.setAirspeedNom(calibrated_airspeed_sp * _eas2tas);
|
|
|
|
|
_npfg.setAirspeedMax(_param_fw_airspd_max.get() * _eas2tas);
|
|
|
|
|
navigateWaypoints(prev_wp_local, curr_wp_local, curr_pos_local, ground_speed, _wind_vel);
|
|
|
|
|
_att_sp.roll_body = _npfg.getRollSetpoint();
|
|
|
|
|
calibrated_airspeed_sp = _npfg.getAirspeedRef() / _eas2tas;
|
|
|
|
|
|
|
|
|
|
_att_sp.yaw_body = _yaw; // yaw is not controlled, so set setpoint to current yaw
|
|
|
|
|
}
|
|
|
|
|
@ -2186,12 +2065,7 @@ FixedwingPositionControl::Run()
|
|
|
|
|
update_in_air_states(_local_pos.timestamp);
|
|
|
|
|
|
|
|
|
|
// update lateral guidance timesteps for slewrates
|
|
|
|
|
if (_param_fw_use_npfg.get()) {
|
|
|
|
|
_npfg.setDt(control_interval);
|
|
|
|
|
|
|
|
|
|
} else {
|
|
|
|
|
_l1_control.set_dt(control_interval);
|
|
|
|
|
}
|
|
|
|
|
_npfg.setDt(control_interval);
|
|
|
|
|
|
|
|
|
|
// restore nominal TECS parameters in case changed intermittently (e.g. in landing handling)
|
|
|
|
|
_tecs.set_speed_weight(_param_fw_t_spdweight.get());
|
|
|
|
|
@ -2199,7 +2073,6 @@ FixedwingPositionControl::Run()
|
|
|
|
|
|
|
|
|
|
// restore lateral-directional guidance parameters (changed in takeoff mode)
|
|
|
|
|
_npfg.setPeriod(_param_npfg_period.get());
|
|
|
|
|
_l1_control.set_l1_period(_param_fw_l1_period.get());
|
|
|
|
|
|
|
|
|
|
_att_sp.reset_integral = false;
|
|
|
|
|
_att_sp.apply_flaps = vehicle_attitude_setpoint_s::FLAPS_OFF;
|
|
|
|
|
@ -2295,8 +2168,6 @@ FixedwingPositionControl::Run()
|
|
|
|
|
|
|
|
|
|
}
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
_l1_control.reset_has_guidance_updated();
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
perf_end(_loop_perf);
|
|
|
|
|
@ -2333,19 +2204,13 @@ FixedwingPositionControl::reset_landing_state()
|
|
|
|
|
}
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
bool FixedwingPositionControl::using_npfg_with_wind_estimate() const
|
|
|
|
|
{
|
|
|
|
|
// high wind mitigation is handled by NPFG if the wind estimate is valid
|
|
|
|
|
return _param_fw_use_npfg.get() && _wind_valid;
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
Vector2f
|
|
|
|
|
FixedwingPositionControl::get_nav_speed_2d(const Vector2f &ground_speed)
|
|
|
|
|
{
|
|
|
|
|
|
|
|
|
|
Vector2f nav_speed_2d{ground_speed};
|
|
|
|
|
|
|
|
|
|
if (_airspeed_valid && !using_npfg_with_wind_estimate()) {
|
|
|
|
|
if (_airspeed_valid && !_wind_valid) {
|
|
|
|
|
// l1 navigation logic breaks down when wind speed exceeds max airspeed
|
|
|
|
|
// compute 2D groundspeed from airspeed-heading projection
|
|
|
|
|
const Vector2f air_speed_2d{_airspeed * cosf(_yaw), _airspeed * sinf(_yaw)};
|
|
|
|
|
@ -2683,14 +2548,7 @@ void FixedwingPositionControl::publishLocalPositionSetpoint(const position_setpo
|
|
|
|
|
|
|
|
|
|
Vector2f current_setpoint;
|
|
|
|
|
|
|
|
|
|
if (!_param_fw_use_npfg.get()) {
|
|
|
|
|
if (_global_local_proj_ref.isInitialized()) {
|
|
|
|
|
current_setpoint = _global_local_proj_ref.project(current_waypoint.lat, current_waypoint.lon);
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
} else {
|
|
|
|
|
current_setpoint = _closest_point_on_path;
|
|
|
|
|
}
|
|
|
|
|
current_setpoint = _closest_point_on_path;
|
|
|
|
|
|
|
|
|
|
local_position_setpoint.x = current_setpoint(0);
|
|
|
|
|
local_position_setpoint.y = current_setpoint(1);
|
|
|
|
|
|
Konrad
Silvan Fuhrer