|
|
|
@ -973,11 +973,15 @@ void GCS_MAVLINK_Copter::handleMessage(mavlink_message_t* msg)
@@ -973,11 +973,15 @@ void GCS_MAVLINK_Copter::handleMessage(mavlink_message_t* msg)
|
|
|
|
|
// param6 : longitude
|
|
|
|
|
// param7 : altitude (absolute)
|
|
|
|
|
result = MAV_RESULT_FAILED; // assume failure
|
|
|
|
|
if(is_equal(packet.param1,1.0f) || (is_zero(packet.param5) && is_zero(packet.param6) && is_zero(packet.param7))) { |
|
|
|
|
if (is_equal(packet.param1,1.0f)) { |
|
|
|
|
if (copter.set_home_to_current_location(true)) { |
|
|
|
|
result = MAV_RESULT_ACCEPTED; |
|
|
|
|
} |
|
|
|
|
} else { |
|
|
|
|
// ensure param1 is zero
|
|
|
|
|
if (!is_zero(packet.param1)) { |
|
|
|
|
break; |
|
|
|
|
} |
|
|
|
|
// sanity check location
|
|
|
|
|
if (!check_latlng(packet.param5, packet.param6)) { |
|
|
|
|
break; |
|
|
|
|