|
|
@ -3633,26 +3633,17 @@ MAV_RESULT GCS_MAVLINK::handle_command_int_do_set_home(const mavlink_command_int |
|
|
|
if (!is_zero(packet.param1)) { |
|
|
|
if (!is_zero(packet.param1)) { |
|
|
|
return MAV_RESULT_FAILED; |
|
|
|
return MAV_RESULT_FAILED; |
|
|
|
} |
|
|
|
} |
|
|
|
// check frame type is supported
|
|
|
|
Location::ALT_FRAME frame; |
|
|
|
if (packet.frame != MAV_FRAME_GLOBAL && |
|
|
|
if (!mavlink_coordinate_frame_to_location_alt_frame(packet.frame, frame)) { |
|
|
|
packet.frame != MAV_FRAME_GLOBAL_INT && |
|
|
|
// unknown coordinate frame
|
|
|
|
packet.frame != MAV_FRAME_GLOBAL_RELATIVE_ALT && |
|
|
|
|
|
|
|
packet.frame != MAV_FRAME_GLOBAL_RELATIVE_ALT_INT) { |
|
|
|
|
|
|
|
return MAV_RESULT_UNSUPPORTED; |
|
|
|
return MAV_RESULT_UNSUPPORTED; |
|
|
|
} |
|
|
|
} |
|
|
|
Location new_home_loc {}; |
|
|
|
const Location new_home_loc{ |
|
|
|
new_home_loc.lat = packet.x; |
|
|
|
packet.x, |
|
|
|
new_home_loc.lng = packet.y; |
|
|
|
packet.y, |
|
|
|
new_home_loc.alt = packet.z * 100; |
|
|
|
int32_t(packet.z * 100), |
|
|
|
// handle relative altitude
|
|
|
|
frame, |
|
|
|
if (packet.frame == MAV_FRAME_GLOBAL_RELATIVE_ALT || |
|
|
|
}; |
|
|
|
packet.frame == MAV_FRAME_GLOBAL_RELATIVE_ALT_INT) { |
|
|
|
|
|
|
|
if (!AP::ahrs().home_is_set()) { |
|
|
|
|
|
|
|
// cannot use relative altitude if home is not set
|
|
|
|
|
|
|
|
return MAV_RESULT_FAILED; |
|
|
|
|
|
|
|
} |
|
|
|
|
|
|
|
new_home_loc.alt += AP::ahrs().get_home().alt; |
|
|
|
|
|
|
|
} |
|
|
|
|
|
|
|
if (!set_home(new_home_loc, true)) { |
|
|
|
if (!set_home(new_home_loc, true)) { |
|
|
|
return MAV_RESULT_FAILED; |
|
|
|
return MAV_RESULT_FAILED; |
|
|
|
} |
|
|
|
} |
|
|
|