Tracker: add ModeGuided

AntennaTracker/GCS_Mavlink.cpp

@@ -41,6 +41,7 @@ MAV_MODE GCS_MAVLINK_Tracker::base_mode() const
     case Mode::Number::SCAN:
     case Mode::Number::SERVOTEST:
     case Mode::Number::AUTO:
+    case Mode::Number::GUIDED:
         _base_mode |= MAV_MODE_FLAG_GUIDED_ENABLED |
             MAV_MODE_FLAG_STABILIZE_ENABLED;
         // note that MAV_MODE_FLAG_AUTO_ENABLED does not match what
@@ -91,6 +92,46 @@ void GCS_MAVLINK_Tracker::send_nav_controller_output() const
         0);
 }
 
+void GCS_MAVLINK_Tracker::handle_set_attitude_target(const mavlink_message_t &msg)
+{
+    // decode packet
+    mavlink_set_attitude_target_t packet;
+    mavlink_msg_set_attitude_target_decode(&msg, &packet);
+
+    // exit if vehicle is not in Guided mode
+    if (tracker.mode != &tracker.mode_guided) {
+        return;
+    }
+
+    // sanity checks:
+    if (!is_zero(packet.body_roll_rate)) {
+        return;
+    }
+    if (!(packet.type_mask & (1<<0))) {
+        // not told to ignore body roll rate
+        return;
+    }
+    if (!(packet.type_mask & (1<<6))) {
+        // not told to ignore throttle
+        return;
+    }
+    if (packet.type_mask & (1<<7)) {
+        // told to ignore attitude (we don't allow continuous motion yet)
+        return;
+    }
+    if ((packet.type_mask & (1<<3)) && (packet.type_mask&(1<<4))) {
+        // told to ignore both pitch and yaw rates - nothing to do?!
+        return;
+    }
+
+    const bool use_yaw_rate = !(packet.type_mask & (1<<2));
+
+    tracker.mode_guided.set_angle(
+        Quaternion(packet.q[0],packet.q[1],packet.q[2],packet.q[3]),
+        use_yaw_rate,
+        packet.body_yaw_rate);
+}
+
 /*
   send PID tuning message
  */
@@ -428,6 +469,10 @@ void GCS_MAVLINK_Tracker::handleMessage(const mavlink_message_t &msg)
 {
     switch (msg.msgid) {
 
+    case MAVLINK_MSG_ID_SET_ATTITUDE_TARGET:
+        handle_set_attitude_target(msg);
+        break;
+
     // When mavproxy 'wp sethome' 
     case MAVLINK_MSG_ID_MISSION_WRITE_PARTIAL_LIST:
     {

AntennaTracker/GCS_Mavlink.h

@@ -38,6 +38,8 @@ private:
     void handleMessage(const mavlink_message_t &msg) override;
     bool handle_guided_request(AP_Mission::Mission_Command &cmd) override;
     void handle_change_alt_request(AP_Mission::Mission_Command &cmd) override;
+    void handle_set_attitude_target(const mavlink_message_t &msg);
+
     void send_global_position_int() override;
 
     MAV_MODE base_mode() const override;

AntennaTracker/Tracker.cpp

@@ -143,6 +143,9 @@ Mode *Tracker::mode_from_mode_num(const Mode::Number num)
     case Mode::Number::SCAN:
         ret = &mode_scan;
         break;
+    case Mode::Number::GUIDED:
+        ret = &mode_guided;
+        break;
     case Mode::Number::SERVOTEST:
         ret = &mode_servotest;
         break;

AntennaTracker/Tracker.h

@@ -83,6 +83,7 @@ public:
     friend class GCS_Tracker;
     friend class Parameters;
     friend class ModeAuto;
+    friend class ModeGuided;
     friend class Mode;
 
     Tracker(void);
@@ -146,6 +147,7 @@ private:
     ModeAuto mode_auto;
     ModeInitialising mode_initialising;
     ModeManual mode_manual;
+    ModeGuided mode_guided;
     ModeScan mode_scan;
     ModeServoTest mode_servotest;
     ModeStop mode_stop;

AntennaTracker/mode.h

@@ -1,6 +1,7 @@
 #pragma once
 
 #include <stdint.h>
+#include <AP_Math/AP_Math.h>
 
 class Mode {
 public:
@@ -9,6 +10,7 @@ public:
         STOP=1,
         SCAN=2,
         SERVOTEST=3,
+        GUIDED=4,
         AUTO=10,
         INITIALISING=16
     };
@@ -44,6 +46,24 @@ public:
     void update() override;
 };
 
+class ModeGuided : public Mode {
+public:
+    Mode::Number number() const override { return Mode::Number::GUIDED; }
+    bool requires_armed_servos() const override { return true; }
+    void update() override;
+
+    void set_angle(const Quaternion &target_att, bool use_yaw_rate, float yaw_rate_rads) {
+        _target_att = target_att;
+        _use_yaw_rate = use_yaw_rate;
+        _yaw_rate_rads = yaw_rate_rads;
+    }
+
+private:
+    Quaternion _target_att;
+    bool _use_yaw_rate;
+    float _yaw_rate_rads;
+};
+
 class ModeInitialising : public Mode {
 public:
     Mode::Number number() const override { return Mode::Number::INITIALISING; }

AntennaTracker/mode_guided.cpp

@@ -0,0 +1,22 @@
+#include "mode.h"
+
+#include "Tracker.h"
+#include <GCS_MAVLink/GCS.h>
+
+void ModeGuided::update()
+{
+    static uint32_t last_debug;
+    const uint32_t now = AP_HAL::millis();
+    float target_roll, target_pitch, target_yaw;
+    _target_att.to_euler(target_roll, target_pitch, target_yaw);
+    if (now - last_debug > 5000) {
+        last_debug = now;
+        gcs().send_text(MAV_SEVERITY_INFO, "target_yaw=%f target_pitch=%f", degrees(target_yaw), degrees(target_pitch));
+    }
+    calc_angle_error(degrees(target_pitch)*100, degrees(target_yaw)*100, false);
+    float bf_pitch;
+    float bf_yaw;
+    convert_ef_to_bf(target_pitch, target_yaw, bf_pitch, bf_yaw);
+    tracker.update_pitch_servo(bf_pitch);
+    tracker.update_yaw_servo(bf_yaw);
+}

AntennaTracker/system.cpp

@@ -243,6 +243,9 @@ bool Tracker::set_mode(const uint8_t new_mode, const ModeReason reason)
     case Mode::Number::STOP:
         fred = &mode_stop;
         break;
+    case Mode::Number::GUIDED:
+        fred = &mode_guided;
+        break;
     }
     if (fred == nullptr) {
         return false;