Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-02-23 16:23:56 -04:00
Code Issues Packages Projects Releases Wiki Activity

Plane: allow loiter waypoints to have a zero lat/lng or alt to mean use current

Browse Source
This commit is contained in:
Tom Pittenger 2016-03-02 08:44:09 -08:00
parent 278fb2e60d
commit bf5005103c
1 changed files with 17 additions and 19 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

36
ArduPlane/commands_logic.cpp
View File

@ -408,24 +408,32 @@ void Plane::loiter_set_direction_wp(const AP_Mission::Mission_Command& cmd)
void Plane::do_loiter_unlimited(const AP_Mission::Mission_Command& cmd) void Plane::do_loiter_unlimited(const AP_Mission::Mission_Command& cmd)
{ {
set_next_WP(cmd.content.location); Location cmdloc = cmd.content.location;
location_sanitize(current_loc, cmdloc);
set_next_WP(cmdloc);
loiter_set_direction_wp(cmd); loiter_set_direction_wp(cmd);
} }
void Plane::do_loiter_turns(const AP_Mission::Mission_Command& cmd) void Plane::do_loiter_turns(const AP_Mission::Mission_Command& cmd)
{ {
set_next_WP(cmd.content.location); Location cmdloc = cmd.content.location;
loiter.total_cd = (uint32_t)(LOWBYTE(cmd.p1)) * 36000UL; location_sanitize(current_loc, cmdloc);
set_next_WP(cmdloc);
loiter_set_direction_wp(cmd); loiter_set_direction_wp(cmd);
loiter.total_cd = (uint32_t)(LOWBYTE(cmd.p1)) * 36000UL;
condition_value = 1; // used to signify primary turns goal not yet met condition_value = 1; // used to signify primary turns goal not yet met
} }
void Plane::do_loiter_time(const AP_Mission::Mission_Command& cmd) void Plane::do_loiter_time(const AP_Mission::Mission_Command& cmd)
{ {
set_next_WP(cmd.content.location); Location cmdloc = cmd.content.location;
location_sanitize(current_loc, cmdloc);
set_next_WP(cmdloc);
loiter_set_direction_wp(cmd);
// we set start_time_ms when we reach the waypoint // we set start_time_ms when we reach the waypoint
loiter.time_max_ms = cmd.p1 * (uint32_t)1000; // convert sec to ms loiter.time_max_ms = cmd.p1 * (uint32_t)1000; // convert sec to ms
loiter_set_direction_wp(cmd);
condition_value = 1; // used to signify primary time goal not yet met condition_value = 1; // used to signify primary time goal not yet met
} }
@ -465,13 +473,10 @@ void Plane::do_altitude_wait(const AP_Mission::Mission_Command& cmd)
void Plane::do_loiter_to_alt(const AP_Mission::Mission_Command& cmd) void Plane::do_loiter_to_alt(const AP_Mission::Mission_Command& cmd)
{ {
//set target alt //set target alt
next_WP_loc.alt = cmd.content.location.alt; Location loc = cmd.content.location;
location_sanitize(current_loc, loc);
// convert relative alt to absolute alt set_next_WP(loc);
if (cmd.content.location.flags.relative_alt) { loiter_set_direction_wp(cmd);
next_WP_loc.flags.relative_alt = false;
next_WP_loc.alt += home.alt;
}
// used to signify primary turns goal not yet met when non-zero // used to signify primary turns goal not yet met when non-zero
condition_value = next_WP_loc.alt; condition_value = next_WP_loc.alt;
@ -479,13 +484,6 @@ void Plane::do_loiter_to_alt(const AP_Mission::Mission_Command& cmd)
// the value of 0 is used to signify it has been reached. Lets bump alt to 1 which is 10cm. Close enough! // the value of 0 is used to signify it has been reached. Lets bump alt to 1 which is 10cm. Close enough!
condition_value = 1; condition_value = 1;
} }
//are lat and lon 0? if so, don't change the current wp lat/lon
if (cmd.content.location.lat != 0 || cmd.content.location.lng != 0) {
set_next_WP(cmd.content.location);
}
//set loiter direction
loiter_set_direction_wp(cmd);
} }
/********************************************************************************/ /********************************************************************************/