Sub: implement set_target_angle_and_climbrate for scripting

Author: Williangalvani

ArduSub/Sub.cpp

@@ -218,6 +218,34 @@ void Sub::update_batt_compass()
     }
 }
 
+#if AP_SCRIPTING_ENABLED
+bool Sub::set_target_velocity_NED(const Vector3f& target_vel_ned_ms)
+{
+    // exit if vehicle is not in Guided mode or Auto-Guided mode
+    if (!flightmode->in_guided_mode()) {
+        return false;
+    }
+
+    const Vector3f vel_neu_ms{target_vel_ned_ms.x, target_vel_ned_ms.y, -target_vel_ned_ms.z};
+    mode_guided.set_vel_NEU_ms(vel_neu_ms);
+    return true;
+}
+
+// set target roll pitch and yaw angles with throttle (for use by scripting)
+bool Sub::set_target_angle_and_climbrate(float roll_deg, float pitch_deg, float yaw_deg, float climb_rate_ms, bool use_yaw_rate, float yaw_rate_degs)
+{
+    // exit if vehicle is not in Guided mode or Auto-Guided mode
+    if (!flightmode->in_guided_mode()) {
+        return false;
+    }
+
+    Quaternion q;
+    q.from_euler(radians(roll_deg), radians(pitch_deg), radians(yaw_deg));
+
+    mode_guided.guided_set_angle(q, climb_rate_ms * 100);
+    return true;
+}
+#endif // AP_SCRIPTING_ENABLED
 #if HAL_LOGGING_ENABLED
 // ten_hz_logging_loop
 // should be run at 10hz

ArduSub/Sub.h

@@ -650,6 +650,9 @@ class Sub : public AP_Vehicle {
     // For Lua scripting, so index is 1..4, not 0..3
     uint8_t get_and_clear_button_count(uint8_t index);
 
+    bool set_target_velocity_NED(const Vector3f& vel_ned) override;
+    bool set_target_angle_and_climbrate(float roll_deg, float pitch_deg, float yaw_deg, float climb_rate_ms, bool use_yaw_rate, float yaw_rate_degs) 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

@@ -278,6 +278,7 @@ class ModeGuided : public Mode
     float get_auto_heading();
     void guided_limit_clear();
     void set_auto_yaw_mode(autopilot_yaw_mode yaw_mode);
+    void set_vel_NEU_ms(const Vector3f& vel_neu_ms);
 
 protected:
 

ArduSub/mode_guided.cpp

@@ -17,7 +17,9 @@ struct {
     float pitch_cd;
     float yaw_cd;
     float climb_rate_cms;
-} static guided_angle_state = {0,0.0f, 0.0f, 0.0f, 0.0f};
+    float speed_forward_cms;
+    float speed_lateral_cms;
+} static guided_angle_state = {0, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f};
 
 struct Guided_Limit {
     uint32_t timeout_ms;  // timeout (in seconds) from the time that guided is invoked
@@ -717,6 +719,8 @@ void ModeGuided::guided_angle_control_run()
     // call attitude controller
     attitude_control->input_euler_angle_roll_pitch_yaw_cd(roll_in, pitch_in, yaw_in, true);
 
+    motors.set_lateral(guided_angle_state.speed_lateral_cms * 0.01);
+    motors.set_forward(guided_angle_state.speed_forward_cms * 0.01);
     // call position controller
     position_control->set_pos_target_U_from_climb_rate_cm(climb_rate_cms);
     position_control->update_U_controller();
@@ -733,6 +737,12 @@ void ModeGuided::guided_limit_clear()
     guided_limit.horiz_max_cm = 0.0f;
 }
 
+void ModeGuided::set_vel_NEU_ms(const Vector3f& vel_neu_ms) {
+    guided_angle_state.speed_forward_cms = vel_neu_ms.x * 100.0f;
+    guided_angle_state.speed_lateral_cms = vel_neu_ms.y * 100.0f;
+    guided_angle_state.climb_rate_cms = vel_neu_ms.z * 100.0f;
+    guided_angle_state.update_time_ms = AP_HAL::millis();
+}
 
 // set_auto_yaw_mode - sets the yaw mode for auto
 void ModeGuided::set_auto_yaw_mode(autopilot_yaw_mode yaw_mode)