aepko/Ardupilot2
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Plane: Support a different landing radius for quadplane fw approach

Browse Source
This commit is contained in:
Michael du Breuil 2018-11-16 14:53:43 -07:00 committed by Andrew Tridgell
parent f1154a6e7c
commit 1a45755ffd
3 changed files with 36 additions and 12 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

36
ArduPlane/commands_logic.cpp
View File

@ -418,7 +418,16 @@ void Plane::do_land(const AP_Mission::Mission_Command& cmd)
void Plane::do_landing_vtol_approach(const AP_Mission::Mission_Command& cmd) void Plane::do_landing_vtol_approach(const AP_Mission::Mission_Command& cmd)
{ {
do_loiter_to_alt(cmd); //set target alt
Location loc = cmd.content.location;
location_sanitize(current_loc, loc);
set_next_WP(loc);
if (is_negative(quadplane.fw_land_approach_radius)) {
loiter.direction = -1;
} else if (is_positive(quadplane.fw_land_approach_radius)) {
loiter.direction = 1;
}
vtol_approach_s.approach_stage = LOITER_TO_ALT; vtol_approach_s.approach_stage = LOITER_TO_ALT;
@ -719,6 +728,7 @@ bool Plane::verify_loiter_turns(const AP_Mission::Mission_Command &cmd)
bool Plane::verify_loiter_to_alt(const AP_Mission::Mission_Command &cmd) bool Plane::verify_loiter_to_alt(const AP_Mission::Mission_Command &cmd)
{ {
bool result = false; bool result = false;
update_loiter(cmd.p1); update_loiter(cmd.p1);
// condition_value == 0 means alt has never been reached // condition_value == 0 means alt has never been reached
@ -971,21 +981,23 @@ bool Plane::verify_landing_vtol_approach(const AP_Mission::Mission_Command &cmd)
{ {
switch (vtol_approach_s.approach_stage) { switch (vtol_approach_s.approach_stage) {
case LOITER_TO_ALT: case LOITER_TO_ALT:
if (verify_loiter_to_alt(cmd)) { {
Vector3f wind = ahrs.wind_estimate(); update_loiter(quadplane.fw_land_approach_radius);
vtol_approach_s.approach_direction_deg = degrees(atan2f(-wind.y, -wind.x));
gcs().send_text(MAV_SEVERITY_INFO, "Selected an approach path of %.1f", (double)vtol_approach_s.approach_direction_deg); if (labs(loiter.sum_cd) > 1 && (loiter.reached_target_alt || loiter.unable_to_acheive_target_alt)) {
// select target approach direction Vector3f wind = ahrs.wind_estimate();
// select detransition distance (add in extra distance if the approach does not fit in the required space) vtol_approach_s.approach_direction_deg = degrees(atan2f(-wind.y, -wind.x));
// validate turn gcs().send_text(MAV_SEVERITY_INFO, "Selected an approach path of %.1f", (double)vtol_approach_s.approach_direction_deg);
vtol_approach_s.approach_stage = WAIT_FOR_BREAKOUT; vtol_approach_s.approach_stage = WAIT_FOR_BREAKOUT;
}
break;
} }
break;
case WAIT_FOR_BREAKOUT: case WAIT_FOR_BREAKOUT:
{ {
const float breakout_direction_rad = radians(wrap_180(vtol_approach_s.approach_direction_deg + 270)); const float breakout_direction_rad = radians(wrap_180(vtol_approach_s.approach_direction_deg + 270));
const float radius = is_zero(quadplane.fw_land_approach_radius) ? aparm.loiter_radius : quadplane.fw_land_approach_radius;
nav_controller->update_loiter(cmd.content.location, aparm.loiter_radius, cmd.content.location.flags.loiter_ccw ? -1 : 1); nav_controller->update_loiter(cmd.content.location, radius, cmd.content.location.flags.loiter_ccw ? -1 : 1);
// breakout when within 5 degrees of the opposite direction // breakout when within 5 degrees of the opposite direction
if (fabsf(ahrs.yaw - breakout_direction_rad) < radians(5.0f)) { if (fabsf(ahrs.yaw - breakout_direction_rad) < radians(5.0f)) {
@ -1045,7 +1057,7 @@ bool Plane::verify_loiter_heading(bool init)
return true; return true;
} }
if (get_distance(next_WP_loc, next_nav_cmd.content.location) < abs(aparm.loiter_radius)) { if (get_distance(next_WP_loc, next_nav_cmd.content.location) < fabsf(aparm.loiter_radius)) {
/* Whenever next waypoint is within the loiter radius, /* Whenever next waypoint is within the loiter radius,
maintaining loiter would prevent us from ever pointing toward the next waypoint. maintaining loiter would prevent us from ever pointing toward the next waypoint.
Hence break out of loiter immediately Hence break out of loiter immediately

9
ArduPlane/quadplane.cpp
View File

@ -383,6 +383,15 @@ const AP_Param::GroupInfo QuadPlane::var_info2[] = {
AP_SUBGROUPINFO(qautotune, "AUTOTUNE_", 6, QuadPlane, QAutoTune), AP_SUBGROUPINFO(qautotune, "AUTOTUNE_", 6, QuadPlane, QAutoTune),
#endif #endif
// @Param: FW_LND_APR_RAD
// @DisplayName: Quadplane fixed wing landing approach radius
// @Description: This provides the radius used, when using a fixed wing landing approach. If set to 0 then the WP_LOITER_RAD will be selected.
// @Units: m
// @Range: 0 200
// @Increment: 5
// @User: Advanced
AP_GROUPINFO("FW_LND_APR_RAD", 7, QuadPlane, fw_land_approach_radius, 0),
AP_GROUPEND AP_GROUPEND
}; };

3
ArduPlane/quadplane.h
View File

@ -235,6 +235,9 @@ private:
// Quadplane trim, degrees // Quadplane trim, degrees
AP_Float ahrs_trim_pitch; AP_Float ahrs_trim_pitch;
// fw landing approach radius
AP_Float fw_land_approach_radius;
AP_Int16 rc_speed; AP_Int16 rc_speed;
// min and max PWM for throttle // min and max PWM for throttle