Sub: lthall updates for offset handling

rmackay9 · Sep 28, 2024 · 0cd349f · 0cd349f
1 parent 08233b4
commit 0cd349f
Show file tree

Hide file tree

Showing 6 changed files with 11 additions and 13 deletions.
diff --git a/ArduSub/GCS_Mavlink.cpp b/ArduSub/GCS_Mavlink.cpp
@@ -817,7 +817,7 @@ void GCS_MAVLINK_Sub::handle_message(const mavlink_message_t &msg)
          */
 
         if (!z_ignore && sub.control_mode == Mode::Number::ALT_HOLD) { // Control only target depth when in ALT_HOLD
-            sub.pos_control.set_pos_target_z_cm(packet.alt*100);
+            sub.pos_control.set_pos_desired_z_cm(packet.alt*100);
             break;
         }
 

diff --git a/ArduSub/mode_acro.cpp b/ArduSub/mode_acro.cpp
@@ -7,7 +7,7 @@
 
 bool ModeAcro::init(bool ignore_checks) {
     // set target altitude to zero for reporting
-    position_control->set_pos_target_z_cm(0);
+    position_control->set_pos_desired_z_cm(0);
 
     // attitude hold inputs become thrust inputs in acro mode
     // set to neutral to prevent chaotic behavior (esp. roll/pitch)

diff --git a/ArduSub/mode_althold.cpp b/ArduSub/mode_althold.cpp
@@ -111,9 +111,9 @@ void ModeAlthold::control_depth() {
     //we allow full control to the pilot, but as soon as there's no input, we handle being at surface/bottom
     if (fabsf(target_climb_rate_cm_s) < 0.05f)  {
         if (sub.ap.at_surface) {
-            position_control->set_pos_target_z_cm(MIN(position_control->get_pos_target_z_cm(), g.surface_depth - 5.0f)); // set target to 5 cm below surface level
+            position_control->set_pos_desired_z_cm(MIN(position_control->get_pos_desired_z_cm(), g.surface_depth - 5.0f)); // set target to 5 cm below surface level
         } else if (sub.ap.at_bottom) {
-            position_control->set_pos_target_z_cm(MAX(inertial_nav.get_position_z_up_cm() + 10.0f, position_control->get_pos_target_z_cm())); // set target to 10 cm above bottom
+            position_control->set_pos_desired_z_cm(MAX(inertial_nav.get_position_z_up_cm() + 10.0f, position_control->get_pos_desired_z_cm())); // set target to 10 cm above bottom
         }
     }
 

diff --git a/ArduSub/mode_manual.cpp b/ArduSub/mode_manual.cpp
@@ -3,7 +3,7 @@
 
 bool ModeManual::init(bool ignore_checks) {
     // set target altitude to zero for reporting
-    position_control->set_pos_target_z_cm(0);
+    position_control->set_pos_desired_z_cm(0);
 
     // attitude hold inputs become thrust inputs in manual mode
     // set to neutral to prevent chaotic behavior (esp. roll/pitch)

diff --git a/ArduSub/mode_stabilize.cpp b/ArduSub/mode_stabilize.cpp
@@ -3,7 +3,7 @@
 
 bool ModeStabilize::init(bool ignore_checks) {
     // set target altitude to zero for reporting
-    position_control->set_pos_target_z_cm(0);
+    position_control->set_pos_desired_z_cm(0);
     sub.last_pilot_heading = ahrs.yaw_sensor;
 
     return true;

diff --git a/ArduSub/mode_surftrak.cpp b/ArduSub/mode_surftrak.cpp
@@ -81,8 +81,7 @@ bool ModeSurftrak::set_rangefinder_target_cm(float target_cm)
 
         // Initialize the terrain offset
         auto terrain_offset_cm = sub.inertial_nav.get_position_z_up_cm() - rangefinder_target_cm;
-        sub.pos_control.set_pos_offset_z_cm(terrain_offset_cm);
-        sub.pos_control.set_pos_offset_target_z_cm(terrain_offset_cm);
+        sub.pos_control.init_pos_terrain_cm(terrain_offset_cm);
 
     } else {
         reset();
@@ -97,8 +96,7 @@ void ModeSurftrak::reset()
     rangefinder_target_cm = INVALID_TARGET;
 
     // Reset the terrain offset
-    sub.pos_control.set_pos_offset_z_cm(0);
-    sub.pos_control.set_pos_offset_target_z_cm(0);
+    sub.pos_control.init_pos_terrain_cm(0);
 }
 
 /*
@@ -117,11 +115,11 @@ void ModeSurftrak::control_range() {
         }
         if (sub.ap.at_surface) {
             // Set target depth to 5 cm below SURFACE_DEPTH and reset
-            position_control->set_pos_target_z_cm(MIN(position_control->get_pos_target_z_cm(), g.surface_depth - 5.0f));
+            position_control->set_pos_desired_z_cm(MIN(position_control->get_pos_desired_z_cm(), g.surface_depth - 5.0f));
             reset();
         } else if (sub.ap.at_bottom) {
             // Set target depth to 10 cm above bottom and reset
-            position_control->set_pos_target_z_cm(MAX(inertial_nav.get_position_z_up_cm() + 10.0f, position_control->get_pos_target_z_cm()));
+            position_control->set_pos_desired_z_cm(MAX(inertial_nav.get_position_z_up_cm() + 10.0f, position_control->get_pos_desired_z_cm()));
             reset();
         } else {
             // Typical operation
@@ -164,7 +162,7 @@ void ModeSurftrak::update_surface_offset()
             }
 
             // Set the offset target, AC_PosControl will do the rest
-            sub.pos_control.set_pos_offset_target_z_cm(rangefinder_terrain_offset_cm);
+            sub.pos_control.set_pos_terrain_target_cm(rangefinder_terrain_offset_cm);
         }
     }
 #endif  // AP_RANGEFINDER_ENABLED