AP_Mount: SToRM32 restructure and support for ef/bf angle and rate

libraries/AP_Mount/AP_Mount_SToRM32.cpp

@@ -29,59 +29,66 @@ void AP_Mount_SToRM32::update()
     // update based on mount mode
     switch(get_mode()) {
         // move mount to a "retracted" position.  To-Do: remove support and replace with a relaxed mode?
-        case MAV_MOUNT_MODE_RETRACT:
-            {
+        case MAV_MOUNT_MODE_RETRACT: {
             const Vector3f &target = _state._retract_angles.get();
-            _angle_ef_target_rad.x = ToRad(target.x);
-            _angle_ef_target_rad.y = ToRad(target.y);
-            _angle_ef_target_rad.z = ToRad(target.z);
-            }
+            _angle_rad.roll = radians(target.x);
+            _angle_rad.pitch = radians(target.y);
+            _angle_rad.yaw = radians(target.z);
+            _angle_rad.yaw_is_ef = false;
             break;
+        }
 
         // move mount to a neutral position, typically pointing forward
-        case MAV_MOUNT_MODE_NEUTRAL:
-            {
+        case MAV_MOUNT_MODE_NEUTRAL: {
             const Vector3f &target = _state._neutral_angles.get();
-            _angle_ef_target_rad.x = ToRad(target.x);
-            _angle_ef_target_rad.y = ToRad(target.y);
-            _angle_ef_target_rad.z = ToRad(target.z);
-            }
+            _angle_rad.roll = radians(target.x);
+            _angle_rad.pitch = radians(target.y);
+            _angle_rad.yaw = radians(target.z);
+            _angle_rad.yaw_is_ef = false;
             break;
+        }
 
         // point to the angles given by a mavlink message
         case MAV_MOUNT_MODE_MAVLINK_TARGETING:
-            // do nothing because earth-frame angle targets (i.e. _angle_ef_target_rad) should have already been set by a MOUNT_CONTROL message from GCS
+            switch (mavt_target.target_type) {
+            case MountTargetType::ANGLE:
+                _angle_rad = mavt_target.angle_rad;
+                break;
+            case MountTargetType::RATE:
+                update_angle_target_from_rate(mavt_target.rate_rads, _angle_rad);
+                break;
+            }
             resend_now = true;
             break;
 
         // RC radio manual angle control, but with stabilization from the AHRS
-        case MAV_MOUNT_MODE_RC_TARGETING:
-            // update targets using pilot's rc inputs
-            update_targets_from_rc();
+        case MAV_MOUNT_MODE_RC_TARGETING: {
+            // update targets using pilot's RC inputs
+            MountTarget rc_target {};
+            if (get_rc_rate_target(rc_target)) {
+                update_angle_target_from_rate(rc_target, _angle_rad);
+            } else if (get_rc_angle_target(rc_target)) {
+                _angle_rad = rc_target;
+            }
             resend_now = true;
             break;
+        }
 
-        // point mount to a GPS point given by the mission planner
+        // point mount to a GPS location
         case MAV_MOUNT_MODE_GPS_POINT:
-            if (calc_angle_to_roi_target(_angle_ef_target_rad, true, true, true)) {
+            if (get_angle_target_to_roi(_angle_rad)) {
                 resend_now = true;
             }
             break;
 
         case MAV_MOUNT_MODE_HOME_LOCATION:
-            // constantly update the home location:
-            if (!AP::ahrs().home_is_set()) {
-                break;
-            }
-            _roi_target = AP::ahrs().get_home();
-            _roi_target_set = true;
-            if (calc_angle_to_roi_target(_angle_ef_target_rad, true, true, true)) {
+            if (get_angle_target_to_home(_angle_rad)) {
                 resend_now = true;
             }
             break;
 
         case MAV_MOUNT_MODE_SYSID_TARGET:
-            if (calc_angle_to_sysid_target(_angle_ef_target_rad, true, true, true)) {
+            if (get_angle_target_to_sysid(_angle_rad)) {
                 resend_now = true;
             }
             break;
@@ -93,7 +100,7 @@ void AP_Mount_SToRM32::update()
 
     // resend target angles at least once per second
     if (resend_now || ((AP_HAL::millis() - _last_send) > AP_MOUNT_STORM32_RESEND_MS)) {
-        send_do_mount_control(ToDeg(_angle_ef_target_rad.y), ToDeg(_angle_ef_target_rad.x), ToDeg(_angle_ef_target_rad.z), MAV_MOUNT_MODE_MAVLINK_TARGETING);
+        send_do_mount_control(_angle_rad);
     }
 }
 
@@ -120,7 +127,7 @@ void AP_Mount_SToRM32::set_mode(enum MAV_MOUNT_MODE mode)
 void AP_Mount_SToRM32::send_mount_status(mavlink_channel_t chan)
 {
     // return target angles as gimbal's actual attitude.  To-Do: retrieve actual gimbal attitude and send these instead
-    mavlink_msg_mount_status_send(chan, 0, 0, ToDeg(_angle_ef_target_rad.y)*100, ToDeg(_angle_ef_target_rad.x)*100, ToDeg(_angle_ef_target_rad.z)*100, _mode);
+    mavlink_msg_mount_status_send(chan, 0, 0, degrees(_angle_rad.pitch)*100, degrees(_angle_rad.roll)*100, degrees(get_bf_yaw_angle(_angle_rad))*100, _mode);
 }
 
 // search for gimbal in GCS_MAVLink routing table
@@ -142,7 +149,7 @@ void AP_Mount_SToRM32::find_gimbal()
 }
 
 // send_do_mount_control - send a COMMAND_LONG containing a do_mount_control message
-void AP_Mount_SToRM32::send_do_mount_control(float pitch_deg, float roll_deg, float yaw_deg, enum MAV_MOUNT_MODE mount_mode)
+void AP_Mount_SToRM32::send_do_mount_control(const MountTarget& angle_target_rad)
 {
     // exit immediately if not initialised
     if (!_initialised) {
@@ -154,21 +161,18 @@ void AP_Mount_SToRM32::send_do_mount_control(float pitch_deg, float roll_deg, fl
         return;
     }
 
-    // reverse pitch and yaw control
-    pitch_deg = -pitch_deg;
-    yaw_deg = -yaw_deg;
-
     // send command_long command containing a do_mount_control command
+    // Note: pitch and yaw are reversed
     mavlink_msg_command_long_send(_chan,
                                   _sysid,
                                   _compid,
                                   MAV_CMD_DO_MOUNT_CONTROL,
                                   0,        // confirmation of zero means this is the first time this message has been sent
-                                  pitch_deg,
-                                  roll_deg,
-                                  yaw_deg,
+                                  -degrees(angle_target_rad.pitch),
+                                  degrees(angle_target_rad.roll),
+                                  -degrees(get_bf_yaw_angle(angle_target_rad)),
                                   0, 0, 0,  // param4 ~ param6 unused
-                                  mount_mode);
+                                  MAV_MOUNT_MODE_MAVLINK_TARGETING);
 
     // store time of send
     _last_send = AP_HAL::millis();

libraries/AP_Mount/AP_Mount_SToRM32.h

@@ -43,8 +43,8 @@ private:
     // search for gimbal in GCS_MAVLink routing table
     void find_gimbal();
 
-    // send_do_mount_control - send a COMMAND_LONG containing a do_mount_control message
-    void send_do_mount_control(float pitch_deg, float roll_deg, float yaw_deg, enum MAV_MOUNT_MODE mount_mode);
+    // send_do_mount_control with latest angle targets
+    void send_do_mount_control(const MountTarget& angle_target_rad);
 
     // internal variables
     bool _initialised;              // true once the driver has been initialised
@@ -52,5 +52,6 @@ private:
     uint8_t _compid;                // component id of gimbal
     mavlink_channel_t _chan;        // mavlink channel used to communicate with gimbal
     uint32_t _last_send;            // system time of last do_mount_control sent to gimbal
+    MountTarget _angle_rad;         // latest angle target
 };
 #endif // HAL_MOUNT_STORM32MAVLINK_ENABLED