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

Plane: loiter missions to default to loiter_radius if mission says 0 or 1.

Browse Source 
- except loiter_unlim which uses RTL_loiter, although I'm not sure why.
This commit is contained in:
Tom Pittenger 2016-02-29 10:11:31 -08:00
parent ed98617d42
commit 9e452838ab
3 changed files with 19 additions and 26 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

20
ArduPlane/ArduPlane.cpp
View File

@ -762,25 +762,15 @@ void Plane::update_navigation()
// on every loop
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);
if (radius > 0) {
loiter.direction = (g.rtl_radius < 0) ? -1 : 1;
}
// fall through to LOITER
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(abs(radius));
update_loiter(radius);
break;
case CRUISE:

19
ArduPlane/commands_logic.cpp
View File

@ -601,15 +601,12 @@ bool Plane::verify_nav_wp(const AP_Mission::Mission_Command& cmd)
bool Plane::verify_loiter_unlim()
{
if (g.rtl_radius < 0) {
loiter.direction = -1;
} else {
loiter.direction = 1;
}
if (mission.get_current_nav_cmd().p1 == 0) {
if (mission.get_current_nav_cmd().p1 <= 1 && abs(g.rtl_radius) > 1) {
// if mission radius is 0,1, and rtl_radius is valid, use rtl_radius.
loiter.direction = (g.rtl_radius < 0) ? -1 : 1;
update_loiter(abs(g.rtl_radius));
} else {
// else use mission radius
update_loiter(mission.get_current_nav_cmd().p1);
}
return false;
@ -617,6 +614,7 @@ bool Plane::verify_loiter_unlim()
bool Plane::verify_loiter_time()
{
// mission radius is always g.loiter_radius
update_loiter(0);
if (loiter.start_time_ms == 0) {
if (nav_controller->reached_loiter_target()) {
@ -632,7 +630,9 @@ bool Plane::verify_loiter_time()
bool Plane::verify_loiter_turns()
{
update_loiter(HIGHBYTE(mission.get_current_nav_cmd().p1));
uint16_t radius = HIGHBYTE(mission.get_current_nav_cmd().p1);
update_loiter(radius);
if (loiter.sum_cd > loiter.total_cd) {
loiter.total_cd = 0;
gcs_send_text(MAV_SEVERITY_WARNING,"Verify nav: LOITER orbits complete");
@ -649,7 +649,8 @@ bool Plane::verify_loiter_turns()
*/
bool Plane::verify_loiter_to_alt()
{
update_loiter(HIGHBYTE(mission.get_current_nav_cmd().p1));
uint16_t radius = HIGHBYTE(mission.get_current_nav_cmd().p1);
update_loiter(radius);
//has target altitude been reached?
if (condition_value != 0) {

6
ArduPlane/navigation.cpp
View File

@ -129,8 +129,10 @@ void Plane::calc_gndspeed_undershoot()
void Plane::update_loiter(uint16_t radius)
{
if (radius == 0) {
radius = abs(g.loiter_radius);
if (radius <= 1) {
// if radius is <=1 then use the general loiter radius. if it's small, use default
radius = (abs(g.loiter_radius <= 1)) ? LOITER_RADIUS_DEFAULT : abs(g.loiter_radius);
loiter.direction = (g.loiter_radius < 0) ? -1 : 1;
}
if (loiter.start_time_ms == 0 &&