AP_Mount: support sysid targetting

libraries/AP_Mount/AP_Mount.cpp

@@ -7,6 +7,7 @@
 #include "AP_Mount_Alexmos.h"
 #include "AP_Mount_SToRM32.h"
 #include "AP_Mount_SToRM32_serial.h"
+#include <AP_Math/location.h>
 
 const AP_Param::GroupInfo AP_Mount::var_info[] = {
     // @Param: _DEFLT_MODE
@@ -596,6 +597,33 @@ MAV_RESULT AP_Mount::handle_command_long(const mavlink_command_long_t &packet)
     }
 }
 
+/// Change the configuration of the mount
+void AP_Mount::handle_global_position_int(const mavlink_message_t &msg)
+{
+    mavlink_global_position_int_t packet;
+    mavlink_msg_global_position_int_decode(&msg, &packet);
+
+    if (!check_latlng(packet.lat, packet.lon)) {
+        return;
+    }
+
+    for (uint8_t instance=0; instance<AP_MOUNT_MAX_INSTANCES; instance++) {
+        if (_backends[instance] == nullptr) {
+            continue;
+        }
+        struct mount_state &_state = state[instance];
+        if (_state._target_sysid != msg.sysid) {
+            continue;
+        }
+        _state._target_sysid_location.lat = packet.lat;
+        _state._target_sysid_location.lng = packet.lon;
+        // global_position_int.alt is *UP*, so is location.
+        _state._target_sysid_location.set_alt_cm(packet.alt*0.1,
+                                                 Location::AltFrame::ABSOLUTE);
+        _state._target_sysid_location_set = true;
+    }
+}
+
 /// Change the configuration of the mount
 void AP_Mount::handle_mount_configure(const mavlink_message_t &msg)
 {
@@ -635,6 +663,15 @@ void AP_Mount::send_mount_status(mavlink_channel_t chan)
     }
 }
 
+// point at system ID sysid
+void AP_Mount::set_target_sysid(uint8_t instance, uint8_t sysid)
+{
+    // call instance's set_roi_cmd
+    if (instance < AP_MOUNT_MAX_INSTANCES && _backends[instance] != nullptr) {
+        _backends[instance]->set_target_sysid(sysid);
+    }
+}
+
 // set_roi_target - sets target location that mount should attempt to point towards
 void AP_Mount::set_roi_target(uint8_t instance, const struct Location &target_loc)
 {
@@ -666,6 +703,9 @@ void AP_Mount::handle_message(mavlink_channel_t chan, const mavlink_message_t &m
     case MAVLINK_MSG_ID_MOUNT_CONTROL:
         handle_mount_control(msg);
         break;
+    case MAVLINK_MSG_ID_GLOBAL_POSITION_INT:
+        handle_global_position_int(msg);
+        break;
     default:
 #if CONFIG_HAL_BOARD == HAL_BOARD_SITL
         AP_HAL::panic("Unhandled mount case");

libraries/AP_Mount/AP_Mount.h

@@ -111,6 +111,10 @@ public:
     void set_roi_target(const struct Location &target_loc) { set_roi_target(_primary,target_loc); }
     void set_roi_target(uint8_t instance, const struct Location &target_loc);
 
+    // point at system ID sysid
+    void set_target_sysid(uint8_t instance, const uint8_t sysid);
+    void set_target_sysid(const uint8_t sysid) { set_target_sysid(_primary, sysid); }
+
     // mavlink message handling:
     MAV_RESULT handle_command_long(const mavlink_command_long_t &packet);
     void handle_param_value(const mavlink_message_t &msg);
@@ -168,6 +172,11 @@ protected:
         MAV_MOUNT_MODE  _mode;              // current mode (see MAV_MOUNT_MODE enum)
         struct Location _roi_target;        // roi target location
         uint32_t _roi_target_set_ms;
+
+        uint8_t _target_sysid;           // sysid to track
+        Location _target_sysid_location; // sysid target location
+        bool _target_sysid_location_set;
+
     } state[AP_MOUNT_MAX_INSTANCES];
 
 private:
@@ -178,7 +187,7 @@ private:
 
     MAV_RESULT handle_command_do_mount_configure(const mavlink_command_long_t &packet);
     MAV_RESULT handle_command_do_mount_control(const mavlink_command_long_t &packet);
-
+    void handle_global_position_int(const mavlink_message_t &msg);
 };
 
 namespace AP {

libraries/AP_Mount/AP_Mount_Alexmos.cpp

@@ -57,6 +57,14 @@ void AP_Mount_Alexmos::update()
             }
             break;
 
+        case MAV_MOUNT_MODE_SYSID_TARGET:
+            if (calc_angle_to_sysid_target(_angle_ef_target_rad,
+                                           true,
+                                           false)) {
+                control_axis(_angle_ef_target_rad, false);
+            }
+            break;
+
         default:
             // we do not know this mode so do nothing
             break;

libraries/AP_Mount/AP_Mount_Backend.cpp

@@ -26,6 +26,15 @@ void AP_Mount_Backend::set_roi_target(const struct Location &target_loc)
     _frontend.set_mode(_instance, MAV_MOUNT_MODE_GPS_POINT);
 }
 
+// set_sys_target - sets system that mount should attempt to point towards
+void AP_Mount_Backend::set_target_sysid(uint8_t sysid)
+{
+    _state._target_sysid = sysid;
+
+    // set the mode to sysid tracking mode
+    _frontend.set_mode(_instance, MAV_MOUNT_MODE_SYSID_TARGET);
+}
+
 // process MOUNT_CONFIGURE messages received from GCS.  deprecated.
 void AP_Mount_Backend::handle_mount_configure(const mavlink_mount_configure_t &packet)
 {
@@ -135,7 +144,7 @@ float AP_Mount_Backend::angle_input_rad(const RC_Channel* rc, int16_t angle_min,
 bool AP_Mount_Backend::calc_angle_to_roi_target(Vector3f& angles_to_target_rad,
                                                 bool calc_tilt,
                                                 bool calc_pan,
-                                                bool relative_pan)
+                                                bool relative_pan) const
 {
     if (_state._roi_target_set_ms == 0) {
         return false;
@@ -143,8 +152,26 @@ bool AP_Mount_Backend::calc_angle_to_roi_target(Vector3f& angles_to_target_rad,
     return calc_angle_to_location(_state._roi_target, angles_to_target_rad, calc_tilt, calc_pan, relative_pan);
 }
 
+bool AP_Mount_Backend::calc_angle_to_sysid_target(Vector3f& angles_to_target_rad,
+                                                  bool calc_tilt,
+                                                  bool calc_pan,
+                                                  bool relative_pan) const
+{
+    if (!_state._target_sysid_location_set) {
+        return false;
+    }
+    if (!_state._target_sysid) {
+        return false;
+    }
+    return calc_angle_to_location(_state._target_sysid_location,
+                                  angles_to_target_rad,
+                                  calc_tilt,
+                                  calc_pan,
+                                  relative_pan);
+}
+
 // calc_angle_to_location - calculates the earth-frame roll, tilt and pan angles (and radians) to point at the given target
-bool AP_Mount_Backend::calc_angle_to_location(const struct Location &target, Vector3f& angles_to_target_rad, bool calc_tilt, bool calc_pan, bool relative_pan)
+bool AP_Mount_Backend::calc_angle_to_location(const struct Location &target, Vector3f& angles_to_target_rad, bool calc_tilt, bool calc_pan, bool relative_pan) const
 {
     Location current_loc;
     if (!AP::ahrs().get_position(current_loc)) {

libraries/AP_Mount/AP_Mount_Backend.h

@@ -57,6 +57,9 @@ public:
     // set_roi_target - sets target location that mount should attempt to point towards
     void set_roi_target(const struct Location &target_loc);
 
+    // set_sys_target - sets system that mount should attempt to point towards
+    void set_target_sysid(uint8_t sysid);
+
     // control - control the mount
     virtual void control(int32_t pitch_or_lat, int32_t roll_or_lon, int32_t yaw_or_alt, MAV_MOUNT_MODE mount_mode);
     
@@ -92,14 +95,23 @@ protected:
                                 Vector3f& angles_to_target_rad,
                                 bool calc_tilt,
                                 bool calc_pan,
-                                bool relative_pan = true) WARN_IF_UNUSED;
+                                bool relative_pan = true) const WARN_IF_UNUSED;
+
     // calc_angle_to_roi_target - calculates the earth-frame roll, tilt
     // and pan angles (and radians) to point at the ROI-target (as set
     // by various mavlink messages)
     bool calc_angle_to_roi_target(Vector3f& angles_to_target_rad,
                                   bool calc_tilt,
                                   bool calc_pan,
-                                  bool relative_pan = true) WARN_IF_UNUSED;
+                                  bool relative_pan = true) const WARN_IF_UNUSED;
+
+    // calc_angle_to_sysid_target - calculates the earth-frame roll, tilt
+    // and pan angles (and radians) to point at the sysid-target (as set
+    // by various mavlink messages)
+    bool calc_angle_to_sysid_target(Vector3f& angles_to_target_rad,
+                                    bool calc_tilt,
+                                    bool calc_pan,
+                                    bool relative_pan = true) const WARN_IF_UNUSED;
 
     // get the mount mode from frontend
     MAV_MOUNT_MODE get_mode(void) const { return _frontend.get_mode(_instance); }

libraries/AP_Mount/AP_Mount_SToRM32.cpp

@@ -64,6 +64,12 @@ void AP_Mount_SToRM32::update()
             }
             break;
 
+        case MAV_MOUNT_MODE_SYSID_TARGET:
+            if (calc_angle_to_sysid_target(_angle_ef_target_rad, true, true)) {
+                resend_now = true;
+            }
+            break;
+
         default:
             // we do not know this mode so do nothing
             break;

libraries/AP_Mount/AP_Mount_SToRM32_serial.cpp

@@ -80,6 +80,14 @@ void AP_Mount_SToRM32_serial::update()
             }
             break;
 
+        case MAV_MOUNT_MODE_SYSID_TARGET:
+            if (calc_angle_to_sysid_target(_angle_ef_target_rad,
+                                           true,
+                                           true)) {
+                resend_now = true;
+            }
+            break;
+
         default:
             // we do not know this mode so do nothing
             break;

libraries/AP_Mount/AP_Mount_Servo.cpp

@@ -76,6 +76,15 @@ void AP_Mount_Servo::update()
             break;
         }
 
+        case MAV_MOUNT_MODE_SYSID_TARGET:
+            if (calc_angle_to_sysid_target(_angle_ef_target_rad,
+                                           _flags.tilt_control,
+                                           _flags.pan_control,
+                                           false)) {
+                stabilize();
+            }
+            break;
+
         default:
             //do nothing
             break;

libraries/AP_Mount/AP_Mount_SoloGimbal.cpp

@@ -74,6 +74,11 @@ void AP_Mount_SoloGimbal::update()
             }
             break;
 
+        case MAV_MOUNT_MODE_SYSID_TARGET:
+            if (calc_angle_to_sysid_target(_angle_ef_target_rad, true, true)) {
+            }
+            break;
+
         default:
             // we do not know this mode so do nothing
             break;