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

Copter: do-loiter accepts terrain altitudes

Browse Source
This commit is contained in:
Randy Mackay 2016-03-09 19:26:18 +09:00
parent 27517004c0
commit 4892446c55
1 changed files with 19 additions and 32 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

51
ArduCopter/commands_logic.cpp
View File

@ -342,27 +342,32 @@ void Copter::do_land(const AP_Mission::Mission_Command& cmd)
// note: caller should set yaw_mode // note: caller should set yaw_mode
void Copter::do_loiter_unlimited(const AP_Mission::Mission_Command& cmd) void Copter::do_loiter_unlimited(const AP_Mission::Mission_Command& cmd)
{ {
Vector3f target_pos; // convert back to location
Location_Class target_loc(cmd.content.location);
// get current position
Vector3f curr_pos = inertial_nav.get_position();
// default to use position provided
target_pos = pv_location_to_vector(cmd.content.location);
// use current location if not provided // use current location if not provided
if(cmd.content.location.lat == 0 && cmd.content.location.lng == 0) { if (target_loc.lat == 0 && target_loc.lng == 0) {
wp_nav.get_wp_stopping_point_xy(target_pos); // To-Do: make this simpler
Vector3f temp_pos;
wp_nav.get_wp_stopping_point_xy(temp_pos);
target_loc.offset(temp_pos.x * 100.0f, temp_pos.y * 100.0f);
} }
// use current altitude if not provided // use current altitude if not provided
// To-Do: use z-axis stopping point instead of current alt // To-Do: use z-axis stopping point instead of current alt
if( cmd.content.location.alt == 0 ) { if (target_loc.alt == 0) {
target_pos.z = curr_pos.z; // set to current altitude but in command's alt frame
int32_t curr_alt;
if (current_loc.get_alt_cm(target_loc.get_alt_frame(),curr_alt)) {
target_loc.set_alt(curr_alt, target_loc.get_alt_frame());
} else {
// default to current altitude as alt-above-home
target_loc.set_alt(current_loc.alt, current_loc.get_alt_frame());
}
} }
// start way point navigator and provide it the desired location // start way point navigator and provide it the desired location
auto_wp_start(target_pos); auto_wp_start(target_loc);
} }
// do_circle - initiate moving in a circle // do_circle - initiate moving in a circle
@ -411,26 +416,8 @@ void Copter::do_circle(const AP_Mission::Mission_Command& cmd)
// note: caller should set yaw_mode // note: caller should set yaw_mode
void Copter::do_loiter_time(const AP_Mission::Mission_Command& cmd) void Copter::do_loiter_time(const AP_Mission::Mission_Command& cmd)
{ {
Vector3f target_pos; // re-use loiter unlimited
do_loiter_unlimited(cmd);
// get current position
Vector3f curr_pos = inertial_nav.get_position();
// default to use position provided
target_pos = pv_location_to_vector(cmd.content.location);
// use current location if not provided
if(cmd.content.location.lat == 0 && cmd.content.location.lng == 0) {
wp_nav.get_wp_stopping_point_xy(target_pos);
}
// use current altitude if not provided
if( cmd.content.location.alt == 0 ) {
target_pos.z = curr_pos.z;
}
// start way point navigator and provide it the desired location
auto_wp_start(target_pos);
// setup loiter timer // setup loiter timer
loiter_time = 0; loiter_time = 0;