mirror of https://github.com/ArduPilot/ardupilot
Plane: added parameter RTL_RADIUS
this allows the loiter radius for RTL to be controlled separately from the radius for loiter (as requested by a user)
This commit is contained in:
parent
56f8e3d414
commit
23eef91c59
|
@ -728,7 +728,8 @@ void Plane::update_navigation()
|
|||
// wp_distance is in ACTUAL meters, not the *100 meters we get from the GPS
|
||||
// ------------------------------------------------------------------------
|
||||
|
||||
// distance and bearing calcs only
|
||||
uint16_t radius = 0;
|
||||
|
||||
switch(control_mode) {
|
||||
case AUTO:
|
||||
update_commands();
|
||||
|
@ -756,16 +757,24 @@ void Plane::update_navigation()
|
|||
auto_state.checked_for_autoland = true;
|
||||
}
|
||||
// fall through to LOITER
|
||||
if (g.rtl_radius < 0) {
|
||||
loiter.direction = -1;
|
||||
} else {
|
||||
loiter.direction = 1;
|
||||
}
|
||||
radius = abs(g.rtl_radius);
|
||||
|
||||
case LOITER:
|
||||
case GUIDED:
|
||||
// allow loiter direction to be changed in flight
|
||||
if (radius == 0) {
|
||||
if (g.loiter_radius < 0) {
|
||||
loiter.direction = -1;
|
||||
} else {
|
||||
loiter.direction = 1;
|
||||
}
|
||||
update_loiter();
|
||||
}
|
||||
update_loiter(abs(radius));
|
||||
break;
|
||||
|
||||
case CRUISE:
|
||||
|
|
|
@ -319,6 +319,15 @@ const AP_Param::Info Plane::var_info[] = {
|
|||
// @User: Standard
|
||||
GSCALAR(loiter_radius, "WP_LOITER_RAD", LOITER_RADIUS_DEFAULT),
|
||||
|
||||
// @Param: RTL_RADIUS
|
||||
// @DisplayName: RTL loiter radius
|
||||
// @Description: Defines the radius of the loiter circle when in RTL mode. If this is zero then WP_LOITER_RAD is used. If the radius is negative then a counter-clockwise is used. If positive then a clockwise loiter is used.
|
||||
// @Units: Meters
|
||||
// @Range: -32767 32767
|
||||
// @Increment: 1
|
||||
// @User: Standard
|
||||
GSCALAR(rtl_radius, "RTL_RADIUS", 0),
|
||||
|
||||
#if GEOFENCE_ENABLED == ENABLED
|
||||
// @Param: FENCE_ACTION
|
||||
// @DisplayName: Action on geofence breach
|
||||
|
|
|
@ -277,6 +277,7 @@ public:
|
|||
k_param_kff_throttle_to_pitch,
|
||||
k_param_scaling_speed,
|
||||
k_param_quadplane,
|
||||
k_param_rtl_radius,
|
||||
|
||||
//
|
||||
// 210: flight modes
|
||||
|
@ -383,6 +384,7 @@ public:
|
|||
AP_Int16 waypoint_radius;
|
||||
AP_Int16 waypoint_max_radius;
|
||||
AP_Int16 loiter_radius;
|
||||
AP_Int16 rtl_radius;
|
||||
|
||||
#if GEOFENCE_ENABLED == ENABLED
|
||||
AP_Int8 fence_action;
|
||||
|
|
|
@ -859,7 +859,7 @@ private:
|
|||
void navigate();
|
||||
void calc_airspeed_errors();
|
||||
void calc_gndspeed_undershoot();
|
||||
void update_loiter();
|
||||
void update_loiter(uint16_t radius);
|
||||
void update_cruise();
|
||||
void update_fbwb_speed_height(void);
|
||||
void setup_turn_angle(void);
|
||||
|
|
|
@ -600,13 +600,18 @@ bool Plane::verify_nav_wp(const AP_Mission::Mission_Command& cmd)
|
|||
|
||||
bool Plane::verify_loiter_unlim()
|
||||
{
|
||||
update_loiter();
|
||||
if (g.rtl_radius < 0) {
|
||||
loiter.direction = -1;
|
||||
} else {
|
||||
loiter.direction = 1;
|
||||
}
|
||||
update_loiter(abs(g.rtl_radius));
|
||||
return false;
|
||||
}
|
||||
|
||||
bool Plane::verify_loiter_time()
|
||||
{
|
||||
update_loiter();
|
||||
update_loiter(0);
|
||||
if (loiter.start_time_ms == 0) {
|
||||
if (nav_controller->reached_loiter_target()) {
|
||||
// we've reached the target, start the timer
|
||||
|
@ -621,7 +626,7 @@ bool Plane::verify_loiter_time()
|
|||
|
||||
bool Plane::verify_loiter_turns()
|
||||
{
|
||||
update_loiter();
|
||||
update_loiter(0);
|
||||
if (loiter.sum_cd > loiter.total_cd) {
|
||||
loiter.total_cd = 0;
|
||||
gcs_send_text(MAV_SEVERITY_WARNING,"Verify nav: LOITER orbits complete");
|
||||
|
@ -638,7 +643,7 @@ bool Plane::verify_loiter_turns()
|
|||
*/
|
||||
bool Plane::verify_loiter_to_alt()
|
||||
{
|
||||
update_loiter();
|
||||
update_loiter(0);
|
||||
|
||||
//has target altitude been reached?
|
||||
if (condition_value != 0) {
|
||||
|
@ -705,7 +710,12 @@ bool Plane::verify_loiter_to_alt()
|
|||
|
||||
bool Plane::verify_RTL()
|
||||
{
|
||||
update_loiter();
|
||||
if (g.rtl_radius < 0) {
|
||||
loiter.direction = -1;
|
||||
} else {
|
||||
loiter.direction = 1;
|
||||
}
|
||||
update_loiter(abs(g.rtl_radius));
|
||||
if (auto_state.wp_distance <= (uint32_t)MAX(g.waypoint_radius,0) ||
|
||||
nav_controller->reached_loiter_target()) {
|
||||
gcs_send_text(MAV_SEVERITY_INFO,"Reached HOME");
|
||||
|
|
|
@ -127,9 +127,11 @@ void Plane::calc_gndspeed_undershoot()
|
|||
}
|
||||
}
|
||||
|
||||
void Plane::update_loiter()
|
||||
void Plane::update_loiter(uint16_t radius)
|
||||
{
|
||||
int16_t radius = abs(g.loiter_radius);
|
||||
if (radius == 0) {
|
||||
radius = abs(g.loiter_radius);
|
||||
}
|
||||
|
||||
if (loiter.start_time_ms == 0 &&
|
||||
control_mode == AUTO &&
|
||||
|
|
Loading…
Reference in New Issue