ArduSub: Remove inertial_nav

peterbarker · claude · peterbarker · commit c1170033f414 · 2026-05-09T21:41:19.000+10:00
Co-Authored-By: Claude Haiku 4.5 &lt;noreply@anthropic.com&gt;
diff --git a/ArduSub/Attitude.cpp b/ArduSub/Attitude.cpp
@@ -64,15 +64,16 @@ 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;
 }
 
 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))) {
diff --git a/ArduSub/GCS_MAVLink_Sub.cpp b/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;
             }
         }
 
diff --git a/ArduSub/Log.cpp b/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,
diff --git a/ArduSub/Sub.cpp b/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),
diff --git a/ArduSub/Sub.h b/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 @@ class Sub : public AP_Vehicle {
     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
diff --git a/ArduSub/commands_logic.cpp b/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
diff --git a/ArduSub/inertia.cpp b/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;
 }
diff --git a/ArduSub/mode.cpp b/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),
diff --git a/ArduSub/mode.h b/ArduSub/mode.h
@@ -96,7 +96,6 @@ class Mode
     // 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;
diff --git a/ArduSub/mode_althold.cpp b/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
         }
     }
 
diff --git a/ArduSub/mode_auto.cpp b/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 {
diff --git a/ArduSub/mode_guided.cpp b/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)) {
diff --git a/ArduSub/mode_surftrak.cpp b/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,15 +126,15 @@ 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
             update_surface_offset();
         }
     } 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;
     }
 
diff --git a/ArduSub/sensors.cpp b/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);
 
diff --git a/ArduSub/wscript b/ArduSub/wscript
@@ -9,7 +9,6 @@ def build(bld):
             'AC_AttitudeControl',
             'AC_WPNav',
             'AP_Camera',
-            'AP_InertialNav',
             'AP_JSButton',
             'AP_LeakDetector',
             'AP_Motors',

Original file line number	Diff line number	Diff line change
`@@ -64,15 +64,16 @@ float Sub::get_roi_yaw()`
`64`	`64`	`roi_yaw_counter++;`
`65`	`65`	`if (roi_yaw_counter >= 4) {`
`66`	`66`	`roi_yaw_counter = 0;`
`67`		`- yaw_look_at_WP_bearing = get_bearing_cd(inertial_nav.get_position_xy_cm(), roi_WP.xy());`
	`67`	`+ yaw_look_at_WP_bearing = get_bearing_cd((pos_control.get_pos_estimate_NED_m().xy() * 100.0f).tofloat(), roi_WP.xy());`
`68`	`68`	`}`
`69`	`69`
`70`	`70`	`return yaw_look_at_WP_bearing;`
`71`	`71`	`}`
`72`	`72`
`73`	`73`	`float Sub::get_look_ahead_yaw()`
`74`	`74`	`{`
`75`		`- const Vector3f& vel = inertial_nav.get_velocity_neu_cms();`
	`75`	`+ Vector3f vel = (pos_control.get_vel_estimate_NED_ms() * 100.0f).tofloat();`
	`76`	`+ vel.z = -vel.z;`
`76`	`77`	`const float speed_sq = vel.xy().length_squared();`
`77`	`78`	`// Commanded Yaw to automatically look ahead.`
`78`	`79`	`if (position_ok() && (speed_sq > (YAW_LOOK_AHEAD_MIN_SPEED * YAW_LOOK_AHEAD_MIN_SPEED))) {`
Original file line number	Diff line number	Diff line change
`@@ -641,7 +641,9 @@ void GCS_MAVLINK_Sub::handle_message(const mavlink_message_t &msg)`
`641`	`641`	`packet.coordinate_frame == MAV_FRAME_BODY_NED \|\|`
`642`	`642`	`packet.coordinate_frame == MAV_FRAME_BODY_FRD \|\|`
`643`	`643`	`packet.coordinate_frame == MAV_FRAME_BODY_OFFSET_NED) {`
`644`		`- pos_vector += sub.inertial_nav.get_position_neu_cm();`
	`644`	`+ Vector3f pos_cm = (sub.pos_control.get_pos_estimate_NED_m() * 100.0f).tofloat();`
	`645`	`+ pos_cm.z = -pos_cm.z;`
	`646`	`+ pos_vector += pos_cm;`
`645`	`647`	`}`
`646`	`648`	`}`
`647`	`649`
Original file line number	Diff line number	Diff line change
`@@ -486,7 +486,7 @@ bool Sub::verify_circle(const AP_Mission::Mission_Command& cmd)`
`486`	`486`
`487`	`487`	`// set lat/lon position if not provided`
`488`	`488`	`if (cmd.content.location.lat == 0 && cmd.content.location.lng == 0) {`
`489`		`- circle_center.xy() = inertial_nav.get_position_xy_cm();`
	`489`	`+ circle_center.xy() = (sub.pos_control.get_pos_estimate_NED_m().xy() * 100.0f).tofloat();`
`490`	`490`	`}`
`491`	`491`
`492`	`492`	`// start circling`
Original file line number	Diff line number	Diff line change
`@@ -4,7 +4,6 @@`
`4`	`4`	`void Sub::read_inertia()`
`5`	`5`	`{`
`6`	`6`	`// inertial altitude estimates`
`7`		`- inertial_nav.update();`
`8`	`7`	`sub.pos_control.update_estimates();`
`9`	`8`
`10`	`9`	`// pull position from ahrs`
`@@ -19,9 +18,9 @@ void Sub::read_inertia()`
`19`	`18`	`return;`
`20`	`19`	`}`
`21`	`20`
`22`		`- current_loc.alt = inertial_nav.get_position_z_up_cm();`
	`21`	`+ current_loc.alt = pos_control.get_pos_estimate_U_m() * 100.0f;`
`23`	`22`
`24`	`23`	`// get velocity, altitude is always absolute frame, referenced from`
`25`	`24`	`// water's surface`
`26`		`- climb_rate = inertial_nav.get_velocity_z_up_cms();`
	`25`	`+ climb_rate = pos_control.get_vel_estimate_U_ms() * 100.0f;`
`27`	`26`	`}`