Sub: implement set_target_posvel methods

ArduPilot · Aug 30, 2024 · d91a3a7 · d91a3a7
1 parent 1394311
commit d91a3a7
Show file tree

Hide file tree

Showing 2 changed files with 37 additions and 0 deletions.
diff --git a/ArduSub/Sub.cpp b/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;

diff --git a/ArduSub/Sub.h b/ArduSub/Sub.h
@@ -632,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
 };