ArduSub: add acceleration target to Guided_PosVel

ArduSub/GCS_MAVLink_Sub.cpp

@@ -103,9 +103,9 @@ bool GCS_MAVLINK_Sub::get_target_location(Location &loc) const
         switch (sub.guided_mode) {
         case Guided_WP:
             break;
-        case Guided_PosVel: {
+        case Guided_PosVelAccel: {
             Vector3f pos_neu_cm;
-            if (!sub.mode_guided.get_posvel_target_NEU_cm(pos_neu_cm)) {
+            if (!sub.mode_guided.get_posvelaccel_target_NEU_cm(pos_neu_cm)) {
                 return false;
             }
             loc = Location(pos_neu_cm, Location::AltFrame::ABOVE_ORIGIN);
@@ -675,6 +675,17 @@ void GCS_MAVLINK_Sub::handle_message(const mavlink_message_t &msg)
             }
         }
 
+        // prepare acceleration
+        Vector3f acc_vector_neu_cmss;
+        if (!acc_ignore) {
+            // convert to cm/s/s
+            acc_vector_neu_cmss = Vector3f(packet.afx * 100.0f, packet.afy * 100.0f, -packet.afz * 100.0f);
+            // rotate from body-frame if necessary
+            if (packet.coordinate_frame == MAV_FRAME_BODY_NED || packet.coordinate_frame == MAV_FRAME_BODY_FRD || packet.coordinate_frame == MAV_FRAME_BODY_OFFSET_NED) {
+                sub.rotate_body_frame_to_NE(acc_vector_neu_cmss.x, acc_vector_neu_cmss.y);
+            }
+        }
+
         // prepare yaw
         float yaw_cd =  0.0f;
         bool yaw_relative = false;
@@ -688,8 +699,8 @@ void GCS_MAVLINK_Sub::handle_message(const mavlink_message_t &msg)
         }
 
         // send request
-        if (!pos_ignore && !vel_ignore && acc_ignore) {
-            sub.mode_guided.guided_set_destination_posvel(pos_vector_neu_cm, vel_vector_neu_cms, !yaw_ignore, yaw_cd, !yaw_rate_ignore, yaw_rate_cds, yaw_relative);
+        if (!pos_ignore && !vel_ignore) {
+            sub.mode_guided.guided_set_posvelaccel(pos_vector_neu_cm, vel_vector_neu_cms, acc_vector_neu_cmss, !yaw_ignore, yaw_cd, !yaw_rate_ignore, yaw_rate_cds, yaw_relative);
         } else if (pos_ignore && !vel_ignore && acc_ignore) {
             sub.mode_guided.guided_set_velocity(vel_vector_neu_cms, !yaw_ignore, yaw_cd, !yaw_rate_ignore, yaw_rate_cds, yaw_relative);
         } else if (!pos_ignore && vel_ignore && acc_ignore) {
@@ -747,12 +758,17 @@ void GCS_MAVLINK_Sub::handle_message(const mavlink_message_t &msg)
             };
         }
 
-        if (!pos_ignore && !vel_ignore && acc_ignore) {
+        Vector3f acc_vector_neu_cmss;
+        if (!acc_ignore) {
+            acc_vector_neu_cmss = Vector3f(packet.afx * 100.0f, packet.afy * 100.0f, -packet.afz * 100.0f);
+        }
+
+        if (!pos_ignore && !vel_ignore) {
             Vector3f pos_neu_cm;
             if (!loc.get_vector_from_origin_NEU_cm(pos_neu_cm)) {
                 break;
             }
-            sub.mode_guided.guided_set_destination_posvel(pos_neu_cm, Vector3f(packet.vx * 100.0f, packet.vy * 100.0f, -packet.vz * 100.0f));
+            sub.mode_guided.guided_set_posvelaccel(pos_neu_cm, Vector3f(packet.vx * 100.0f, packet.vy * 100.0f, -packet.vz * 100.0f), acc_vector_neu_cmss);
         } else if (pos_ignore && !vel_ignore && acc_ignore) {
             sub.mode_guided.guided_set_velocity(Vector3f(packet.vx * 100.0f, packet.vy * 100.0f, -packet.vz * 100.0f));
         } else if (!pos_ignore && vel_ignore && acc_ignore) {

ArduSub/Log.cpp

@@ -182,21 +182,27 @@ struct PACKED log_GuidedTarget {
     float vel_target_x;
     float vel_target_y;
     float vel_target_z;
+    float acc_target_x;
+    float acc_target_y;
+    float acc_target_z;
 };
 
 // Write a Guided mode target
-void Sub::Log_Write_GuidedTarget(uint8_t target_type, const Vector3f& pos_target_neu_cm, const Vector3f& vel_target_neu_cms)
+void Sub::Log_Write_GuidedTarget(uint8_t target_type, const Vector3f& pos_target_neu_cm, const Vector3f& vel_target_neu_cms, const Vector3f& acc_target_neu_cmss)
 {
     struct log_GuidedTarget pkt = {
         LOG_PACKET_HEADER_INIT(LOG_GUIDEDTARGET_MSG),
         time_us         : AP_HAL::micros64(),
         type            : target_type,
-        pos_target_x    : pos_target_neu_cm.x,
-        pos_target_y    : pos_target_neu_cm.y,
-        pos_target_z    : pos_target_neu_cm.z,
-        vel_target_x    : vel_target_neu_cms.x,
-        vel_target_y    : vel_target_neu_cms.y,
-        vel_target_z    : vel_target_neu_cms.z
+        pos_target_x    : pos_target_neu_cm.x * 0.01f,
+        pos_target_y    : pos_target_neu_cm.y * 0.01f,
+        pos_target_z    : pos_target_neu_cm.z * 0.01f,
+        vel_target_x    : vel_target_neu_cms.x * 0.01f,
+        vel_target_y    : vel_target_neu_cms.y * 0.01f,
+        vel_target_z    : vel_target_neu_cms.z * 0.01f,
+        acc_target_x    : acc_target_neu_cmss.x * 0.01f,
+        acc_target_y    : acc_target_neu_cmss.y * 0.01f,
+        acc_target_z    : acc_target_neu_cmss.z * 0.01f
     };
     logger.WriteBlock(&pkt, sizeof(pkt));
 }
@@ -257,6 +263,9 @@ void Sub::Log_Write_GuidedTarget(uint8_t target_type, const Vector3f& pos_target
 // @Field: vX: Target velocity, X-Axis
 // @Field: vY: Target velocity, Y-Axis
 // @Field: vZ: Target velocity, Z-Axis
+// @Field: aX: Target acceleration, X-Axis
+// @Field: aY: Target acceleration, Y-Axis
+// @Field: aZ: Target acceleration, Z-Axis
 
 // type and unit information can be found in
 // libraries/AP_Logger/Logstructure.h; search for "log_Units" for
@@ -276,7 +285,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",  "QBfffffffff",    "TimeUS,Type,pX,pY,pZ,vX,vY,vZ,aX,aY,aZ", "s-mmmnnnooo", "F-000000000" },
 };
 
 uint8_t Sub::get_num_log_structures() const

ArduSub/Sub.cpp

@@ -465,6 +465,23 @@ void Sub::rc_loop()
 }
 #endif
 
+#if AP_SCRIPTING_ENABLED
+// set target position, velocity and acceleration (for use by scripting)
+bool Sub::set_target_posvelaccel_NED(const Vector3f& target_pos_ned_m, const Vector3f& target_vel_ned_ms, const Vector3f& target_accel_ned_mss, bool use_yaw, float yaw_deg, bool use_yaw_rate, float yaw_rate_degs, bool yaw_relative)
+{
+    // exit if vehicle is not in Guided mode or Auto-Guided mode
+    if ((control_mode != Mode::Number::GUIDED) && !(control_mode == Mode::Number::AUTO && auto_mode == Auto_NavGuided)) {
+        return false;
+    }
+
+    const Vector3f pos_neu_cm(target_pos_ned_m.x * 100.0f, target_pos_ned_m.y * 100.0f, -target_pos_ned_m.z * 100.0f);
+    const Vector3f vel_neu_cms(target_vel_ned_ms.x * 100.0f, target_vel_ned_ms.y * 100.0f, -target_vel_ned_ms.z * 100.0f);
+    const Vector3f accel_neu_cmss(target_accel_ned_mss.x * 100.0f, target_accel_ned_mss.y * 100.0f, -target_accel_ned_mss.z * 100.0f);
+
+    return mode_guided.guided_set_posvelaccel(pos_neu_cm, vel_neu_cms, accel_neu_cmss, use_yaw, yaw_deg * 100.0f, use_yaw_rate, yaw_rate_degs * 100.0f, yaw_relative);
+}
+#endif
+
 Sub *Sub::_singleton = nullptr;
 
 Sub sub;

ArduSub/Sub.h

@@ -417,7 +417,7 @@ private:
     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_neu_cm, const Vector3f& vel_target_neu_cms);
+    void Log_Write_GuidedTarget(uint8_t target_type, const Vector3f& pos_target_neu_cm, const Vector3f& vel_target_neu_cms, const Vector3f& acc_target_neu_cmss);
     void Log_Write_Vehicle_Startup_Messages();
 #endif
     void load_parameters(void) override;
@@ -642,6 +642,9 @@ public:
     // For Lua scripting, so index is 1..4, not 0..3
     uint8_t get_and_clear_button_count(uint8_t index);
 
+    // Set targets in GUIDED mode
+    bool set_target_posvelaccel_NED(const Vector3f& target_pos_ned_m, const Vector3f& target_vel_ned_ms, const Vector3f& target_accel_ned_mss, bool use_yaw, float yaw_deg, bool use_yaw_rate, float yaw_rate_degs, bool yaw_relative) override;
+
 #if AP_RANGEFINDER_ENABLED
     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); }

ArduSub/mode.h

@@ -10,7 +10,7 @@ class GCS_Sub;
 enum GuidedSubMode {
     Guided_WP,
     Guided_Velocity,
-    Guided_PosVel,
+    Guided_PosVelAccel,
     Guided_Angle,
 };
 
@@ -266,17 +266,17 @@ public:
     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_neu_cm, const Vector3f& velocity_neu_cms);
-    bool guided_set_destination_posvel(const Vector3f& destination_neu_cm, const Vector3f& velocity_neu_cms, bool use_yaw, float yaw_cd, bool use_yaw_rate, float yaw_rate_cds, bool relative_yaw);
+    bool guided_set_posvelaccel(const Vector3f& destination_neu_cm, const Vector3f& velocity_neu_cms, const Vector3f& accel_neu_cmss);
+    bool guided_set_posvelaccel(const Vector3f& destination_neu_cm, const Vector3f& velocity_neu_cms, const Vector3f& accel_neu_cmss, bool use_yaw, float yaw_cd, bool use_yaw_rate, float yaw_rate_cds, bool relative_yaw);
     bool guided_set_destination(const Location&);
     bool guided_set_destination(const Vector3f& destination_neu_cm, bool use_yaw, float yaw_cd, bool use_yaw_rate, float yaw_rate_cds, bool relative_yaw);
     void guided_set_velocity(const Vector3f& velocity_neu_cms);
     void guided_set_velocity(const Vector3f& velocity_neu_cms, bool use_yaw, float yaw_cd, bool use_yaw_rate, float yaw_rate_cds, bool relative_yaw);
     void guided_set_yaw_state(bool use_yaw, float yaw_cd, bool use_yaw_rate, float yaw_rate_cds, bool relative_angle);
-    // fills pos with the current Guided_PosVel position target (NEU cm
-    // relative to EKF origin). returns false if not in Guided_PosVel.
+    // fills pos with the current Guided_PosVelAccel position target (NEU cm
+    // relative to EKF origin). returns false if not in Guided_PosVelAccel.
     // velocity target not exposed; base GCS sends 0 with VX/VY/VZ_IGNORE.
-    bool get_posvel_target_NEU_cm(Vector3f &pos) const;
+    bool get_posvelaccel_target_NEU_cm(Vector3f &pos) const;
     float get_auto_heading();
     void guided_limit_clear();
     void set_auto_yaw_mode(autopilot_yaw_mode yaw_mode);
@@ -292,12 +292,12 @@ protected:
 private:
     void guided_pos_control_run();
     void guided_vel_control_run();
-    void guided_posvel_control_run();
+    void guided_posvelaccel_control_run();
     void guided_angle_control_run();
     void guided_takeoff_run();
     void guided_pos_control_start();
     void guided_vel_control_start();
-    void guided_posvel_control_start();
+    void guided_posvelaccel_control_start();
     void guided_angle_control_start();
 };