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

Copter: bug fix for do_loiter_time alt target

Browse Source
This commit is contained in:
Randy Mackay 2014-01-28 13:58:20 +09:00 committed by Andrew Tridgell
parent 37bde3406d
commit 212ced63f6
1 changed files with 8 additions and 6 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

14
ArduCopter/commands_logic.pde
View File

@ -295,15 +295,16 @@ static void do_loiter_unlimited()
// get current position // get current position
Vector3f curr_pos = inertial_nav.get_position(); Vector3f curr_pos = inertial_nav.get_position();
// default to use position provided
target_pos = pv_location_to_vector(command_nav_queue);
// use current location if not provided // use current location if not provided
if(command_nav_queue.lat == 0 && command_nav_queue.lng == 0) { if(command_nav_queue.lat == 0 && command_nav_queue.lng == 0) {
wp_nav.get_wp_stopping_point_xy(target_pos); wp_nav.get_wp_stopping_point_xy(target_pos);
}else{
// default to use position provided
target_pos = pv_location_to_vector(command_nav_queue);
} }
// use current altitude if not provided // use current altitude if not provided
// To-Do: use z-axis stopping point instead of current alt
if( command_nav_queue.alt == 0 ) { if( command_nav_queue.alt == 0 ) {
target_pos.z = curr_pos.z; target_pos.z = curr_pos.z;
} }
@ -324,6 +325,7 @@ static void do_circle()
} }
// set lat/lon position if not provided // set lat/lon position if not provided
// To-Do: use stopping point instead of current location
if (command_nav_queue.lat == 0 && command_nav_queue.lng == 0) { if (command_nav_queue.lat == 0 && command_nav_queue.lng == 0) {
circle_center.x = curr_pos.x; circle_center.x = curr_pos.x;
circle_center.y = curr_pos.y; circle_center.y = curr_pos.y;
@ -345,12 +347,12 @@ static void do_loiter_time()
// get current position // get current position
Vector3f curr_pos = inertial_nav.get_position(); Vector3f curr_pos = inertial_nav.get_position();
// default to use position provided
target_pos = pv_location_to_vector(command_nav_queue);
// use current location if not provided // use current location if not provided
if(command_nav_queue.lat == 0 && command_nav_queue.lng == 0) { if(command_nav_queue.lat == 0 && command_nav_queue.lng == 0) {
wp_nav.get_wp_stopping_point_xy(target_pos); wp_nav.get_wp_stopping_point_xy(target_pos);
}else{
// default to use position provided
target_pos = pv_location_to_vector(command_nav_queue);
} }
// use current altitude if not provided // use current altitude if not provided