Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-03 14:38:30 -04:00
Code Issues Packages Projects Releases Wiki Activity

Copter: point nose at landing point if specified

Browse Source
This commit is contained in:
Randy Mackay 2013-06-03 15:20:37 +09:00
parent 9f59f2724f
commit 993bccc60e
2 changed files with 9 additions and 6 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

13
ArduCopter/commands_logic.pde
View File

@ -17,7 +17,7 @@ static void process_nav_command()
break; break;
case MAV_CMD_NAV_LAND: // 21 LAND to Waypoint case MAV_CMD_NAV_LAND: // 21 LAND to Waypoint
do_land(); do_land(&command_nav_queue);
break; break;
case MAV_CMD_NAV_LOITER_UNLIM: // 17 Loiter indefinitely case MAV_CMD_NAV_LOITER_UNLIM: // 17 Loiter indefinitely
@ -290,13 +290,12 @@ static void do_nav_wp()
// do_land - initiate landing procedure // do_land - initiate landing procedure
// caller should set roll_pitch_mode to ROLL_PITCH_AUTO (for no pilot input) or ROLL_PITCH_LOITER (for pilot input) // caller should set roll_pitch_mode to ROLL_PITCH_AUTO (for no pilot input) or ROLL_PITCH_LOITER (for pilot input)
// caller should set yaw_mode static void do_land(const struct Location *cmd)
static void do_land()
{ {
// To-Do: check if we have already landed // To-Do: check if we have already landed
// if location provided we fly to that location at current altitude // if location provided we fly to that location at current altitude
if (command_nav_queue.lat != 0 || command_nav_queue.lng != 0) { if (cmd != NULL && (cmd->lat != 0 || cmd->lng != 0)) {
// set state to fly to location // set state to fly to location
land_state = LAND_STATE_FLY_TO_LOCATION; land_state = LAND_STATE_FLY_TO_LOCATION;
@ -313,9 +312,13 @@ static void do_land()
set_nav_mode(NAV_WP); set_nav_mode(NAV_WP);
// calculate and set desired location above landing target // calculate and set desired location above landing target
Vector3f pos = pv_location_to_vector(command_nav_queue); Vector3f pos = pv_location_to_vector(*cmd);
pos.z = min(current_loc.alt, RTL_ALT_MAX); pos.z = min(current_loc.alt, RTL_ALT_MAX);
wp_nav.set_destination(pos); wp_nav.set_destination(pos);
// initialise original_wp_bearing which is used to check if we have missed the waypoint
wp_bearing = wp_nav.get_bearing_to_destination();
original_wp_bearing = wp_bearing;
}else{ }else{
// set landing state // set landing state
land_state = LAND_STATE_DESCENDING; land_state = LAND_STATE_DESCENDING;

2
ArduCopter/system.pde
View File

@ -442,7 +442,7 @@ static void set_mode(uint8_t mode)
ap.manual_attitude = true; ap.manual_attitude = true;
} }
ap.manual_throttle = false; ap.manual_throttle = false;
do_land(); do_land(NULL); // land at current location
break; break;
case RTL: case RTL: