Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

Copter: default WAYPOINT to current pos if lat, lon, alt are zero

This commit is contained in:
Randy Mackay 2014-04-16 20:32:49 +09:00
parent d2dad1b2c1
commit 7f00bd7f5d
1 changed files with 12 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

12
ArduCopter/commands_logic.pde
View File

@ -291,8 +291,20 @@ static void do_takeoff(const AP_Mission::Mission_Command& cmd)
// do_nav_wp - initiate move to next waypoint
static void do_nav_wp(const AP_Mission::Mission_Command& cmd)
{
const Vector3f &curr_pos = inertial_nav.get_position();
Vector3f local_pos = pv_location_to_vector(cmd.content.location);
// set target altitude to current altitude if not provided
if (cmd.content.location.alt == 0) {
local_pos.z = curr_pos.z;
}
// set lat/lon position to current position if not provided
if (cmd.content.location.lat == 0 && cmd.content.location.lng == 0) {
local_pos.x = curr_pos.x;
local_pos.y = curr_pos.y;
}
// this will be used to remember the time in millis after we reach or pass the WP.
loiter_time = 0;
// this is the delay, stored in seconds