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

Copter: support set-home-position message

Browse Source
This commit is contained in:
Randy Mackay 2015-10-03 10:37:02 +09:00
parent 330961b524
commit 1dcf58bc20
1 changed files with 23 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

23
ArduCopter/GCS_Mavlink.cpp
View File

@ -1866,6 +1866,29 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
AP_Notify::handle_led_control(msg); AP_Notify::handle_led_control(msg);
break; break;
case MAVLINK_MSG_ID_SET_HOME_POSITION:
{
mavlink_set_home_position_t packet;
mavlink_msg_set_home_position_decode(msg, &packet);
if((packet.latitude == 0) && (packet.longitude == 0) && (packet.altitude == 0)) {
copter.set_home_to_current_location_and_lock();
} else {
// sanity check location
if (labs(packet.latitude) > 90*10e7 || labs(packet.longitude) > 180 * 10e7) {
break;
}
Location new_home_loc;
new_home_loc.lat = packet.latitude;
new_home_loc.lng = packet.longitude;
new_home_loc.alt = packet.altitude * 100;
if (copter.far_from_EKF_origin(new_home_loc)) {
break;
}
copter.set_home_and_lock(new_home_loc);
}
break;
}
} // end switch } // end switch
} // end handle mavlink } // end handle mavlink