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

GCS_MAVLink: move command-long DO_SET_HOME up

Browse Source
This commit is contained in:
Peter Barker 2019-03-05 22:55:53 +11:00 committed by Peter Barker
parent 47f55b5fed
commit 95745f0d31
2 changed files with 32 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
libraries/GCS_MAVLink/GCS.h
View File

@ -337,6 +337,8 @@ protected:
virtual bool set_home_to_current_location(bool lock) = 0;
virtual bool set_home(const Location& loc, bool lock) = 0;
MAV_RESULT handle_command_do_set_home(const mavlink_command_long_t &packet);
void handle_mission_request_list(AP_Mission &mission, mavlink_message_t *msg);
void handle_mission_request(AP_Mission &mission, mavlink_message_t *msg);
void handle_mission_clear_all(AP_Mission &mission, mavlink_message_t *msg);

30
libraries/GCS_MAVLink/GCS_Common.cpp
View File

@ -3521,6 +3521,32 @@ MAV_RESULT GCS_MAVLINK::handle_command_mount(const mavlink_command_long_t &packe
return mount->handle_command_long(packet);
}
MAV_RESULT GCS_MAVLINK::handle_command_do_set_home(const mavlink_command_long_t &packet)
{
// if param1 is 1 use current location:
if (is_equal(packet.param1, 1.0f)) {
if (!set_home_to_current_location(true)) {
return MAV_RESULT_FAILED;
}
return MAV_RESULT_ACCEPTED;
}
// ensure param1 is zero
if (!is_zero(packet.param1)) {
return MAV_RESULT_FAILED;
}
Location new_home_loc;
new_home_loc.lat = (int32_t)(packet.param5 * 1.0e7f);
new_home_loc.lng = (int32_t)(packet.param6 * 1.0e7f);
new_home_loc.alt = (int32_t)(packet.param7 * 100.0f);
if (!set_home(new_home_loc, true)) {
return MAV_RESULT_FAILED;
}
return MAV_RESULT_ACCEPTED;
}
MAV_RESULT GCS_MAVLINK::handle_command_long_packet(const mavlink_command_long_t &packet)
{
MAV_RESULT result = MAV_RESULT_FAILED;
@ -3539,6 +3565,10 @@ MAV_RESULT GCS_MAVLINK::handle_command_long_packet(const mavlink_command_long_t
result = handle_command_do_send_banner(packet);
break;
case MAV_CMD_DO_SET_HOME:
result = handle_command_do_set_home(packet);
break;
case MAV_CMD_DO_FENCE_ENABLE:
result = handle_command_do_fence_enable(packet);
break;