Sub: Add attitude control with althold via mavlink

Allow ordinary attitude positions without a gps system

Signed-off-by: Patrick José Pereira <patrickelectric@gmail.com>

ArduSub/GCS_Mavlink.cpp

@@ -1006,6 +1006,12 @@ void GCS_MAVLINK_Sub::handleMessage(mavlink_message_t* msg)
         mavlink_set_attitude_target_t packet;
         mavlink_msg_set_attitude_target_decode(msg, &packet);
 
+        // ensure type_mask specifies to use attitude
+        // the thrust can be used from the altitude hold
+        if (packet.type_mask & (1<<6)) {
+            sub.set_attitude_target_no_gps = {AP_HAL::millis(), packet};
+        }
+
         // ensure type_mask specifies to use attitude and thrust
         if ((packet.type_mask & ((1<<7)|(1<<6))) != 0) {
             break;

ArduSub/Sub.h

@@ -436,6 +436,14 @@ private:
     AP_Terrain terrain{ahrs, mission, rally};
 #endif
 
+    // used to allow attitude and depth control without a position system
+    struct attitude_no_gps_struct {
+        uint32_t last_message_ms;
+        mavlink_set_attitude_target_t packet;
+    };
+
+    attitude_no_gps_struct set_attitude_target_no_gps {0};
+
     // Top-level logic
     // setup the var_info table
     AP_Param param_loader;

ArduSub/control_althold.cpp

@@ -49,7 +49,27 @@ void Sub::althold_run()
 
     // get pilot desired lean angles
     float target_roll, target_pitch;
-    get_pilot_desired_lean_angles(channel_roll->get_control_in(), channel_pitch->get_control_in(), target_roll, target_pitch, attitude_control.get_althold_lean_angle_max());
+
+    // Check if set_attitude_target_no_gps is valid
+    if (tnow - sub.set_attitude_target_no_gps.last_message_ms < 5000) {
+        float target_yaw;
+        Quaternion(
+            set_attitude_target_no_gps.packet.q
+        ).to_euler(
+            target_roll,
+            target_pitch,
+            target_yaw
+        );
+        target_roll = degrees(target_roll);
+        target_pitch = degrees(target_pitch);
+        target_yaw = degrees(target_yaw);
+
+        attitude_control.input_euler_angle_roll_pitch_yaw(target_roll * 1e2f, target_pitch * 1e2f, target_yaw * 1e2f, true);
+        return;
+
+    } else {
+        get_pilot_desired_lean_angles(channel_roll->get_control_in(), channel_pitch->get_control_in(), target_roll, target_pitch, attitude_control.get_althold_lean_angle_max());
+    }
 
     // get pilot's desired yaw rate
     float target_yaw_rate = get_pilot_desired_yaw_rate(channel_yaw->get_control_in());