zrzk
/
px4_autopilot
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity
Browse Source

fw_pos_ctrl_l1 move landing to control_landing()

sbg
Daniel Agar 7 years ago
parent
f194c5424c
commit
c72340e039
2 changed files with 251 additions and 224 deletions
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Split View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 472
      src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp
  2. 3
      src/modules/fw_pos_control_l1/FixedwingPositionControl.hpp

472
src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp
Unescape View File

@ -802,230 +802,7 @@ FixedwingPositionControl::control_position(const math::Vector<2> &curr_pos, cons @@ -802,230 +802,7 @@ FixedwingPositionControl::control_position(const math::Vector<2> &curr_pos, cons
} else if (pos_sp_curr.type == position_setpoint_s::SETPOINT_TYPE_LAND) {
// apply full flaps for landings. this flag will also trigger the use of flaperons
// if they have been enabled using the corresponding parameter
_att_sp.apply_flaps = true;
// save time at which we started landing and reset abort_landing
if (_time_started_landing == 0) {
_time_started_landing = hrt_absolute_time();
_fw_pos_ctrl_status.abort_landing = false;
}
float bearing_lastwp_currwp = get_bearing_to_next_waypoint(prev_wp(0), prev_wp(1), curr_wp(0), curr_wp(1));
float bearing_airplane_currwp = get_bearing_to_next_waypoint(curr_pos(0), curr_pos(1), curr_wp(0), curr_wp(1));
/* Horizontal landing control */
/* switch to heading hold for the last meters, continue heading hold after */
float wp_distance = get_distance_to_next_waypoint(curr_pos(0), curr_pos(1), curr_wp(0), curr_wp(1));
/* calculate a waypoint distance value which is 0 when the aircraft is behind the waypoint */
float wp_distance_save = wp_distance;
if (fabsf(bearing_airplane_currwp - bearing_lastwp_currwp) >= radians(90.0f)) {
wp_distance_save = 0.0f;
}
// we want the plane to keep tracking the desired flight path until we start flaring
// if we go into heading hold mode earlier then we risk to be pushed away from the runway by cross winds
if ((_parameters.land_heading_hold_horizontal_distance > 0.0f) && !_land_noreturn_horizontal &&
((wp_distance < _parameters.land_heading_hold_horizontal_distance) || _land_noreturn_vertical)) {
if (pos_sp_prev.valid) {
/* heading hold, along the line connecting this and the last waypoint */
_target_bearing = bearing_lastwp_currwp;
} else {
_target_bearing = _yaw;
}
_land_noreturn_horizontal = true;
mavlink_log_info(&_mavlink_log_pub, "Landing, heading hold");
}
if (_land_noreturn_horizontal) {
// heading hold
_l1_control.navigate_heading(_target_bearing, _yaw, nav_speed_2d);
} else {
// normal navigation
_l1_control.navigate_waypoints(prev_wp, curr_wp, curr_pos, nav_speed_2d);
}
_att_sp.roll_body = _l1_control.nav_roll();
_att_sp.yaw_body = _l1_control.nav_bearing();
if (_land_noreturn_horizontal) {
/* limit roll motion to prevent wings from touching the ground first */
_att_sp.roll_body = constrain(_att_sp.roll_body, radians(-10.0f), radians(10.0f));
}
/* Vertical landing control */
/* apply minimum pitch (flare) and limit roll if close to touch down, altitude error is negative (going down) */
float throttle_land = _parameters.throttle_min + (_parameters.throttle_max - _parameters.throttle_min) * 0.1f;
float airspeed_land = _parameters.land_airspeed_scale * _parameters.airspeed_min;
float airspeed_approach = _parameters.land_airspeed_scale * _parameters.airspeed_min;
// default to no terrain estimation, just use landing waypoint altitude
float terrain_alt = pos_sp_curr.alt;
if (_parameters.land_use_terrain_estimate == 1) {
if (_global_pos.terrain_alt_valid) {
// all good, have valid terrain altitude
terrain_alt = _global_pos.terrain_alt;
_t_alt_prev_valid = terrain_alt;
_time_last_t_alt = hrt_absolute_time();
} else if (_time_last_t_alt == 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
if (hrt_elapsed_time(&_time_started_landing) < 10 * 1000 * 1000) {
terrain_alt = pos_sp_curr.alt;
} else {
// still no valid terrain, abort landing
terrain_alt = pos_sp_curr.alt;
_fw_pos_ctrl_status.abort_landing = true;
}
} else if ((!_global_pos.terrain_alt_valid && hrt_elapsed_time(&_time_last_t_alt) < T_ALT_TIMEOUT * 1000 * 1000)
|| _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;
} else {
// terrain alt was not valid for long time, abort landing
terrain_alt = _t_alt_prev_valid;
_fw_pos_ctrl_status.abort_landing = true;
}
}
/* Calculate distance (to landing waypoint) and altitude of last ordinary waypoint L */
float L_altitude_rel = 0.0f;
if (pos_sp_prev.valid) {
L_altitude_rel = pos_sp_prev.alt - terrain_alt;
}
float landing_slope_alt_rel_desired = _landingslope.getLandingSlopeRelativeAltitudeSave(wp_distance,
bearing_lastwp_currwp, bearing_airplane_currwp);
/* Check if we should start flaring with a vertical and a
* horizontal limit (with some tolerance)
* The horizontal limit is only applied when we are in front of the wp
*/
if (((_global_pos.alt < terrain_alt + _landingslope.flare_relative_alt()) &&
(wp_distance_save < _landingslope.flare_length() + 5.0f)) ||
_land_noreturn_vertical) { //checking for land_noreturn to avoid unwanted climb out
/* land with minimal speed */
/* force TECS to only control speed with pitch, altitude is only implicitly controlled now */
// _tecs.set_speed_weight(2.0f);
/* kill the throttle if param requests it */
throttle_max = _parameters.throttle_max;
/* enable direct yaw control using rudder/wheel */
if (_land_noreturn_horizontal) {
_att_sp.yaw_body = _target_bearing;
_att_sp.fw_control_yaw = true;
}
if (_global_pos.alt < terrain_alt + _landingslope.motor_lim_relative_alt() || _land_motor_lim) {
throttle_max = min(throttle_max, _parameters.throttle_land_max);
if (!_land_motor_lim) {
_land_motor_lim = true;
mavlink_log_info(&_mavlink_log_pub, "Landing, limiting throttle");
}
}
float flare_curve_alt_rel = _landingslope.getFlareCurveRelativeAltitudeSave(wp_distance, bearing_lastwp_currwp,
bearing_airplane_currwp);
/* avoid climbout */
if ((_flare_curve_alt_rel_last < flare_curve_alt_rel && _land_noreturn_vertical) || _land_stayonground) {
flare_curve_alt_rel = 0.0f; // stay on ground
_land_stayonground = true;
}
tecs_update_pitch_throttle(terrain_alt + flare_curve_alt_rel,
calculate_target_airspeed(airspeed_land),
radians(_parameters.land_flare_pitch_min_deg),
radians(_parameters.land_flare_pitch_max_deg),
0.0f,
throttle_max,
throttle_land,
false,
_land_motor_lim ? radians(_parameters.land_flare_pitch_min_deg) : radians(_parameters.pitch_limit_min),
_land_motor_lim ? tecs_status_s::TECS_MODE_LAND_THROTTLELIM : tecs_status_s::TECS_MODE_LAND);
if (!_land_noreturn_vertical) {
// just started with the flaring phase
_att_sp.pitch_body = 0.0f;
_flare_height = _global_pos.alt - terrain_alt;
mavlink_log_info(&_mavlink_log_pub, "Landing, flaring");
_land_noreturn_vertical = true;
} else {
if (_global_pos.vel_d > 0.1f) {
_att_sp.pitch_body = radians(_parameters.land_flare_pitch_min_deg) *
constrain((_flare_height - (_global_pos.alt - terrain_alt)) / _flare_height, 0.0f, 1.0f);
}
// otherwise continue using previous _att_sp.pitch_body
}
_flare_curve_alt_rel_last = flare_curve_alt_rel;
} else {
/* intersect glide slope:
* minimize speed to approach speed
* if current position is higher than the slope follow the glide slope (sink to the
* glide slope)
* also if the system captures the slope it should stay
* on the slope (bool land_onslope)
* if current position is below the slope continue at previous wp altitude
* until the intersection with slope
* */
float altitude_desired_rel{0.0f};
if (_global_pos.alt > terrain_alt + landing_slope_alt_rel_desired || _land_onslope) {
/* stay on slope */
altitude_desired_rel = landing_slope_alt_rel_desired;
if (!_land_onslope) {
mavlink_log_info(&_mavlink_log_pub, "Landing, on slope");
_land_onslope = true;
}
} else {
/* continue horizontally */
if (pos_sp_prev.valid) {
altitude_desired_rel = L_altitude_rel;
} else {
altitude_desired_rel = _global_pos.alt - terrain_alt;
}
}
tecs_update_pitch_throttle(terrain_alt + altitude_desired_rel,
calculate_target_airspeed(airspeed_approach),
radians(_parameters.pitch_limit_min),
radians(_parameters.pitch_limit_max),
_parameters.throttle_min,
_parameters.throttle_max,
_parameters.throttle_cruise,
false,
radians(_parameters.pitch_limit_min));
}
control_landing(curr_pos, ground_speed, pos_sp_prev, pos_sp_curr);
} else if (pos_sp_curr.type == position_setpoint_s::SETPOINT_TYPE_TAKEOFF) {
@ -1427,6 +1204,253 @@ FixedwingPositionControl::control_position(const math::Vector<2> &curr_pos, cons @@ -1427,6 +1204,253 @@ FixedwingPositionControl::control_position(const math::Vector<2> &curr_pos, cons
return setpoint;
}
void
FixedwingPositionControl::control_landing(const math::Vector<2> &curr_pos, const math::Vector<2> &ground_speed,
const position_setpoint_s &pos_sp_prev, const position_setpoint_s &pos_sp_curr)
{
/* current waypoint (the one currently heading for) */
math::Vector<2> curr_wp((float)pos_sp_curr.lat, (float)pos_sp_curr.lon);
math::Vector<2> prev_wp{0.0f, 0.0f}; /* previous waypoint */
if (pos_sp_prev.valid) {
prev_wp(0) = (float)pos_sp_prev.lat;
prev_wp(1) = (float)pos_sp_prev.lon;
} else {
/*
* No valid previous waypoint, go for the current wp.
* This is automatically handled by the L1 library.
*/
prev_wp(0) = (float)pos_sp_curr.lat;
prev_wp(1) = (float)pos_sp_curr.lon;
}
// apply full flaps for landings. this flag will also trigger the use of flaperons
// if they have been enabled using the corresponding parameter
_att_sp.apply_flaps = true;
// save time at which we started landing and reset abort_landing
if (_time_started_landing == 0) {
_time_started_landing = hrt_absolute_time();
_fw_pos_ctrl_status.abort_landing = false;
}
float bearing_lastwp_currwp = get_bearing_to_next_waypoint(prev_wp(0), prev_wp(1), curr_wp(0), curr_wp(1));
float bearing_airplane_currwp = get_bearing_to_next_waypoint(curr_pos(0), curr_pos(1), curr_wp(0), curr_wp(1));
/* Horizontal landing control */
/* switch to heading hold for the last meters, continue heading hold after */
float wp_distance = get_distance_to_next_waypoint(curr_pos(0), curr_pos(1), curr_wp(0), curr_wp(1));
/* calculate a waypoint distance value which is 0 when the aircraft is behind the waypoint */
float wp_distance_save = wp_distance;
if (fabsf(bearing_airplane_currwp - bearing_lastwp_currwp) >= radians(90.0f)) {
wp_distance_save = 0.0f;
}
// we want the plane to keep tracking the desired flight path until we start flaring
// if we go into heading hold mode earlier then we risk to be pushed away from the runway by cross winds
if ((_parameters.land_heading_hold_horizontal_distance > 0.0f) && !_land_noreturn_horizontal &&
((wp_distance < _parameters.land_heading_hold_horizontal_distance) || _land_noreturn_vertical)) {
if (pos_sp_prev.valid) {
/* heading hold, along the line connecting this and the last waypoint */
_target_bearing = bearing_lastwp_currwp;
} else {
_target_bearing = _yaw;
}
_land_noreturn_horizontal = true;
mavlink_log_info(&_mavlink_log_pub, "Landing, heading hold");
}
if (_land_noreturn_horizontal) {
// heading hold
_l1_control.navigate_heading(_target_bearing, _yaw, ground_speed);
} else {
// normal navigation
_l1_control.navigate_waypoints(prev_wp, curr_wp, curr_pos, ground_speed);
}
_att_sp.roll_body = _l1_control.nav_roll();
_att_sp.yaw_body = _l1_control.nav_bearing();
if (_land_noreturn_horizontal) {
/* limit roll motion to prevent wings from touching the ground first */
_att_sp.roll_body = constrain(_att_sp.roll_body, radians(-10.0f), radians(10.0f));
}
/* Vertical landing control */
/* apply minimum pitch (flare) and limit roll if close to touch down, altitude error is negative (going down) */
float throttle_land = _parameters.throttle_min + (_parameters.throttle_max - _parameters.throttle_min) * 0.1f;
float airspeed_land = _parameters.land_airspeed_scale * _parameters.airspeed_min;
float airspeed_approach = _parameters.land_airspeed_scale * _parameters.airspeed_min;
// default to no terrain estimation, just use landing waypoint altitude
float terrain_alt = pos_sp_curr.alt;
if (_parameters.land_use_terrain_estimate == 1) {
if (_global_pos.terrain_alt_valid) {
// all good, have valid terrain altitude
terrain_alt = _global_pos.terrain_alt;
_t_alt_prev_valid = terrain_alt;
_time_last_t_alt = hrt_absolute_time();
} else if (_time_last_t_alt == 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
if (hrt_elapsed_time(&_time_started_landing) < 10 * 1000 * 1000) {
terrain_alt = pos_sp_curr.alt;
} else {
// still no valid terrain, abort landing
terrain_alt = pos_sp_curr.alt;
_fw_pos_ctrl_status.abort_landing = true;
}
} else if ((!_global_pos.terrain_alt_valid && hrt_elapsed_time(&_time_last_t_alt) < T_ALT_TIMEOUT * 1000 * 1000)
|| _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;
} else {
// terrain alt was not valid for long time, abort landing
terrain_alt = _t_alt_prev_valid;
_fw_pos_ctrl_status.abort_landing = true;
}
}
/* Calculate distance (to landing waypoint) and altitude of last ordinary waypoint L */
float L_altitude_rel = 0.0f;
if (pos_sp_prev.valid) {
L_altitude_rel = pos_sp_prev.alt - terrain_alt;
}
float landing_slope_alt_rel_desired = _landingslope.getLandingSlopeRelativeAltitudeSave(wp_distance,
bearing_lastwp_currwp, bearing_airplane_currwp);
/* Check if we should start flaring with a vertical and a
* horizontal limit (with some tolerance)
* The horizontal limit is only applied when we are in front of the wp
*/
if (((_global_pos.alt < terrain_alt + _landingslope.flare_relative_alt()) &&
(wp_distance_save < _landingslope.flare_length() + 5.0f)) ||
_land_noreturn_vertical) { //checking for land_noreturn to avoid unwanted climb out
/* land with minimal speed */
/* force TECS to only control speed with pitch, altitude is only implicitly controlled now */
// _tecs.set_speed_weight(2.0f);
/* kill the throttle if param requests it */
float throttle_max = _parameters.throttle_max;
/* enable direct yaw control using rudder/wheel */
if (_land_noreturn_horizontal) {
_att_sp.yaw_body = _target_bearing;
_att_sp.fw_control_yaw = true;
}
if (_global_pos.alt < terrain_alt + _landingslope.motor_lim_relative_alt() || _land_motor_lim) {
throttle_max = min(throttle_max, _parameters.throttle_land_max);
if (!_land_motor_lim) {
_land_motor_lim = true;
mavlink_log_info(&_mavlink_log_pub, "Landing, limiting throttle");
}
}
float flare_curve_alt_rel = _landingslope.getFlareCurveRelativeAltitudeSave(wp_distance, bearing_lastwp_currwp,
bearing_airplane_currwp);
/* avoid climbout */
if ((_flare_curve_alt_rel_last < flare_curve_alt_rel && _land_noreturn_vertical) || _land_stayonground) {
flare_curve_alt_rel = 0.0f; // stay on ground
_land_stayonground = true;
}
tecs_update_pitch_throttle(terrain_alt + flare_curve_alt_rel,
calculate_target_airspeed(airspeed_land),
radians(_parameters.land_flare_pitch_min_deg),
radians(_parameters.land_flare_pitch_max_deg),
0.0f,
throttle_max,
throttle_land,
false,
_land_motor_lim ? radians(_parameters.land_flare_pitch_min_deg) : radians(_parameters.pitch_limit_min),
_land_motor_lim ? tecs_status_s::TECS_MODE_LAND_THROTTLELIM : tecs_status_s::TECS_MODE_LAND);
if (!_land_noreturn_vertical) {
// just started with the flaring phase
_att_sp.pitch_body = 0.0f;
_flare_height = _global_pos.alt - terrain_alt;
mavlink_log_info(&_mavlink_log_pub, "Landing, flaring");
_land_noreturn_vertical = true;
} else {
if (_global_pos.vel_d > 0.1f) {
_att_sp.pitch_body = radians(_parameters.land_flare_pitch_min_deg) *
constrain((_flare_height - (_global_pos.alt - terrain_alt)) / _flare_height, 0.0f, 1.0f);
}
// otherwise continue using previous _att_sp.pitch_body
}
_flare_curve_alt_rel_last = flare_curve_alt_rel;
} else {
/* intersect glide slope:
* minimize speed to approach speed
* if current position is higher than the slope follow the glide slope (sink to the
* glide slope)
* also if the system captures the slope it should stay
* on the slope (bool land_onslope)
* if current position is below the slope continue at previous wp altitude
* until the intersection with slope
* */
float altitude_desired_rel{0.0f};
if (_global_pos.alt > terrain_alt + landing_slope_alt_rel_desired || _land_onslope) {
/* stay on slope */
altitude_desired_rel = landing_slope_alt_rel_desired;
if (!_land_onslope) {
mavlink_log_info(&_mavlink_log_pub, "Landing, on slope");
_land_onslope = true;
}
} else {
/* continue horizontally */
if (pos_sp_prev.valid) {
altitude_desired_rel = L_altitude_rel;
} else {
altitude_desired_rel = _global_pos.alt - terrain_alt;
}
}
tecs_update_pitch_throttle(terrain_alt + altitude_desired_rel,
calculate_target_airspeed(airspeed_approach),
radians(_parameters.pitch_limit_min),
radians(_parameters.pitch_limit_max),
_parameters.throttle_min,
_parameters.throttle_max,
_parameters.throttle_cruise,
false,
radians(_parameters.pitch_limit_min));
}
}
float
FixedwingPositionControl::get_tecs_pitch()
{

3
src/modules/fw_pos_control_l1/FixedwingPositionControl.hpp
Unescape View File

@ -440,6 +440,9 @@ private: @@ -440,6 +440,9 @@ private:
bool control_position(const math::Vector<2> &curr_pos, const math::Vector<2> &ground_speed,
const position_setpoint_s &pos_sp_prev, const position_setpoint_s &pos_sp_curr);
void control_landing(const math::Vector<2> &curr_pos, const math::Vector<2> &ground_speed,
const position_setpoint_s &pos_sp_prev, const position_setpoint_s &pos_sp_curr);
float get_tecs_pitch();
float get_tecs_thrust();

Write Preview
Loading…
Cancel
Save