Copter: INAV rename for neu & cm/cms

ArduCopter/AP_Arming.cpp

@@ -81,7 +81,7 @@ bool AP_Arming_Copter::barometer_checks(bool display_failure)
         nav_filter_status filt_status = copter.inertial_nav.get_filter_status();
         bool using_baro_ref = (!filt_status.flags.pred_horiz_pos_rel && filt_status.flags.pred_horiz_pos_abs);
         if (using_baro_ref) {
-            if (fabsf(copter.inertial_nav.get_altitude() - copter.baro_alt) > PREARM_MAX_ALT_DISPARITY_CM) {
+            if (fabsf(copter.inertial_nav.get_position_z_up_cm() - copter.baro_alt) > PREARM_MAX_ALT_DISPARITY_CM) {
                 check_failed(ARMING_CHECK_BARO, display_failure, "Altitude disparity");
                 ret = false;
             }
@@ -816,7 +816,7 @@ bool AP_Arming_Copter::arm(const AP_Arming::Method method, const bool do_arming_
         }
 
         // remember the height when we armed
-        copter.arming_altitude_m = copter.inertial_nav.get_altitude() * 0.01;
+        copter.arming_altitude_m = copter.inertial_nav.get_position_z_up_cm() * 0.01;
     }
     copter.update_super_simple_bearing(false);
 

ArduCopter/Attitude.cpp

@@ -27,7 +27,7 @@ void Copter::update_throttle_hover()
     float throttle = motors->get_throttle();
 
     // calc average throttle if we are in a level hover.  accounts for heli hover roll trim
-    if (throttle > 0.0f && fabsf(inertial_nav.get_velocity_z()) < 60 &&
+    if (throttle > 0.0f && fabsf(inertial_nav.get_velocity_z_up_cms()) < 60 &&
         labs(ahrs.roll_sensor-attitude_control->get_roll_trim_cd()) < 500 && labs(ahrs.pitch_sensor) < 500) {
         // Can we set the time constant automatically
         motors->update_throttle_hover(0.01f);

ArduCopter/GCS_Mavlink.cpp

@@ -1179,7 +1179,7 @@ void GCS_MAVLINK_Copter::handleMessage(const mavlink_message_t &msg)
             if (packet.coordinate_frame == MAV_FRAME_LOCAL_OFFSET_NED ||
                 packet.coordinate_frame == MAV_FRAME_BODY_NED ||
                 packet.coordinate_frame == MAV_FRAME_BODY_OFFSET_NED) {
-                pos_vector += copter.inertial_nav.get_position();
+                pos_vector += copter.inertial_nav.get_position_neu_cm();
             }
         }
 

ArduCopter/Log.cpp

@@ -53,13 +53,13 @@ void Copter::Log_Write_Control_Tuning()
         throttle_out        : motors->get_throttle(),
         throttle_hover      : motors->get_throttle_hover(),
         desired_alt         : des_alt_m,
-        inav_alt            : inertial_nav.get_altitude() / 100.0f,
+        inav_alt            : inertial_nav.get_position_z_up_cm() * 0.01f,
         baro_alt            : baro_alt,
         desired_rangefinder_alt : desired_rangefinder_alt,
         rangefinder_alt     : surface_tracking.get_dist_for_logging(),
         terr_alt            : terr_alt,
         target_climb_rate   : target_climb_rate_cms,
-        climb_rate          : int16_t(inertial_nav.get_velocity_z()) // float -> int16_t
+        climb_rate          : int16_t(inertial_nav.get_velocity_z_up_cms()) // float -> int16_t
     };
     logger.WriteBlock(&pkt, sizeof(pkt));
 }

ArduCopter/autoyaw.cpp

@@ -5,15 +5,15 @@ Mode::AutoYaw Mode::auto_yaw;
 // roi_yaw - returns heading towards location held in roi
 float Mode::AutoYaw::roi_yaw() const
 {
-    return get_bearing_cd(copter.inertial_nav.get_position_xy(), roi.xy());
+    return get_bearing_cd(copter.inertial_nav.get_position_xy_cm(), roi.xy());
 }
 
 float Mode::AutoYaw::look_ahead_yaw()
 {
-    const Vector3f& vel = copter.inertial_nav.get_velocity();
+    const Vector3f& vel = copter.inertial_nav.get_velocity_neu_cms();
-    float speed = vel.xy().length();
+    const float speed_sq = vel.xy().length_squared();
     // Commanded Yaw to automatically look ahead.
-    if (copter.position_ok() && (speed > YAW_LOOK_AHEAD_MIN_SPEED)) {
+    if (copter.position_ok() && (speed_sq > (YAW_LOOK_AHEAD_MIN_SPEED * YAW_LOOK_AHEAD_MIN_SPEED))) {
         _look_ahead_yaw = degrees(atan2f(vel.y,vel.x))*100.0f;
     }
     return _look_ahead_yaw;

ArduCopter/baro_ground_effect.cpp

@@ -24,7 +24,7 @@ void Copter::update_ground_effect_detector(void)
     }
 
     if (position_ok() || ekf_has_relative_position()) {
-        Vector3f vel = inertial_nav.get_velocity();
+        Vector3f vel = inertial_nav.get_velocity_neu_cms();
         vel.z = 0.0f;
         xy_speed_cms = vel.length();
     }
@@ -43,12 +43,12 @@ void Copter::update_ground_effect_detector(void)
     const bool throttle_up = flightmode->has_manual_throttle() && channel_throttle->get_control_in() > 0;
     if (!throttle_up && ap.land_complete) {
         gndeffect_state.takeoff_time_ms = tnow_ms;
-        gndeffect_state.takeoff_alt_cm = inertial_nav.get_altitude();
+        gndeffect_state.takeoff_alt_cm = inertial_nav.get_position_z_up_cm();
     }
 
     // if we are in takeoff_expected and we meet the conditions for having taken off
     // end the takeoff_expected state
-    if (gndeffect_state.takeoff_expected && (tnow_ms-gndeffect_state.takeoff_time_ms > 5000 || inertial_nav.get_altitude()-gndeffect_state.takeoff_alt_cm > 50.0f)) {
+    if (gndeffect_state.takeoff_expected && (tnow_ms-gndeffect_state.takeoff_time_ms > 5000 || inertial_nav.get_position_z_up_cm()-gndeffect_state.takeoff_alt_cm > 50.0f)) {
         gndeffect_state.takeoff_expected = false;
     }
 
@@ -61,7 +61,7 @@ void Copter::update_ground_effect_detector(void)
 
     bool descent_demanded = pos_control->is_active_z() && des_climb_rate_cms < 0.0f;
     bool slow_descent_demanded = descent_demanded && des_climb_rate_cms >= -100.0f;
-    bool z_speed_low = fabsf(inertial_nav.get_velocity_z()) <= 60.0f;
+    bool z_speed_low = fabsf(inertial_nav.get_velocity_z_up_cms()) <= 60.0f;
     bool slow_descent = (slow_descent_demanded || (z_speed_low && descent_demanded));
 
     gndeffect_state.touchdown_expected = slow_horizontal && slow_descent;

ArduCopter/crash_check.cpp

@@ -137,7 +137,7 @@ void Copter::thrust_loss_check()
     }
 
     // check for descent
-    if (!is_negative(inertial_nav.get_velocity_z())) {
+    if (!is_negative(inertial_nav.get_velocity_z_up_cms())) {
         thrust_loss_counter = 0;
         return;
     }
@@ -240,7 +240,7 @@ void Copter::parachute_check()
     parachute.set_is_flying(!ap.land_complete);
 
     // pass sink rate to parachute library
-    parachute.set_sink_rate(-inertial_nav.get_velocity_z() * 0.01f);
+    parachute.set_sink_rate(-inertial_nav.get_velocity_z_up_cms() * 0.01f);
 
     // exit immediately if in standby
     if (standby_active) {

ArduCopter/heli.cpp

@@ -35,7 +35,7 @@ void Copter::check_dynamic_flight(void)
     // with GPS lock use inertial nav to determine if we are moving
     if (position_ok()) {
         // get horizontal speed
-        const float speed = inertial_nav.get_speed_xy();
+        const float speed = inertial_nav.get_speed_xy_cms();
         moving = (speed >= HELI_DYNAMIC_FLIGHT_SPEED_MIN);
     }else{
         // with no GPS lock base it on throttle and forward lean angle

ArduCopter/inertia.cpp

@@ -18,7 +18,7 @@ void Copter::read_inertia()
     }
 
     // current_loc.alt is alt-above-home, converted from inertial nav's alt-above-ekf-origin
-    const int32_t alt_above_origin_cm = inertial_nav.get_altitude();
+    const int32_t alt_above_origin_cm = inertial_nav.get_position_z_up_cm();
     current_loc.set_alt_cm(alt_above_origin_cm, Location::AltFrame::ABOVE_ORIGIN);
     if (!ahrs.home_is_set() || !current_loc.change_alt_frame(Location::AltFrame::ABOVE_HOME)) {
         // if home has not been set yet we treat alt-above-origin as alt-above-home

ArduCopter/land_detector.cpp

@@ -84,7 +84,7 @@ void Copter::update_land_detector()
         bool accel_stationary = (land_accel_ef_filter.get().length() <= LAND_DETECTOR_ACCEL_MAX * land_detector_scalar);
 
         // check that vertical speed is within 1m/s of zero
-        bool descent_rate_low = fabsf(inertial_nav.get_velocity_z()) < 100 * land_detector_scalar;
+        bool descent_rate_low = fabsf(inertial_nav.get_velocity_z_up_cms()) < 100 * land_detector_scalar;
 
         // if we have a healthy rangefinder only allow landing detection below 2 meters
         bool rangefinder_check = (!rangefinder_alt_ok() || rangefinder_state.alt_cm_filt.get() < LAND_RANGEFINDER_MIN_ALT_CM);

ArduCopter/mode.cpp

@@ -569,7 +569,7 @@ void Mode::land_run_vertical_control(bool pause_descent)
             Vector2f target_pos;
             float target_error_cm = 0.0f;
             if (copter.precland.get_target_position_cm(target_pos)) {
-                const Vector2f current_pos = inertial_nav.get_position_xy();
+                const Vector2f current_pos = inertial_nav.get_position_xy_cm();
                 // target is this many cm away from the vehicle
                 target_error_cm = (target_pos - current_pos).length();
             }
@@ -647,10 +647,10 @@ void Mode::land_run_horizontal_control()
     if (doing_precision_landing) {
         Vector2f target_pos, target_vel_rel;
         if (!copter.precland.get_target_position_cm(target_pos)) {
-            target_pos = inertial_nav.get_position_xy();
+            target_pos = inertial_nav.get_position_xy_cm();
         }
         if (!copter.precland.get_target_velocity_relative_cms(target_vel_rel)) {
-            target_vel_rel = -inertial_nav.get_velocity_xy();
+            target_vel_rel = -inertial_nav.get_velocity_xy_cms();
         }
         pos_control->set_pos_target_xy_cm(target_pos.x, target_pos.y);
         pos_control->override_vehicle_velocity_xy(-target_vel_rel);

ArduCopter/mode_auto.cpp

@@ -355,7 +355,7 @@ void ModeAuto::circle_movetoedge_start(const Location &circle_center, float radi
     // check our distance from edge of circle
     Vector3f circle_edge_neu;
     copter.circle_nav->get_closest_point_on_circle(circle_edge_neu);
-    float dist_to_edge = (inertial_nav.get_position() - circle_edge_neu).length();
+    float dist_to_edge = (inertial_nav.get_position_neu_cm() - circle_edge_neu).length();
 
     // if more than 3m then fly to edge
     if (dist_to_edge > 300.0f) {
@@ -375,7 +375,7 @@ void ModeAuto::circle_movetoedge_start(const Location &circle_center, float radi
         }
 
         // if we are outside the circle, point at the edge, otherwise hold yaw
-        const float dist_to_center = get_horizontal_distance_cm(inertial_nav.get_position_xy().topostype(), copter.circle_nav->get_center().xy());
+        const float dist_to_center = get_horizontal_distance_cm(inertial_nav.get_position_xy_cm().topostype(), copter.circle_nav->get_center().xy());
         // initialise yaw
         // To-Do: reset the yaw only when the previous navigation command is not a WP.  this would allow removing the special check for ROI
         if (auto_yaw.mode() != AUTO_YAW_ROI) {
@@ -1753,15 +1753,15 @@ bool ModeAuto::verify_payload_place()
         FALLTHROUGH;
     case PayloadPlaceStateType_Descending_Start:
         nav_payload_place.descend_start_timestamp = now;
-        nav_payload_place.descend_start_altitude = inertial_nav.get_altitude();
+        nav_payload_place.descend_start_altitude = inertial_nav.get_position_z_up_cm();
         nav_payload_place.descend_throttle_level = 0;
         nav_payload_place.state = PayloadPlaceStateType_Descending;
         FALLTHROUGH;
     case PayloadPlaceStateType_Descending:
         // make sure we don't descend too far:
-        debug("descended: %f cm (%f cm max)", (nav_payload_place.descend_start_altitude - inertial_nav.get_altitude()), nav_payload_place.descend_max);
+        debug("descended: %f cm (%f cm max)", (nav_payload_place.descend_start_altitude - inertial_nav.get_position_z_up_cm()), nav_payload_place.descend_max);
         if (!is_zero(nav_payload_place.descend_max) &&
-            nav_payload_place.descend_start_altitude - inertial_nav.get_altitude()  > nav_payload_place.descend_max) {
+            nav_payload_place.descend_start_altitude - inertial_nav.get_position_z_up_cm()  > nav_payload_place.descend_max) {
             nav_payload_place.state = PayloadPlaceStateType_Ascending;
             gcs().send_text(MAV_SEVERITY_WARNING, "Reached maximum descent");
             return false; // we'll do any cleanups required next time through the loop
@@ -1819,7 +1819,7 @@ bool ModeAuto::verify_payload_place()
         }
         FALLTHROUGH;
     case PayloadPlaceStateType_Ascending_Start: {
-        Location target_loc(inertial_nav.get_position(), Location::AltFrame::ABOVE_ORIGIN);
+        Location target_loc(inertial_nav.get_position_neu_cm(), Location::AltFrame::ABOVE_ORIGIN);
         target_loc.alt = nav_payload_place.descend_start_altitude;
         wp_start(target_loc);
         nav_payload_place.state = PayloadPlaceStateType_Ascending;

ArduCopter/mode_autorotate.cpp

@@ -86,7 +86,7 @@ void ModeAutorotate::run()
     uint32_t now = millis(); //milliseconds
 
     // Initialise internal variables
-    float curr_vel_z = inertial_nav.get_velocity().z;   // Current vertical descent
+    float curr_vel_z = inertial_nav.get_velocity_z_up_cms();   // Current vertical descent
 
     //----------------------------------------------------------------
     //                  State machine logic

ArduCopter/mode_brake.cpp

@@ -10,8 +10,8 @@
 bool ModeBrake::init(bool ignore_checks)
 {
     // initialise pos controller speed and acceleration
-    pos_control->set_max_speed_accel_xy(inertial_nav.get_velocity().length(), BRAKE_MODE_DECEL_RATE);
+    pos_control->set_max_speed_accel_xy(inertial_nav.get_velocity_neu_cms().length(), BRAKE_MODE_DECEL_RATE);
-    pos_control->set_correction_speed_accel_xy(inertial_nav.get_velocity().length(), BRAKE_MODE_DECEL_RATE);
+    pos_control->set_correction_speed_accel_xy(inertial_nav.get_velocity_neu_cms().length(), BRAKE_MODE_DECEL_RATE);
 
     // initialise position controller
     pos_control->init_xy_controller();

ArduCopter/mode_drift.cpp

@@ -46,7 +46,7 @@ void ModeDrift::run()
     get_pilot_desired_lean_angles(target_roll, target_pitch, copter.aparm.angle_max, copter.aparm.angle_max);
 
     // Grab inertial velocity
-    const Vector3f& vel = inertial_nav.get_velocity();
+    const Vector3f& vel = inertial_nav.get_velocity_neu_cms();
 
     // rotate roll, pitch input from north facing to vehicle's perspective
     float roll_vel =  vel.y * ahrs.cos_yaw() - vel.x * ahrs.sin_yaw(); // body roll vel

ArduCopter/mode_flowhold.cpp

@@ -106,7 +106,7 @@ bool ModeFlowHold::init(bool ignore_checks)
     flow_pi_xy.set_dt(1.0/copter.scheduler.get_loop_rate_hz());
 
     // start with INS height
-    last_ins_height = copter.inertial_nav.get_altitude() * 0.01;
+    last_ins_height = copter.inertial_nav.get_position_z_up_cm() * 0.01;
     height_offset = 0;
 
     return true;
@@ -130,7 +130,7 @@ void ModeFlowHold::flowhold_flow_to_angle(Vector2f &bf_angles, bool stick_input)
     Vector2f sensor_flow = flow_filter.apply(raw_flow);
 
     // scale by height estimate, limiting it to height_min to height_max
-    float ins_height = copter.inertial_nav.get_altitude() * 0.01;
+    float ins_height = copter.inertial_nav.get_position_z_up_cm() * 0.01;
     float height_estimate = ins_height + height_offset;
 
     // compensate for height, this converts to (approx) m/s
@@ -347,7 +347,7 @@ void ModeFlowHold::run()
  */
 void ModeFlowHold::update_height_estimate(void)
 {
-    float ins_height = copter.inertial_nav.get_altitude() * 0.01;
+    float ins_height = copter.inertial_nav.get_position_z_up_cm() * 0.01;
 
 #if 1
     // assume on ground when disarmed, or if we have only just started spooling the motors up

ArduCopter/mode_follow.cpp

@@ -108,7 +108,7 @@ void ModeFollow::run()
         // calculate vehicle heading
         switch (g2.follow.get_yaw_behave()) {
             case AP_Follow::YAW_BEHAVE_FACE_LEAD_VEHICLE: {
-                if (dist_vec.xy().length() > 1.0f) {
+                if (dist_vec.xy().length_squared() > 1.0) {
                     yaw_cd = get_bearing_cd(Vector2f{}, dist_vec.xy());
                     use_yaw = true;
                 }
@@ -125,7 +125,7 @@ void ModeFollow::run()
             }
 
             case AP_Follow::YAW_BEHAVE_DIR_OF_FLIGHT: {
-                if (desired_velocity_neu_cms.xy().length() > 100.0f) {
+                if (desired_velocity_neu_cms.xy().length_squared() > (100.0 * 100.0)) {
                     yaw_cd = get_bearing_cd(Vector2f{}, desired_velocity_neu_cms.xy());
                     use_yaw = true;
                 }

ArduCopter/mode_guided.cpp

@@ -1060,7 +1060,7 @@ void ModeGuided::limit_init_time_and_pos()
     guided_limit.start_time = AP_HAL::millis();
 
     // initialise start position from current position
-    guided_limit.start_pos = inertial_nav.get_position();
+    guided_limit.start_pos = inertial_nav.get_position_neu_cm();
 }
 
 // limit_check - returns true if guided mode has breached a limit
@@ -1073,7 +1073,7 @@ bool ModeGuided::limit_check()
     }
 
     // get current location
-    const Vector3f& curr_pos = inertial_nav.get_position();
+    const Vector3f& curr_pos = inertial_nav.get_position_neu_cm();
 
     // check if we have gone below min alt
     if (!is_zero(guided_limit.alt_min_cm) && (curr_pos.z < guided_limit.alt_min_cm)) {
@@ -1118,7 +1118,7 @@ uint32_t ModeGuided::wp_distance() const
     case SubMode::WP:
         return wp_nav->get_wp_distance_to_destination();
     case SubMode::Pos:
-        return get_horizontal_distance_cm(inertial_nav.get_position_xy(), guided_pos_target_cm.tofloat().xy());
+        return get_horizontal_distance_cm(inertial_nav.get_position_xy_cm(), guided_pos_target_cm.tofloat().xy());
     case SubMode::PosVelAccel:
         return pos_control->get_pos_error_xy_cm();
         break;
@@ -1133,7 +1133,7 @@ int32_t ModeGuided::wp_bearing() const
     case SubMode::WP:
         return wp_nav->get_wp_bearing_to_destination();
     case SubMode::Pos:
-        return get_bearing_cd(inertial_nav.get_position_xy(), guided_pos_target_cm.tofloat().xy());
+        return get_bearing_cd(inertial_nav.get_position_xy_cm(), guided_pos_target_cm.tofloat().xy());
     case SubMode::PosVelAccel:
         return pos_control->get_bearing_to_target_cd();
         break;

ArduCopter/mode_loiter.cpp

@@ -61,10 +61,10 @@ void ModeLoiter::precision_loiter_xy()
     loiter_nav->clear_pilot_desired_acceleration();
     Vector2f target_pos, target_vel_rel;
     if (!copter.precland.get_target_position_cm(target_pos)) {
-        target_pos = inertial_nav.get_position_xy();
+        target_pos = inertial_nav.get_position_xy_cm();
     }
     if (!copter.precland.get_target_velocity_relative_cms(target_vel_rel)) {
-        target_vel_rel = -inertial_nav.get_velocity_xy();
+        target_vel_rel = -inertial_nav.get_velocity_xy_cms();
     }
     pos_control->set_pos_target_xy_cm(target_pos.x, target_pos.y);
     pos_control->override_vehicle_velocity_xy(-target_vel_rel);

ArduCopter/mode_poshold.cpp

@@ -69,7 +69,7 @@ void ModePosHold::run()
 {
     float controller_to_pilot_roll_mix; // mix of controller and pilot controls.  0 = fully last controller controls, 1 = fully pilot controls
     float controller_to_pilot_pitch_mix;    // mix of controller and pilot controls.  0 = fully last controller controls, 1 = fully pilot controls
-    const Vector3f& vel = inertial_nav.get_velocity();
+    const Vector3f& vel = inertial_nav.get_velocity_neu_cms();
 
     // set vertical speed and acceleration limits
     pos_control->set_max_speed_accel_z(-get_pilot_speed_dn(), g.pilot_speed_up, g.pilot_accel_z);
@@ -378,7 +378,7 @@ void ModePosHold::run()
         pitch_mode = RPMode::BRAKE_TO_LOITER;
         brake.to_loiter_timer = POSHOLD_BRAKE_TO_LOITER_TIMER;
         // init loiter controller
-        loiter_nav->init_target(inertial_nav.get_position_xy());
+        loiter_nav->init_target(inertial_nav.get_position_xy_cm());
         // set delay to start of wind compensation estimate updates
         wind_comp_start_timer = POSHOLD_WIND_COMP_START_TIMER;
     }
@@ -569,7 +569,7 @@ void ModePosHold::update_wind_comp_estimate()
     }
 
     // check horizontal velocity is low
-    if (inertial_nav.get_speed_xy() > POSHOLD_WIND_COMP_ESTIMATE_SPEED_MAX) {
+    if (inertial_nav.get_speed_xy_cms() > POSHOLD_WIND_COMP_ESTIMATE_SPEED_MAX) {
         return;
     }
 

ArduCopter/mode_throw.cpp

@@ -71,9 +71,9 @@ void ModeThrow::run()
         // initialise the demanded height to 3m above the throw height
         // we want to rapidly clear surrounding obstacles
         if (g2.throw_type == ThrowType::Drop) {
-            pos_control->set_pos_target_z_cm(inertial_nav.get_altitude() - 100);
+            pos_control->set_pos_target_z_cm(inertial_nav.get_position_z_up_cm() - 100);
         } else {
-            pos_control->set_pos_target_z_cm(inertial_nav.get_altitude() + 300);
+            pos_control->set_pos_target_z_cm(inertial_nav.get_position_z_up_cm() + 300);
         }
 
         // Set the auto_arm status to true to avoid a possible automatic disarm caused by selection of an auto mode with throttle at minimum
@@ -204,8 +204,8 @@ void ModeThrow::run()
     if ((stage != prev_stage) || (now - last_log_ms) > 100) {
         prev_stage = stage;
         last_log_ms = now;
-        const float velocity = inertial_nav.get_velocity().length();
+        const float velocity = inertial_nav.get_velocity_neu_cms().length();
-        const float velocity_z = inertial_nav.get_velocity().z;
+        const float velocity_z = inertial_nav.get_velocity_z_up_cms();
         const float accel = copter.ins.get_accel().length();
         const float ef_accel_z = ahrs.get_accel_ef().z;
         const bool throw_detect = (stage > Throw_Detecting) || throw_detected();
@@ -255,14 +255,14 @@ bool ModeThrow::throw_detected()
     }
 
     // Check for high speed (>500 cm/s)
-    bool high_speed = inertial_nav.get_velocity().length() > THROW_HIGH_SPEED;
+    bool high_speed = inertial_nav.get_velocity_neu_cms().length_squared() > (THROW_HIGH_SPEED * THROW_HIGH_SPEED);
 
     // check for upwards or downwards trajectory (airdrop) of 50cm/s
     bool changing_height;
     if (g2.throw_type == ThrowType::Drop) {
-        changing_height = inertial_nav.get_velocity().z < -THROW_VERTICAL_SPEED;
+        changing_height = inertial_nav.get_velocity_z_up_cms() < -THROW_VERTICAL_SPEED;
     } else {
-        changing_height = inertial_nav.get_velocity().z > THROW_VERTICAL_SPEED;
+        changing_height = inertial_nav.get_velocity_z_up_cms() > THROW_VERTICAL_SPEED;
     }
 
     // Check the vertical acceleraton is greater than 0.25g
@@ -277,11 +277,11 @@ bool ModeThrow::throw_detected()
     // Record time and vertical velocity when we detect the possible throw
     if (possible_throw_detected && ((AP_HAL::millis() - free_fall_start_ms) > 500)) {
         free_fall_start_ms = AP_HAL::millis();
-        free_fall_start_velz = inertial_nav.get_velocity().z;
+        free_fall_start_velz = inertial_nav.get_velocity_z_up_cms();
     }
 
     // Once a possible throw condition has been detected, we check for 2.5 m/s of downwards velocity change in less than 0.5 seconds to confirm
-    bool throw_condition_confirmed = ((AP_HAL::millis() - free_fall_start_ms < 500) && ((inertial_nav.get_velocity().z - free_fall_start_velz) < -250.0f));
+    bool throw_condition_confirmed = ((AP_HAL::millis() - free_fall_start_ms < 500) && ((inertial_nav.get_velocity_z_up_cms() - free_fall_start_velz) < -250.0f));
 
     // start motors and enter the control mode if we are in continuous freefall
     return throw_condition_confirmed;

ArduCopter/mode_zigzag.cpp

@@ -161,7 +161,7 @@ void ModeZigZag::run()
 void ModeZigZag::save_or_move_to_destination(Destination ab_dest)
 {
     // get current position as an offset from EKF origin
-    const Vector2f curr_pos {inertial_nav.get_position_xy()};
+    const Vector2f curr_pos {inertial_nav.get_position_xy_cm()};
 
     // handle state machine changes
     switch (stage) {
@@ -427,7 +427,7 @@ bool ModeZigZag::calculate_next_dest(Destination ab_dest, bool use_wpnav_alt, Ve
     }
 
     // get distance from vehicle to start_pos
-    const Vector2f curr_pos2d {inertial_nav.get_position_xy()};
+    const Vector2f curr_pos2d {inertial_nav.get_position_xy_cm()};
     Vector2f veh_to_start_pos = curr_pos2d - start_pos;
 
     // lengthen AB_diff so that it is at least as long as vehicle is from start point
@@ -458,7 +458,7 @@ bool ModeZigZag::calculate_next_dest(Destination ab_dest, bool use_wpnav_alt, Ve
                 next_dest.z = copter.rangefinder_state.alt_cm_filt.get();
             }
         } else {
-            next_dest.z = pos_control->is_active_z() ? pos_control->get_pos_target_z_cm() : inertial_nav.get_altitude();
+            next_dest.z = pos_control->is_active_z() ? pos_control->get_pos_target_z_cm() : inertial_nav.get_position_z_up_cm();
         }
     }
 
@@ -499,7 +499,7 @@ bool ModeZigZag::calculate_side_dest(Vector3f& next_dest, bool& terrain_alt) con
     float scalar = constrain_float(_side_dist, 0.1f, 100.0f) * 100 / safe_sqrt(AB_side.length_squared());
 
     // get distance from vehicle to start_pos
-    const Vector2f curr_pos2d {inertial_nav.get_position_xy()};
+    const Vector2f curr_pos2d {inertial_nav.get_position_xy_cm()};
     next_dest.x = curr_pos2d.x + (AB_side.x * scalar);
     next_dest.y = curr_pos2d.y + (AB_side.y * scalar);
 
@@ -510,7 +510,7 @@ bool ModeZigZag::calculate_side_dest(Vector3f& next_dest, bool& terrain_alt) con
             next_dest.z = copter.rangefinder_state.alt_cm_filt.get();
         }
     } else {
-        next_dest.z = pos_control->is_active_z() ? pos_control->get_pos_target_z_cm() : inertial_nav.get_altitude();
+        next_dest.z = pos_control->is_active_z() ? pos_control->get_pos_target_z_cm() : inertial_nav.get_position_z_up_cm();
     }
 
     return true;

ArduCopter/sensors.cpp

@@ -55,7 +55,7 @@ void Copter::read_rangefinder(void)
         rf_state.alt_cm = tilt_correction * rangefinder.distance_cm_orient(rf_orient);
 
         // remember inertial alt to allow us to interpolate rangefinder
-        rf_state.inertial_alt_cm = inertial_nav.get_altitude();
+        rf_state.inertial_alt_cm = inertial_nav.get_position_z_up_cm();
 
         // glitch handling.  rangefinder readings more than RANGEFINDER_GLITCH_ALT_CM from the last good reading
         // are considered a glitch and glitch_count becomes non-zero
@@ -141,7 +141,7 @@ bool Copter::get_rangefinder_height_interpolated_cm(int32_t& ret)
         return false;
     }
     ret = rangefinder_state.alt_cm_filt.get();
-    float inertial_alt_cm = inertial_nav.get_altitude();
+    float inertial_alt_cm = inertial_nav.get_position_z_up_cm();
     ret += inertial_alt_cm - rangefinder_state.inertial_alt_cm;
     return true;
 }

ArduCopter/surface_tracking.cpp

@@ -17,7 +17,7 @@ void Copter::SurfaceTracking::update_surface_offset()
         // e.g. if vehicle is 10m above the EKF origin and rangefinder reports alt of 3m.  curr_surface_alt_above_origin_cm is 7m (or 700cm)
         RangeFinderState &rf_state = (surface == Surface::GROUND) ? copter.rangefinder_state : copter.rangefinder_up_state;
         const float dir = (surface == Surface::GROUND) ? 1.0f : -1.0f;
-        float curr_surface_alt_above_origin_cm = copter.inertial_nav.get_altitude() - dir * rf_state.alt_cm;
+        const float curr_surface_alt_above_origin_cm = copter.inertial_nav.get_position_z_up_cm() - dir * rf_state.alt_cm;
 
         // update position controller target offset to the surface's alt above the EKF origin
         copter.pos_control->set_pos_offset_target_z_cm(curr_surface_alt_above_origin_cm);
@@ -74,7 +74,7 @@ void Copter::SurfaceTracking::set_target_alt_cm(float _target_alt_cm)
     if (surface != Surface::GROUND) {
         return;
     }
-    copter.pos_control->set_pos_offset_z_cm(copter.inertial_nav.get_altitude() - _target_alt_cm);
+    copter.pos_control->set_pos_offset_z_cm(copter.inertial_nav.get_position_z_up_cm() - _target_alt_cm);
     last_update_ms = AP_HAL::millis();
 }
 

ArduCopter/takeoff.cpp

@@ -133,7 +133,7 @@ void Mode::auto_takeoff_run()
     // check if we are not navigating because of low altitude
     if (auto_takeoff_no_nav_active) {
         // check if vehicle has reached no_nav_alt threshold
-        if (inertial_nav.get_altitude() >= auto_takeoff_no_nav_alt_cm) {
+        if (inertial_nav.get_position_z_up_cm() >= auto_takeoff_no_nav_alt_cm) {
             auto_takeoff_no_nav_active = false;
             wp_nav->shift_wp_origin_and_destination_to_stopping_point_xy();
         } else {
@@ -173,7 +173,7 @@ void Mode::auto_takeoff_set_start_alt(void)
 {
     if ((g2.wp_navalt_min > 0) && (is_disarmed_or_landed() || !motors->get_interlock())) {
         // we are not flying, climb with no navigation to current alt-above-ekf-origin + wp_navalt_min
-        auto_takeoff_no_nav_alt_cm = inertial_nav.get_altitude() + g2.wp_navalt_min * 100;
+        auto_takeoff_no_nav_alt_cm = inertial_nav.get_position_z_up_cm() + g2.wp_navalt_min * 100;
         auto_takeoff_no_nav_active = true;
     } else {
         auto_takeoff_no_nav_active = false;

ArduCopter/toy_mode.cpp

@@ -846,7 +846,7 @@ void ToyMode::throttle_adjust(float &throttle_control)
     }
 
     // limit descent rate close to the ground
-    float height = copter.inertial_nav.get_altitude() * 0.01 - copter.arming_altitude_m;
+    float height = copter.inertial_nav.get_position_z_up_cm() * 0.01 - copter.arming_altitude_m;
     if (throttle_control < 500 &&
         height < TOY_DESCENT_SLOW_HEIGHT + TOY_DESCENT_SLOW_RAMP &&
         copter.motors->armed() && !copter.ap.land_complete) {