diff --git a/ArduCopter/mode_auto.cpp b/ArduCopter/mode_auto.cpp index 2defaefd59..a939a1da24 100644 --- a/ArduCopter/mode_auto.cpp +++ b/ArduCopter/mode_auto.cpp @@ -287,7 +287,7 @@ void ModeAuto::circle_movetoedge_start(const Location &circle_center, float radi _mode = Auto_CircleMoveToEdge; // convert circle_edge_neu to Location - Location circle_edge(circle_edge_neu); + Location circle_edge(circle_edge_neu, Location::AltFrame::ABOVE_ORIGIN); // convert altitude to same as command circle_edge.set_alt_cm(circle_center.alt, circle_center.get_alt_frame()); @@ -1199,7 +1199,7 @@ void ModeAuto::do_loiter_unlimited(const AP_Mission::Mission_Command& cmd) // To-Do: make this simpler Vector3f temp_pos; copter.wp_nav->get_wp_stopping_point_xy(temp_pos); - const Location temp_loc(temp_pos); + const Location temp_loc(temp_pos, Location::AltFrame::ABOVE_ORIGIN); target_loc.lat = temp_loc.lat; target_loc.lng = temp_loc.lng; } @@ -1707,7 +1707,7 @@ bool ModeAuto::verify_payload_place() } FALLTHROUGH; case PayloadPlaceStateType_Ascending_Start: { - Location target_loc = inertial_nav.get_position(); + Location target_loc(inertial_nav.get_position(), Location::AltFrame::ABOVE_ORIGIN); target_loc.alt = nav_payload_place.descend_start_altitude; wp_start(target_loc); nav_payload_place.state = PayloadPlaceStateType_Ascending; diff --git a/ArduCopter/mode_guided.cpp b/ArduCopter/mode_guided.cpp index aca72cf9fd..80ec54d46e 100644 --- a/ArduCopter/mode_guided.cpp +++ b/ArduCopter/mode_guided.cpp @@ -241,7 +241,7 @@ bool ModeGuided::set_destination(const Vector3f& destination, bool use_yaw, floa { #if AC_FENCE == ENABLED // reject destination if outside the fence - const Location dest_loc(destination); + const Location dest_loc(destination, terrain_alt ? Location::AltFrame::ABOVE_TERRAIN : Location::AltFrame::ABOVE_ORIGIN); if (!copter.fence.check_destination_within_fence(dest_loc)) { AP::logger().Write_Error(LogErrorSubsystem::NAVIGATION, LogErrorCode::DEST_OUTSIDE_FENCE); // failure is propagated to GCS with NAK @@ -340,7 +340,7 @@ bool ModeGuided::set_destination_posvel(const Vector3f& destination, const Vecto { #if AC_FENCE == ENABLED // reject destination if outside the fence - const Location dest_loc(destination); + const Location dest_loc(destination, Location::AltFrame::ABOVE_ORIGIN); if (!copter.fence.check_destination_within_fence(dest_loc)) { AP::logger().Write_Error(LogErrorSubsystem::NAVIGATION, LogErrorCode::DEST_OUTSIDE_FENCE); // failure is propagated to GCS with NAK diff --git a/ArduCopter/mode_rtl.cpp b/ArduCopter/mode_rtl.cpp index b929c2e577..8ae13e6920 100644 --- a/ArduCopter/mode_rtl.cpp +++ b/ArduCopter/mode_rtl.cpp @@ -419,7 +419,7 @@ void ModeRTL::build_path() Vector3f stopping_point; pos_control->get_stopping_point_xy(stopping_point); pos_control->get_stopping_point_z(stopping_point); - rtl_path.origin_point = Location(stopping_point); + rtl_path.origin_point = Location(stopping_point, Location::AltFrame::ABOVE_ORIGIN); rtl_path.origin_point.change_alt_frame(Location::AltFrame::ABOVE_HOME); // compute return target