Sub: support ABOVE_TERRAIN frame in GUIDED mode posvel case

ArduSub/GCS_Mavlink.cpp

@@ -836,6 +836,17 @@ void GCS_MAVLINK_Sub::handle_message(const mavlink_message_t &msg)
                 int32_t(packet.alt*100),
                 frame,
             };
+
+            // posvel case supports ABOVE_TERRAIN frame
+            if (!vel_ignore && acc_ignore && frame == Location::AltFrame::ABOVE_TERRAIN) {
+                if (!loc.get_vector_xy_from_origin_NE(pos_neu_cm.xy())) {
+                    break;
+                }
+                pos_neu_cm.z = loc.alt;
+                sub.mode_guided.guided_set_destination_posvel(pos_neu_cm, Vector3f(packet.vx * 100.0f, packet.vy * 100.0f, -packet.vz * 100.0f), frame);
+                break;
+            }
+
             if (!loc.get_vector_from_origin_NEU(pos_neu_cm)) {
                 break;
             }

ArduSub/Log.cpp

@@ -176,6 +176,7 @@ struct PACKED log_GuidedTarget {
     LOG_PACKET_HEADER;
     uint64_t time_us;
     uint8_t type;
+    uint8_t alt_frame;
     float pos_target_x;
     float pos_target_y;
     float pos_target_z;
@@ -185,12 +186,14 @@ struct PACKED log_GuidedTarget {
 };
 
 // Write a Guided mode target
-void Sub::Log_Write_GuidedTarget(uint8_t target_type, const Vector3f& pos_target, const Vector3f& vel_target)
+void Sub::Log_Write_GuidedTarget(uint8_t target_type, const Vector3f& pos_target, const Vector3f& vel_target,
+                                 Location::AltFrame alt_frame)
 {
     struct log_GuidedTarget pkt = {
         LOG_PACKET_HEADER_INIT(LOG_GUIDEDTARGET_MSG),
         time_us         : AP_HAL::micros64(),
         type            : target_type,
+        alt_frame       : static_cast<uint8_t>(alt_frame),
         pos_target_x    : pos_target.x,
         pos_target_y    : pos_target.y,
         pos_target_z    : pos_target.z,
@@ -251,6 +254,7 @@ void Sub::Log_Write_GuidedTarget(uint8_t target_type, const Vector3f& pos_target
 // @Description: Guided mode target information
 // @Field: TimeUS: Time since system startup
 // @Field: Type: Type of guided mode
+// @Field: Frame: Location.AltFrame
 // @Field: pX: Target position, X-Axis
 // @Field: pY: Target position, Y-Axis
 // @Field: pZ: Target position, Z-Axis
@@ -276,7 +280,7 @@ const struct LogStructure Sub::log_structure[] = {
     { LOG_DATA_FLOAT_MSG, sizeof(log_Data_Float),         
       "DFLT",  "QBf",         "TimeUS,Id,Value", "s--", "F--" },
     { LOG_GUIDEDTARGET_MSG, sizeof(log_GuidedTarget),
-      "GUIP",  "QBffffff",    "TimeUS,Type,pX,pY,pZ,vX,vY,vZ", "s-mmmnnn", "F-000000" },
+      "GUIP",  "QBBffffff",    "TimeUS,Type,Frame,pX,pY,pZ,vX,vY,vZ", "s--mmmnnn", "F--000000" },
 };
 
 uint8_t Sub::get_num_log_structures() const

ArduSub/Sub.cpp

@@ -49,6 +49,37 @@ Sub::Sub()
     _singleton = this;
 }
 
+#if AP_SCRIPTING_ENABLED
+// set target position and velocity, alt frame is Location::AltFrame::ABOVE_ORIGIN
+bool Sub::set_target_posvel_NED(const Vector3f& target_pos, const Vector3f& target_vel)
+{
+    // exit if vehicle is not in Guided mode or Auto-Guided mode
+    if (!flightmode->in_guided_mode()) {
+        return false;
+    }
+
+    const Vector3f pos_neu_cm(target_pos.x * 100.0f, target_pos.y * 100.0f, -target_pos.z * 100.0f);
+    const Vector3f vel_neu_cms(target_vel.x * 100.0f, target_vel.y * 100.0f, -target_vel.z * 100.0f);
+
+    return mode_guided.guided_set_destination_posvel(pos_neu_cm, vel_neu_cms, Location::AltFrame::ABOVE_ORIGIN);
+}
+
+// set target position and velocity, alt frame is Location::AltFrame::ABOVE_TERRAIN
+bool Sub::set_target_posvel_terrain(const Vector3f& target_pos, const Vector3f& target_vel)
+{
+    // exit if vehicle is not in Guided mode or Auto-Guided mode
+    if (!flightmode->in_guided_mode()) {
+        return false;
+    }
+
+    // pos.z is rf target, do not flip sign
+    const Vector3f pos_neu_cm(target_pos.x * 100.0f, target_pos.y * 100.0f, target_pos.z * 100.0f);
+    const Vector3f vel_neu_cms(target_vel.x * 100.0f, target_vel.y * 100.0f, -target_vel.z * 100.0f);
+
+    return mode_guided.guided_set_destination_posvel(pos_neu_cm, vel_neu_cms, Location::AltFrame::ABOVE_TERRAIN);
+}
+#endif
+
 Sub *Sub::_singleton = nullptr;
 
 Sub sub;

ArduSub/Sub.h

@@ -422,7 +422,8 @@ class Sub : public AP_Vehicle {
     void Log_Write_Data(LogDataID id, int16_t value);
     void Log_Write_Data(LogDataID id, uint16_t value);
     void Log_Write_Data(LogDataID id, float value);
-    void Log_Write_GuidedTarget(uint8_t target_type, const Vector3f& pos_target, const Vector3f& vel_target);
+    void Log_Write_GuidedTarget(uint8_t target_type, const Vector3f& pos_target, const Vector3f& vel_target,
+                                Location::AltFrame alt_frame=Location::AltFrame::ABOVE_ORIGIN);
     void Log_Write_Vehicle_Startup_Messages();
 #endif
     void load_parameters(void) override;
@@ -631,6 +632,12 @@ class Sub : public AP_Vehicle {
     float get_rangefinder_target_cm() const WARN_IF_UNUSED { return mode_surftrak.get_rangefinder_target_cm(); }
     bool set_rangefinder_target_cm(float new_target_cm) { return mode_surftrak.set_rangefinder_target_cm(new_target_cm); }
 #endif // AP_RANGEFINDER_ENABLED
+
+    // set target position and velocity, alt frame is Location::AltFrame::ABOVE_ORIGIN
+    bool set_target_posvel_NED(const Vector3f& target_pos, const Vector3f& target_vel) override;
+
+    // set target position and velocity, alt frame is Location::AltFrame::ABOVE_TERRAIN
+    bool set_target_posvel_terrain(const Vector3f& target_pos, const Vector3f& target_vel) override;
 #endif // AP_SCRIPTING_ENABLED
 };
 

ArduSub/mode.h

@@ -326,7 +326,9 @@ class ModeGuided : public Mode
     void guided_set_angle(const Quaternion &q, float climb_rate_cms, bool use_yaw_rate, float yaw_rate_rads);
     void guided_set_angle(const Quaternion&, float);
     void guided_limit_set(uint32_t timeout_ms, float alt_min_cm, float alt_max_cm, float horiz_max_cm);
-    bool guided_set_destination_posvel(const Vector3f& destination, const Vector3f& velocity);
+    void set_posvel_frame_above_home();
+    void set_posvel_frame_above_terrain(float destination_z);
+    bool guided_set_destination_posvel(const Vector3f& destination, const Vector3f& velocity, Location::AltFrame alt_frame=Location::AltFrame::ABOVE_ORIGIN);
     bool guided_set_destination_posvel(const Vector3f& destination, const Vector3f& velocity, bool use_yaw, float yaw_cd, bool use_yaw_rate, float yaw_rate_cds, bool relative_yaw);
     bool guided_set_destination(const Vector3f& destination);
     bool guided_set_destination(const Location&);

ArduSub/mode_guided.cpp

@@ -9,6 +9,7 @@
 
 static Vector3p posvel_pos_target_cm;
 static Vector3f posvel_vel_target_cms;
+static Location::AltFrame posvel_frame;
 static uint32_t update_time_ms;
 
 struct {
@@ -114,6 +115,22 @@ void ModeGuided::guided_vel_control_start()
     set_auto_yaw_mode(AUTO_YAW_HOLD);
 }
 
+// posvel helper function: use above home frame
+void ModeGuided::set_posvel_frame_above_home()
+{
+    posvel_frame = Location::AltFrame::ABOVE_HOME;
+    position_control->set_pos_offset_z_cm(0);
+    position_control->set_pos_offset_target_z_cm(0);
+}
+
+// posvel helper function: use above terrain frame
+void ModeGuided::set_posvel_frame_above_terrain(float destination_z)
+{
+    posvel_frame = Location::AltFrame::ABOVE_TERRAIN;
+    position_control->set_pos_offset_z_cm(sub.rangefinder_state.inertial_alt_cm - destination_z);
+    position_control->set_pos_offset_target_z_cm(sub.rangefinder_state.rangefinder_terrain_offset_cm);
+}
+
 // initialise guided mode's posvel controller
 void ModeGuided::guided_posvel_control_start()
 {
@@ -130,6 +147,9 @@ void ModeGuided::guided_posvel_control_start()
 
     // pilot always controls yaw
     set_auto_yaw_mode(AUTO_YAW_HOLD);
+
+    // default frame is ABOVE_HOME
+    set_posvel_frame_above_home();
 }
 
 // initialise guided mode's angle controller
@@ -291,16 +311,35 @@ void ModeGuided::guided_set_velocity(const Vector3f& velocity, bool use_yaw, flo
 }
 
 // set guided mode posvel target
-bool ModeGuided::guided_set_destination_posvel(const Vector3f& destination, const Vector3f& velocity)
+bool ModeGuided::guided_set_destination_posvel(const Vector3f& destination, const Vector3f& velocity, Location::AltFrame alt_frame)
 {
-    // check we are in velocity control mode
+    // check we are in posvel control mode
     if (sub.guided_mode != Guided_PosVel) {
+        // catch some configuration errors by requiring a healthy rangefinder to specify the ABOVE_TERRAIN frame
+        if (alt_frame == Location::AltFrame::ABOVE_TERRAIN && !sub.rangefinder_alt_ok()) {
+            gcs().send_text(MAV_SEVERITY_WARNING, "Terrain data (rangefinder) not available");
+            return false;
+        }
+
         guided_posvel_control_start();
+
+        if (alt_frame == Location::AltFrame::ABOVE_TERRAIN) {
+            // initialize the terrain offset
+            // destination.z is the rangefinder target
+            set_posvel_frame_above_terrain(destination.z);
+        }
+    } else {
+        if (alt_frame == Location::AltFrame::ABOVE_TERRAIN) {
+            // rangefinder target may have changed
+            position_control->set_pos_offset_z_cm(position_control->get_pos_offset_z_cm() + posvel_pos_target_cm.z - destination.z);
+        } else {
+            set_posvel_frame_above_home();
+        }
     }
 
 #if AP_FENCE_ENABLED
     // reject destination if outside the fence
-    const Location dest_loc(destination, Location::AltFrame::ABOVE_ORIGIN);
+    const Location dest_loc(destination, alt_frame);
     if (!sub.fence.check_destination_within_fence(dest_loc)) {
         LOGGER_WRITE_ERROR(LogErrorSubsystem::NAVIGATION, LogErrorCode::DEST_OUTSIDE_FENCE);
         // failure is propagated to GCS with NAK
@@ -312,14 +351,9 @@ bool ModeGuided::guided_set_destination_posvel(const Vector3f& destination, cons
     posvel_pos_target_cm = destination.topostype();
     posvel_vel_target_cms = velocity;
 
-    position_control->input_pos_vel_accel_xy(posvel_pos_target_cm.xy(), posvel_vel_target_cms.xy(), Vector2f());
-    float dz = posvel_pos_target_cm.z;
-    position_control->input_pos_vel_accel_z(dz, posvel_vel_target_cms.z, 0);
-    posvel_pos_target_cm.z = dz;
-
 #if HAL_LOGGING_ENABLED
     // log target
-    sub.Log_Write_GuidedTarget(sub.guided_mode, destination, velocity);
+    sub.Log_Write_GuidedTarget(sub.guided_mode, destination, velocity, alt_frame);
 #endif
 
     return true;
@@ -634,9 +668,23 @@ void ModeGuided::guided_posvel_control_run()
 
     // send position and velocity targets to position controller
     position_control->input_pos_vel_accel_xy(posvel_pos_target_cm.xy(), posvel_vel_target_cms.xy(), Vector2f());
+#if AP_RANGEFINDER_ENABLED
+    if (posvel_frame == Location::AltFrame::ABOVE_TERRAIN) {
+        if (sub.rangefinder_alt_ok()) {
+            // surftrak: set the offset target to the current terrain altitude estimate
+            position_control->set_pos_offset_target_z_cm(sub.rangefinder_state.rangefinder_terrain_offset_cm);
+        }
+        position_control->set_pos_target_z_from_climb_rate_cm(posvel_vel_target_cms.z);
+    } else {
+        float pz = posvel_pos_target_cm.z;
+        position_control->input_pos_vel_accel_z(pz, posvel_vel_target_cms.z, 0);
+        posvel_pos_target_cm.z = pz;
+    }
+#else
     float pz = posvel_pos_target_cm.z;
     position_control->input_pos_vel_accel_z(pz, posvel_vel_target_cms.z, 0);
     posvel_pos_target_cm.z = pz;
+#endif
 
     // run position controller
     position_control->update_xy_controller();

Tools/autotest/ardusub.py

@@ -12,6 +12,7 @@
 import sys
 
 from pymavlink import mavutil
+from pysim import util
 
 import vehicle_test_suite
 from vehicle_test_suite import NotAchievedException
@@ -440,6 +441,98 @@ def SimTerrainMission(self):
         self.context_pop()
         self.reboot_sitl()  # e.g. revert rangefinder configuration
 
+    def GuidedPosVel(self):
+        """Send SET_POSITION_TARGET_INT msgs at 10Hz with position and velocity targets"""
+
+        # GUIDED mode supports several sub-modes selected by the POSITION_TARGET_TYPE mask
+        # The Guided_PosVel sub-mode supports rapid updates and several altitude frames
+        mask = (mavutil.mavlink.POSITION_TARGET_TYPEMASK_AX_IGNORE |
+                mavutil.mavlink.POSITION_TARGET_TYPEMASK_AY_IGNORE |
+                mavutil.mavlink.POSITION_TARGET_TYPEMASK_AZ_IGNORE |
+                mavutil.mavlink.POSITION_TARGET_TYPEMASK_YAW_IGNORE |
+                mavutil.mavlink.POSITION_TARGET_TYPEMASK_YAW_RATE_IGNORE)
+
+        seafloor_depth = 50
+        speed_cms = 50
+
+        # Generate a synthetic seafloor at -50m
+        self.context_push()
+        self.prepare_synthetic_seafloor_test(seafloor_depth)
+
+        # Guided_PosVel uses WPNAV_SPEED
+        self.set_parameter('WPNAV_SPEED', speed_cms)
+
+        # Dive to -35m
+        start_altitude = -35
+        pwm = 1300 if self.get_altitude(relative=True) > start_altitude else 1700
+        self.wait_ready_to_arm()
+        self.arm_vehicle()
+        self.set_rc(Joystick.Throttle, pwm)
+        self.wait_altitude(altitude_min=start_altitude-1, altitude_max=start_altitude, relative=False, timeout=120)
+        self.set_rc(Joystick.Throttle, 1500)
+        self.delay_sim_time(1)
+
+        # Request GLOBAL_POSITION_INT at 20Hz, this will be our clock
+        self.context_set_message_rate_hz('GLOBAL_POSITION_INT', 10)
+
+        # Run between 2 locations, 30m apart
+        distance_m = 30
+        timeout = distance_m * 100 / speed_cms + 5  # Add a little time to accelerate, etc.
+
+        runs = [{
+            'frame': mavutil.mavlink.MAV_FRAME_GLOBAL_RELATIVE_ALT,
+            'bearing': 180,
+            'target_alt': -35,  # Altitude
+        }, {
+            'frame': mavutil.mavlink.MAV_FRAME_GLOBAL_TERRAIN_ALT,
+            'bearing': 0,
+            'target_alt': 15,  # Distance above seafloor
+        }]
+
+        for run in runs:
+            # Face the direction of travel
+            self.reach_heading_manual(run['bearing'])
+
+            msg = self.mav.recv_match(type='GLOBAL_POSITION_INT', blocking=True)
+            start_loc = (msg.lat * 1e-7, msg.lon * 1e-7)
+            dest_loc = util.gps_newpos(start_loc[0], start_loc[1], run['bearing'], distance_m)
+
+            # Start GUIDED mode
+            self.change_mode('GUIDED')
+
+            # Go!
+            start_time = self.get_sim_time()
+            while msg := self.mav.recv_match(type='GLOBAL_POSITION_INT', blocking=True):
+                current_loc = (msg.lat * 1e-7, msg.lon * 1e-7)
+                distance_remaining = util.gps_distance(
+                    dest_loc[0], dest_loc[1],
+                    current_loc[0], current_loc[1])
+
+                # self.progress('Frame %u, distance remaining %f m' % (run['frame'], distance_remaining))
+
+                if distance_remaining < 0.5:
+                    self.progress('Frame %u reached destination at time %f' %
+                                  (run['frame'], self.get_sim_time_cached() - start_time))
+                    break
+                elif self.get_sim_time_cached() - start_time > timeout:
+                    raise NotAchievedException('Frame %u took too long to reach the destination' % run['frame'])
+
+                # TODO test altitude & rangefinder
+
+                # Set target 10m ahead of current location
+                target_loc = util.gps_newpos(current_loc[0], current_loc[1], run['bearing'], 10)
+
+                self.mav.mav.set_position_target_global_int_send(
+                    0, 1, 1, run['frame'], mask, int(target_loc[0] * 1e7), int(target_loc[1] * 1e7), run['target_alt'],
+                    0, 0, 0, 0, 0, 0, 0, 0)
+
+            # Stop guided mode
+            self.change_mode('MANUAL')
+
+        self.disarm_vehicle()
+        self.context_pop()
+        self.reboot_sitl()  # e.g. revert rangefinder configuration
+
     def ModeChanges(self, delta=0.2):
         """Check if alternating between ALTHOLD, STABILIZE, POSHOLD and SURFTRAK (mode 21) affects altitude"""
         self.wait_ready_to_arm()
@@ -956,6 +1049,7 @@ def tests(self):
             self.Surftrak,
             self.SimTerrainSurftrak,
             self.SimTerrainMission,
+            self.GuidedPosVel,
             self.RngfndQuality,
             self.PositionHold,
             self.ModeChanges,

libraries/AP_Scripting/docs/docs.lua

@@ -2398,6 +2398,12 @@ function vehicle:set_target_posvelaccel_NED(target_pos, target_vel, target_accel
 ---@return boolean
 function vehicle:set_target_posvel_NED(target_pos, target_vel) end
 
+-- Sets the target position and velocity in a guided mode, with ABOVE_TERRAIN frame.
+---@param target_pos Vector3f_ud
+---@param target_vel Vector3f_ud
+---@return boolean
+function vehicle:set_target_posvel_terrain(target_pos, target_vel) end
+
 -- desc
 ---@param target_pos Vector3f_ud
 ---@param use_yaw boolean

libraries/AP_Scripting/generator/description/bindings.desc

@@ -325,6 +325,7 @@ singleton AP_Vehicle method get_target_location boolean Location'Null
 singleton AP_Vehicle method update_target_location boolean Location Location
 singleton AP_Vehicle method set_target_pos_NED boolean Vector3f boolean float -360 +360 boolean float'skip_check boolean boolean
 singleton AP_Vehicle method set_target_posvel_NED boolean Vector3f Vector3f
+singleton AP_Vehicle method set_target_posvel_terrain boolean Vector3f Vector3f
 singleton AP_Vehicle method set_target_posvelaccel_NED boolean Vector3f Vector3f Vector3f boolean float -360 +360 boolean float'skip_check boolean
 singleton AP_Vehicle method set_target_velaccel_NED boolean Vector3f Vector3f boolean float -360 +360 boolean float'skip_check boolean
 singleton AP_Vehicle method set_target_velocity_NED boolean Vector3f

libraries/AP_Vehicle/AP_Vehicle.h

@@ -182,6 +182,7 @@ class AP_Vehicle : public AP_HAL::HAL::Callbacks {
     virtual bool start_takeoff(float alt) { return false; }
     virtual bool set_target_pos_NED(const Vector3f& target_pos, bool use_yaw, float yaw_deg, bool use_yaw_rate, float yaw_rate_degs, bool yaw_relative, bool terrain_alt) { return false; }
     virtual bool set_target_posvel_NED(const Vector3f& target_pos, const Vector3f& target_vel) { return false; }
+    virtual bool set_target_posvel_terrain(const Vector3f& target_pos, const Vector3f& target_vel) { return false; }
     virtual bool set_target_posvelaccel_NED(const Vector3f& target_pos, const Vector3f& target_vel, const Vector3f& target_accel, bool use_yaw, float yaw_deg, bool use_yaw_rate, float yaw_rate_degs, bool yaw_relative) { return false; }
     virtual bool set_target_velocity_NED(const Vector3f& vel_ned) { return false; }
     virtual bool set_target_velaccel_NED(const Vector3f& target_vel, const Vector3f& target_accel, bool use_yaw, float yaw_deg, bool use_yaw_rate, float yaw_rate_degs, bool yaw_relative) { return false; }