ArduSub: Remove inertial_nav

Co-Authored-By: Claude Haiku 4.5 <noreply@anthropic.com>

ArduSub/Attitude.cpp

@@ -64,7 +64,7 @@ float Sub::get_roi_yaw()
     roi_yaw_counter++;
     if (roi_yaw_counter >= 4) {
         roi_yaw_counter = 0;
-        yaw_look_at_WP_bearing = get_bearing_cd(inertial_nav.get_position_xy_cm(), roi_WP.xy());
+        yaw_look_at_WP_bearing = get_bearing_cd((pos_control.get_pos_estimate_NED_m().xy() * 100.0f).tofloat(), roi_WP.xy());
     }
 
     return yaw_look_at_WP_bearing;
@@ -72,7 +72,8 @@ float Sub::get_roi_yaw()
 
 float Sub::get_look_ahead_yaw()
 {
-    const Vector3f& vel = inertial_nav.get_velocity_neu_cms();
+    Vector3f vel = (pos_control.get_vel_estimate_NED_ms() * 100.0f).tofloat();
+    vel.z = -vel.z;
     const float speed_sq = vel.xy().length_squared();
     // Commanded Yaw to automatically look ahead.
     if (position_ok() && (speed_sq > (YAW_LOOK_AHEAD_MIN_SPEED * YAW_LOOK_AHEAD_MIN_SPEED))) {

ArduSub/GCS_MAVLink_Sub.cpp

@@ -641,7 +641,9 @@ void GCS_MAVLINK_Sub::handle_message(const mavlink_message_t &msg)
                     packet.coordinate_frame == MAV_FRAME_BODY_NED ||
                     packet.coordinate_frame == MAV_FRAME_BODY_FRD ||
                     packet.coordinate_frame == MAV_FRAME_BODY_OFFSET_NED) {
-                pos_vector += sub.inertial_nav.get_position_neu_cm();
+                Vector3f pos_cm = (sub.pos_control.get_pos_estimate_NED_m() * 100.0f).tofloat();
+                pos_cm.z = -pos_cm.z;
+                pos_vector += pos_cm;
             }
         }
 

ArduSub/Log.cpp

@@ -45,7 +45,7 @@ void Sub::Log_Write_Control_Tuning()
         throttle_out        : motors.get_throttle(),
         throttle_hover      : motors.get_throttle_hover(),
         desired_alt         : pos_control.get_pos_target_U_cm() * 0.01f,
-        inav_alt            : inertial_nav.get_position_z_up_cm() * 0.01f,
+        inav_alt            : pos_control.get_pos_estimate_U_m(),
         baro_alt            : barometer.get_altitude(),
         desired_rangefinder_alt   : mode_surftrak.get_rangefinder_target_cm() * 0.01,
         rangefinder_alt           : rangefinder_state.alt,

ArduSub/Sub.cpp

@@ -32,7 +32,6 @@ Sub::Sub()
 #endif
           motors(MAIN_LOOP_RATE),
           auto_yaw_mode(AUTO_YAW_LOOK_AT_NEXT_WP),
-          inertial_nav(ahrs),
           ahrs_view(ahrs, ROTATION_NONE),
           attitude_control(ahrs_view, motors),
           pos_control(ahrs_view, motors, attitude_control),

ArduSub/Sub.h

@@ -51,7 +51,6 @@
 #include <AP_Relay/AP_Relay.h>           // APM relay
 #include <AP_Mount/AP_Mount.h>           // Camera/Antenna mount
 #include <AP_Vehicle/AP_Vehicle.h>         // needed for AHRS build
-#include <AP_InertialNav/AP_InertialNav.h>     // inertial navigation library
 #include <AC_WPNav/AC_WPNav.h>           // Waypoint navigation library
 #include <AC_WPNav/AC_Loiter.h>
 #include <AC_WPNav/AC_Circle.h>          // circle navigation library
@@ -331,9 +330,6 @@ private:
     int32_t condition_value;  // used in condition commands (eg delay, change alt, etc.)
     uint32_t condition_start;
 
-    // Inertial Navigation
-    AP_InertialNav inertial_nav;
-
     AP_AHRS_View ahrs_view;
 
     // Attitude, Position and Waypoint navigation objects

ArduSub/commands_logic.cpp

@@ -486,7 +486,7 @@ bool Sub::verify_circle(const AP_Mission::Mission_Command& cmd)
 
             // set lat/lon position if not provided
             if (cmd.content.location.lat == 0 && cmd.content.location.lng == 0) {
-                circle_center.xy() = inertial_nav.get_position_xy_cm();
+                circle_center.xy() = (sub.pos_control.get_pos_estimate_NED_m().xy() * 100.0f).tofloat();
             }
 
             // start circling

ArduSub/inertia.cpp

@@ -4,7 +4,6 @@
 void Sub::read_inertia()
 {
     // inertial altitude estimates
-    inertial_nav.update();
     sub.pos_control.update_estimates();
 
     // pull position from ahrs
@@ -19,9 +18,9 @@ void Sub::read_inertia()
         return;
     }
 
-    current_loc.alt = inertial_nav.get_position_z_up_cm();
+    current_loc.alt = pos_control.get_pos_estimate_U_m() * 100.0f;
 
     // get velocity, altitude is always absolute frame, referenced from
     // water's surface
-    climb_rate = inertial_nav.get_velocity_z_up_cms();
+    climb_rate = pos_control.get_vel_estimate_U_ms() * 100.0f;
 }

ArduSub/mode.cpp

@@ -6,7 +6,6 @@
 Mode::Mode(void) :
     g(sub.g),
     g2(sub.g2),
-    inertial_nav(sub.inertial_nav),
     ahrs(sub.ahrs),
     motors(sub.motors),
     channel_roll(sub.channel_roll),

ArduSub/mode.h

@@ -96,7 +96,6 @@ protected:
     // convenience references to avoid code churn in conversion:
     Parameters &g;
     ParametersG2 &g2;
-    AP_InertialNav &inertial_nav;
     AP_AHRS &ahrs;
     AP_Motors6DOF &motors;
     RC_Channel *&channel_roll;

ArduSub/mode_althold.cpp

@@ -108,7 +108,7 @@ void ModeAlthold::run_post()
 void ModeAlthold::control_depth() {
     // return 0.2f when at the surface to p
     // scale linearly between 0.2f and 1.0f as we approach the surface
-    float distance_to_surface = (g.surface_depth - inertial_nav.get_position_z_up_cm()) * 0.01f;
+    float distance_to_surface = (g.surface_depth - position_control->get_pos_estimate_U_m() * 100.0f) * 0.01f;
     distance_to_surface = constrain_float(distance_to_surface, 0.0f, 1.0f);
     motors.set_max_throttle(g.surface_max_throttle + (1.0f - g.surface_max_throttle) * distance_to_surface);
 
@@ -121,7 +121,7 @@ void ModeAlthold::control_depth() {
         if (sub.ap.at_surface) {
             position_control->set_pos_desired_U_cm(MIN(position_control->get_pos_desired_U_cm(), g.surface_depth)); // set target to 5 cm below surface level
         } else if (sub.ap.at_bottom) {
-            position_control->set_pos_desired_U_cm(MAX(inertial_nav.get_position_z_up_cm() + 10.0f, position_control->get_pos_desired_U_cm())); // set target to 10 cm above bottom
+            position_control->set_pos_desired_U_cm(MAX(position_control->get_pos_estimate_U_m() * 100.0f + 10.0f, position_control->get_pos_desired_U_cm())); // set target to 10 cm above bottom
         }
     }
 

ArduSub/mode_auto.cpp

@@ -208,7 +208,7 @@ void ModeAuto::auto_circle_movetoedge_start(const Location &circle_center, float
         }
 
         // if we are outside the circle, point at the edge, otherwise hold yaw
-        float dist_to_center = get_horizontal_distance(inertial_nav.get_position_xy_cm().topostype(), sub.circle_nav.get_center_NEU_cm().xy());
+        float dist_to_center = get_horizontal_distance((position_control->get_pos_estimate_NED_m().xy() * 100.0f).tofloat(), sub.circle_nav.get_center_NEU_cm().xy().tofloat());
         if (dist_to_center > sub.circle_nav.get_radius_cm() && dist_to_center > 500) {
             set_auto_yaw_mode(get_default_auto_yaw_mode(false));
         } else {

ArduSub/mode_guided.cpp

@@ -850,7 +850,9 @@ void ModeGuided::guided_limit_init_time_and_pos()
     guided_limit.start_time_ms = AP_HAL::millis();
 
     // initialise start position from current position
-    guided_limit.start_pos_neu_cm = inertial_nav.get_position_neu_cm();
+    Vector3f pos_cm = (position_control->get_pos_estimate_NED_m() * 100.0f).tofloat();
+    pos_cm.z = -pos_cm.z;
+    guided_limit.start_pos_neu_cm = pos_cm;
 }
 
 // guided_limit_check - returns true if guided mode has breached a limit
@@ -863,7 +865,8 @@ bool ModeGuided::guided_limit_check()
     }
 
     // get current location
-    const Vector3f& curr_pos_neu_cm = inertial_nav.get_position_neu_cm();
+    Vector3f curr_pos_neu_cm = (position_control->get_pos_estimate_NED_m() * 100.0f).tofloat();
+    curr_pos_neu_cm.z = -curr_pos_neu_cm.z;
 
     // check if we have gone below min alt
     if (!is_zero(guided_limit.alt_min_cm) && (curr_pos_neu_cm.z < guided_limit.alt_min_cm)) {

ArduSub/mode_surftrak.cpp

@@ -40,7 +40,7 @@ bool ModeSurftrak::init(bool ignore_checks)
     if (!sub.rangefinder_alt_ok()) {
         sub.gcs().send_text(MAV_SEVERITY_INFO, "waiting for a rangefinder reading");
 #if AP_RANGEFINDER_ENABLED
-    } else if (sub.inertial_nav.get_position_z_up_cm() >= sub.g.surftrak_depth) {
+    } else if (position_control->get_pos_estimate_U_m() * 100.0f >= sub.g.surftrak_depth) {
         sub.gcs().send_text(MAV_SEVERITY_WARNING, "descend below %f meters to hold range", sub.g.surftrak_depth * 0.01f);
 #endif
     }
@@ -72,7 +72,7 @@ bool ModeSurftrak::set_rangefinder_target_cm(float target_cm)
 #if AP_RANGEFINDER_ENABLED
     if (sub.control_mode != Number::SURFTRAK) {
         sub.gcs().send_text(MAV_SEVERITY_WARNING, "wrong mode, rangefinder target not set");
-    } else if (sub.inertial_nav.get_position_z_up_cm() >= sub.g.surftrak_depth) {
+    } else if (position_control->get_pos_estimate_U_m() * 100.0f >= sub.g.surftrak_depth) {
         sub.gcs().send_text(MAV_SEVERITY_WARNING, "descend below %f meters to set rangefinder target", sub.g.surftrak_depth * 0.01f);
     } else if (target_cm < sub.rangefinder_state.min*100) {
         sub.gcs().send_text(MAV_SEVERITY_WARNING, "rangefinder target below minimum, ignored");
@@ -87,7 +87,7 @@ bool ModeSurftrak::set_rangefinder_target_cm(float target_cm)
         sub.gcs().send_text(MAV_SEVERITY_INFO, "rangefinder target is %.2f meters", rangefinder_target_cm * 0.01f);
 
         // Initialize the terrain offset
-        auto terrain_offset_cm = sub.inertial_nav.get_position_z_up_cm() - rangefinder_target_cm;
+        auto terrain_offset_cm = position_control->get_pos_estimate_U_m() * 100.0f - rangefinder_target_cm;
         sub.pos_control.init_pos_terrain_U_cm(terrain_offset_cm);
 
     } else {
@@ -117,7 +117,7 @@ void ModeSurftrak::control_range() {
     if (fabsf(target_climb_rate_cms) < 0.05f)  {
         if (pilot_in_control) {
             // Pilot has released control; apply the delta to the rangefinder target
-            set_rangefinder_target_cm(rangefinder_target_cm + inertial_nav.get_position_z_up_cm() - pilot_control_start_z_cm);
+            set_rangefinder_target_cm(rangefinder_target_cm + position_control->get_pos_estimate_U_m() * 100.0f - pilot_control_start_z_cm);
             pilot_in_control = false;
         }
         if (sub.ap.at_surface) {
@@ -126,7 +126,7 @@ void ModeSurftrak::control_range() {
             reset();
         } else if (sub.ap.at_bottom) {
             // Set target depth to 10 cm above bottom and reset
-            position_control->set_pos_desired_U_cm(MAX(inertial_nav.get_position_z_up_cm() + 10.0f, position_control->get_pos_desired_U_cm()));
+            position_control->set_pos_desired_U_cm(MAX(position_control->get_pos_estimate_U_m() * 100.0f + 10.0f, position_control->get_pos_desired_U_cm()));
             reset();
         } else {
             // Typical operation
@@ -134,7 +134,7 @@ void ModeSurftrak::control_range() {
         }
     } else if (HAS_VALID_TARGET && !pilot_in_control) {
         // Pilot has taken control; note the current depth
-        pilot_control_start_z_cm = inertial_nav.get_position_z_up_cm();
+        pilot_control_start_z_cm = position_control->get_pos_estimate_U_m() * 100.0f;
         pilot_in_control = true;
     }
 

ArduSub/sensors.cpp

@@ -47,7 +47,7 @@ void Sub::read_rangefinder()
 #endif
 
     rangefinder_state.alt = temp_alt_m;
-    rangefinder_state.inertial_alt_cm = inertial_nav.get_position_z_up_cm();
+    rangefinder_state.inertial_alt_cm = pos_control.get_pos_estimate_U_m() * 100.0f;
     rangefinder_state.min = rangefinder.min_distance_orient(ROTATION_PITCH_270);
     rangefinder_state.max = rangefinder.max_distance_orient(ROTATION_PITCH_270);
 

ArduSub/wscript

@@ -9,7 +9,6 @@ def build(bld):
             'AC_AttitudeControl',
             'AC_WPNav',
             'AP_Camera',
-            'AP_InertialNav',
             'AP_JSButton',
             'AP_LeakDetector',
             'AP_Motors',