diff --git a/libraries/AC_WPNav/AC_Circle.cpp b/libraries/AC_WPNav/AC_Circle.cpp index d583e9ae72..000d8dae58 100644 --- a/libraries/AC_WPNav/AC_Circle.cpp +++ b/libraries/AC_WPNav/AC_Circle.cpp @@ -114,7 +114,7 @@ void AC_Circle::set_center(const Location& center) set_center(Vector3f(center_xy.x, center_xy.y, terr_alt_cm), true); } else { // failed to convert location so set to current position and log error - set_center(_inav.get_position(), false); + set_center(_inav.get_position_neu_cm(), false); AP::logger().Write_Error(LogErrorSubsystem::NAVIGATION, LogErrorCode::FAILED_CIRCLE_INIT); } } else { @@ -122,7 +122,7 @@ void AC_Circle::set_center(const Location& center) Vector3f circle_center_neu; if (!center.get_vector_from_origin_NEU(circle_center_neu)) { // default to current position and log error - circle_center_neu = _inav.get_position(); + circle_center_neu = _inav.get_position_neu_cm(); AP::logger().Write_Error(LogErrorSubsystem::NAVIGATION, LogErrorCode::FAILED_CIRCLE_INIT); } set_center(circle_center_neu, false); @@ -200,7 +200,7 @@ bool AC_Circle::update(float climb_rate_cms) target.y += - _radius * sinf(-_angle); // heading is from vehicle to center of circle - _yaw = get_bearing_cd(_inav.get_position_xy(), _center.tofloat().xy()); + _yaw = get_bearing_cd(_inav.get_position_xy_cm(), _center.tofloat().xy()); if ((_options.get() & CircleOptions::FACE_DIRECTION_OF_TRAVEL) != 0) { _yaw += is_positive(_rate)?-9000.0f:9000.0f; @@ -313,7 +313,7 @@ void AC_Circle::init_start_angle(bool use_heading) _angle = wrap_PI(_ahrs.yaw-M_PI); } else { // if we are exactly at the center of the circle, init angle to directly behind vehicle (so vehicle will backup but not change heading) - const Vector3f &curr_pos = _inav.get_position(); + const Vector3f &curr_pos = _inav.get_position_neu_cm(); if (is_equal(curr_pos.x,float(_center.x)) && is_equal(curr_pos.y,float(_center.y))) { _angle = wrap_PI(_ahrs.yaw-M_PI); } else { @@ -352,7 +352,7 @@ bool AC_Circle::get_terrain_offset(float& offset_cm) return false; case AC_Circle::TerrainSource::TERRAIN_FROM_RANGEFINDER: if (_rangefinder_healthy) { - offset_cm = _inav.get_altitude() - _rangefinder_alt_cm; + offset_cm = _inav.get_position_z_up_cm() - _rangefinder_alt_cm; return true; } return false; @@ -361,7 +361,7 @@ bool AC_Circle::get_terrain_offset(float& offset_cm) float terr_alt = 0.0f; AP_Terrain *terrain = AP_Terrain::get_singleton(); if (terrain != nullptr && terrain->height_above_terrain(terr_alt, true)) { - offset_cm = _inav.get_altitude() - (terr_alt * 100.0f); + offset_cm = _inav.get_position_z_up_cm() - (terr_alt * 100.0); return true; } #endif diff --git a/libraries/AC_WPNav/AC_Loiter.cpp b/libraries/AC_WPNav/AC_Loiter.cpp index 7b14119375..396935e47f 100644 --- a/libraries/AC_WPNav/AC_Loiter.cpp +++ b/libraries/AC_WPNav/AC_Loiter.cpp @@ -131,7 +131,7 @@ void AC_Loiter::init_target() /// reduce response for landing void AC_Loiter::soften_for_landing() { - const Vector3f& curr_pos = _inav.get_position(); + const Vector3f& curr_pos = _inav.get_position_neu_cm(); // set target position to current position _pos_control.set_pos_target_xy_cm(curr_pos.x, curr_pos.y); diff --git a/libraries/AC_WPNav/AC_WPNav.cpp b/libraries/AC_WPNav/AC_WPNav.cpp index 017b275a15..00a3230284 100644 --- a/libraries/AC_WPNav/AC_WPNav.cpp +++ b/libraries/AC_WPNav/AC_WPNav.cpp @@ -399,7 +399,7 @@ void AC_WPNav::shift_wp_origin_and_destination_to_current_pos_xy() _pos_control.init_xy_controller(); // get current and target locations - const Vector2f& curr_pos = _inav.get_position_xy(); + const Vector2f& curr_pos = _inav.get_position_xy_cm(); // shift origin and destination horizontally _origin.xy() = curr_pos; @@ -458,7 +458,7 @@ bool AC_WPNav::advance_wp_target_along_track(float dt) _pos_control.update_pos_offset_z(terr_offset); // get current position and adjust altitude to origin and destination's frame (i.e. _frame) - const Vector3f &curr_pos = _inav.get_position() - Vector3f{0, 0, terr_offset}; + const Vector3f &curr_pos = _inav.get_position_neu_cm() - Vector3f{0, 0, terr_offset}; // Use _track_scalar_dt to slow down S-Curve time to prevent target moving too far in front of aircraft Vector3f curr_target_vel = _pos_control.get_vel_desired_cms(); @@ -469,7 +469,7 @@ bool AC_WPNav::advance_wp_target_along_track(float dt) if (is_positive(curr_target_vel.length())) { Vector3f track_direction = curr_target_vel.normalized(); const float track_error = _pos_control.get_pos_error_cm().dot(track_direction); - const float track_velocity = _inav.get_velocity().dot(track_direction); + const float track_velocity = _inav.get_velocity_neu_cms().dot(track_direction); // set time scaler to be consistent with the achievable aircraft speed with a 5% buffer for short term variation. track_scaler_dt = constrain_float(0.05f + (track_velocity - _pos_control.get_pos_xy_p().kP() * track_error) / curr_target_vel.length(), 0.1f, 1.0f); } @@ -559,13 +559,13 @@ void AC_WPNav::update_track_with_speed_accel_limits() /// get_wp_distance_to_destination - get horizontal distance to destination in cm float AC_WPNav::get_wp_distance_to_destination() const { - return get_horizontal_distance_cm(_inav.get_position_xy(), _destination.xy()); + return get_horizontal_distance_cm(_inav.get_position_xy_cm(), _destination.xy()); } /// get_wp_bearing_to_destination - get bearing to next waypoint in centi-degrees int32_t AC_WPNav::get_wp_bearing_to_destination() const { - return get_bearing_cd(_inav.get_position_xy(), _destination.xy()); + return get_bearing_cd(_inav.get_position_xy_cm(), _destination.xy()); } /// update_wpnav - run the wp controller - should be called at 100hz or higher @@ -614,7 +614,7 @@ bool AC_WPNav::get_terrain_offset(float& offset_cm) return false; case AC_WPNav::TerrainSource::TERRAIN_FROM_RANGEFINDER: if (_rangefinder_healthy) { - offset_cm = _inav.get_altitude() - _rangefinder_alt_cm; + offset_cm = _inav.get_position_z_up_cm() - _rangefinder_alt_cm; return true; } return false; @@ -624,7 +624,7 @@ bool AC_WPNav::get_terrain_offset(float& offset_cm) AP_Terrain *terrain = AP::terrain(); if (terrain != nullptr && terrain->height_above_terrain(terr_alt, true)) { - offset_cm = _inav.get_altitude() - (terr_alt * 100.0f); + offset_cm = _inav.get_position_z_up_cm() - (terr_alt * 100.0); return true; } #endif diff --git a/libraries/AC_WPNav/AC_WPNav_OA.cpp b/libraries/AC_WPNav/AC_WPNav_OA.cpp index d577f228b1..07dfb02796 100644 --- a/libraries/AC_WPNav/AC_WPNav_OA.cpp +++ b/libraries/AC_WPNav/AC_WPNav_OA.cpp @@ -44,7 +44,7 @@ float AC_WPNav_OA::get_wp_distance_to_destination() const return AC_WPNav::get_wp_distance_to_destination(); } - return get_horizontal_distance_cm(_inav.get_position_xy(), _destination_oabak.xy()); + return get_horizontal_distance_cm(_inav.get_position_xy_cm(), _destination_oabak.xy()); } /// get_wp_bearing_to_destination - get bearing to next waypoint in centi-degrees @@ -55,7 +55,7 @@ int32_t AC_WPNav_OA::get_wp_bearing_to_destination() const return AC_WPNav::get_wp_bearing_to_destination(); } - return get_bearing_cd(_inav.get_position_xy(), _destination_oabak.xy()); + return get_bearing_cd(_inav.get_position_xy_cm(), _destination_oabak.xy()); } /// true when we have come within RADIUS cm of the waypoint