AP_Camera: Add mount angles in camera log message

ArduCopter/Copter.cpp

@@ -583,6 +583,11 @@ void Copter::ten_hz_logging_loop()
         g2.winch.write_log();
     }
 #endif
+#if HAL_MOUNT_ENABLED
+    if (should_log(MASK_LOG_CAMERA)) {
+        camera_mount.write_log();
+    }
+#endif
 }
 
 // twentyfive_hz_logging - should be run at 25hz

ArduPlane/ArduPlane.cpp

@@ -245,7 +245,12 @@ void Plane::update_logging10(void)
         ahrs.Write_AOA_SSA();
     } else if (log_faster) {
         ahrs.Write_AOA_SSA();
-    } 
+    }
+#if HAL_MOUNT_ENABLED
+    if (should_log(MASK_LOG_CAMERA)) {
+        camera_mount.write_log();
+    }
+#endif
 }
 
 /*

ArduSub/ArduSub.cpp

@@ -210,6 +210,11 @@ void Sub::ten_hz_logging_loop()
     if (should_log(MASK_LOG_CTUN)) {
         attitude_control.control_monitor_log();
     }
+#if HAL_MOUNT_ENABLED
+    if (should_log(MASK_LOG_CAMERA)) {
+        camera_mount.write_log();
+    }
+#endif
 }
 
 // twentyfive_hz_logging_loop

Rover/Rover.cpp

@@ -421,6 +421,11 @@ void Rover::update_logging2(void)
         gyro_fft.write_log_messages();
 #endif
     }
+#if HAL_MOUNT_ENABLED
+    if (should_log(MASK_LOG_CAMERA)) {
+        camera_mount.write_log();
+    }
+#endif
 }
 
 

libraries/AP_Camera/AP_Camera_Logging.cpp

@@ -1,11 +1,12 @@
 #include "AP_Camera_Backend.h"
+#include <AP_Mount/AP_Mount.h>
 
 #if AP_CAMERA_ENABLED
 
 #include <AP_Logger/AP_Logger.h>
 #include <AP_GPS/AP_GPS.h>
 
-// Write a Camera packet
+// Write a Camera packet.  Also writes a Mount packet if available
 void AP_Camera_Backend::Write_CameraInfo(enum LogMessages msg, uint64_t timestamp_us)
 {
     // exit immediately if no logger
@@ -41,9 +42,14 @@ void AP_Camera_Backend::Write_CameraInfo(enum LogMessages msg, uint64_t timestam
         altitude_gps = 0;
     }
 
+    // if timestamp is zero set to current system time
+    if (timestamp_us == 0) {
+        timestamp_us = AP_HAL::micros64();
+    }
+
     const struct log_Camera pkt{
         LOG_PACKET_HEADER_INIT(static_cast<uint8_t>(msg)),
-        time_us     : timestamp_us ? timestamp_us : AP_HAL::micros64(),
+        time_us     : timestamp_us,
         instance    : _instance,
         image_number: image_index,
         gps_time    : gps.time_week_ms(),
@@ -58,6 +64,14 @@ void AP_Camera_Backend::Write_CameraInfo(enum LogMessages msg, uint64_t timestam
         yaw         : (uint16_t)ahrs.yaw_sensor
     };
     AP::logger().WriteCriticalBlock(&pkt, sizeof(pkt));
+
+#if HAL_MOUNT_ENABLED
+    auto *mount = AP_Mount::get_singleton();
+    if (mount!= nullptr) {
+        // assuming camera instance matches mount instance
+        mount->write_log(_instance, timestamp_us);
+    }
+#endif
 }
 
 // Write a Camera packet

libraries/AP_Camera/AP_Camera_Params.cpp

@@ -8,7 +8,7 @@ const AP_Param::GroupInfo AP_Camera_Params::var_info[] = {
     // @Param: _TYPE
     // @DisplayName: Camera shutter (trigger) type
     // @Description: how to trigger the camera to take a picture
-    // @Values: 1:Servo,2:Relay, 3:GoPro in Solo Gimbal, 4:Mount (Siyi), 5:MAVLink, 6:MAVLinkCamV2, 7:Scripting
+    // @Values: 0:None, 1:Servo, 2:Relay, 3:GoPro in Solo Gimbal, 4:Mount (Siyi), 5:MAVLink, 6:MAVLinkCamV2, 7:Scripting
     // @User: Standard
     AP_GROUPINFO_FLAGS("_TYPE",  1, AP_Camera_Params, type, 0, AP_PARAM_FLAG_ENABLE),
 

libraries/AP_Logger/LogStructure.h

@@ -131,6 +131,7 @@ const struct MultiplierStructure log_Multipliers[] = {
 #include <AP_InertialSensor/LogStructure.h>
 #include <AP_AHRS/LogStructure.h>
 #include <AP_Camera/LogStructure.h>
+#include <AP_Mount/LogStructure.h>
 #include <AP_Baro/LogStructure.h>
 #include <AP_VisualOdom/LogStructure.h>
 #include <AC_PrecLand/LogStructure.h>
@@ -1260,6 +1261,7 @@ LOG_STRUCTURE_FROM_PRECLAND \
     { LOG_RADIO_MSG, sizeof(log_Radio), \
       "RAD", "QBBBBBHH", "TimeUS,RSSI,RemRSSI,TxBuf,Noise,RemNoise,RxErrors,Fixed", "s-------", "F-------", true }, \
 LOG_STRUCTURE_FROM_CAMERA \
+LOG_STRUCTURE_FROM_MOUNT \
     { LOG_ARSP_MSG, sizeof(log_ARSP), "ARSP",  "QBffcffBBffB", "TimeUS,I,Airspeed,DiffPress,Temp,RawPress,Offset,U,H,Hp,TR,Pri", "s#nPOPP-----", "F-00B00-----", true }, \
     LOG_STRUCTURE_FROM_BATTMONITOR \
     { LOG_MAG_MSG, sizeof(log_MAG), \
@@ -1370,6 +1372,7 @@ enum LogMessages : uint8_t {
     LOG_RADIO_MSG,
     LOG_ATRP_MSG,
     LOG_IDS_FROM_CAMERA,
+    LOG_IDS_FROM_MOUNT,
     LOG_TERRAIN_MSG,
     LOG_CSRV_MSG,
     LOG_IDS_FROM_ESC_TELEM,

libraries/AP_Mount/AP_Mount.cpp

@@ -646,7 +646,7 @@ bool AP_Mount::pre_arm_checks(char *failure_msg, uint8_t failure_msg_len)
     return true;
 }
 
-// accessors for scripting backends
+// get target rate in deg/sec. returns true on success
 bool AP_Mount::get_rate_target(uint8_t instance, float& roll_degs, float& pitch_degs, float& yaw_degs, bool& yaw_is_earth_frame)
 {
     auto *backend = get_instance(instance);
@@ -656,6 +656,7 @@ bool AP_Mount::get_rate_target(uint8_t instance, float& roll_degs, float& pitch_
     return backend->get_rate_target(roll_degs, pitch_degs, yaw_degs, yaw_is_earth_frame);
 }
 
+// get target angle in deg. returns true on success
 bool AP_Mount::get_angle_target(uint8_t instance, float& roll_deg, float& pitch_deg, float& yaw_deg, bool& yaw_is_earth_frame)
 {
     auto *backend = get_instance(instance);
@@ -665,6 +666,7 @@ bool AP_Mount::get_angle_target(uint8_t instance, float& roll_deg, float& pitch_
     return backend->get_angle_target(roll_deg, pitch_deg, yaw_deg, yaw_is_earth_frame);
 }
 
+// accessors for scripting backends and logging
 bool AP_Mount::get_location_target(uint8_t instance, Location& target_loc)
 {
     auto *backend = get_instance(instance);
@@ -683,6 +685,26 @@ void AP_Mount::set_attitude_euler(uint8_t instance, float roll_deg, float pitch_
     backend->set_attitude_euler(roll_deg, pitch_deg, yaw_bf_deg);
 }
 
+// write mount log packet for all backends
+void AP_Mount::write_log()
+{
+    // each instance writes log
+    for (uint8_t instance=0; instance<AP_MOUNT_MAX_INSTANCES; instance++) {
+        if (_backends[instance] != nullptr) {
+            _backends[instance]->write_log(0);
+        }
+    }
+}
+
+void AP_Mount::write_log(uint8_t instance, uint64_t timestamp_us)
+{
+    auto *backend = get_instance(instance);
+    if (backend == nullptr) {
+        return;
+    }
+    backend->write_log(timestamp_us);
+}
+
 // point at system ID sysid
 void AP_Mount::set_target_sysid(uint8_t instance, uint8_t sysid)
 {

libraries/AP_Mount/AP_Mount.h

@@ -199,12 +199,22 @@ class AP_Mount
     // any failure_msg returned will not include a prefix
     bool pre_arm_checks(char *failure_msg, uint8_t failure_msg_len);
 
-    // accessors for scripting backends
+    // get target rate in deg/sec. returns true on success
     bool get_rate_target(uint8_t instance, float& roll_degs, float& pitch_degs, float& yaw_degs, bool& yaw_is_earth_frame);
+
+    // get target angle in deg. returns true on success
     bool get_angle_target(uint8_t instance, float& roll_deg, float& pitch_deg, float& yaw_deg, bool& yaw_is_earth_frame);
+
+    // accessors for scripting backends and logging
     bool get_location_target(uint8_t instance, Location& target_loc);
     void set_attitude_euler(uint8_t instance, float roll_deg, float pitch_deg, float yaw_bf_deg);
 
+    // write mount log packet for all backends
+    void write_log();
+
+    // write mount log packet for a single backend (called by camera library)
+    void write_log(uint8_t instance, uint64_t timestamp_us);
+
     //
     // camera controls for gimbals that include a camera
     //

libraries/AP_Mount/AP_Mount_Alexmos.cpp

@@ -30,67 +30,76 @@ void AP_Mount_Alexmos::update()
         // move mount to a "retracted" position.  we do not implement a separate servo based retract mechanism
         case MAV_MOUNT_MODE_RETRACT: {
             const Vector3f &target = _params.retract_angles.get();
-            _angle_rad.roll = radians(target.x);
-            _angle_rad.pitch = radians(target.y);
-            _angle_rad.yaw = radians(target.z);
-            _angle_rad.yaw_is_ef = false;
+            mnt_target.target_type = MountTargetType::ANGLE;
+            mnt_target.angle_rad.set(target*DEG_TO_RAD, false);
             break;
         }
 
         // move mount to a neutral position, typically pointing forward
         case MAV_MOUNT_MODE_NEUTRAL: {
             const Vector3f &target = _params.neutral_angles.get();
-            _angle_rad.roll = radians(target.x);
-            _angle_rad.pitch = radians(target.y);
-            _angle_rad.yaw = radians(target.z);
-            _angle_rad.yaw_is_ef = false;
+            mnt_target.target_type = MountTargetType::ANGLE;
+            mnt_target.angle_rad.set(target*DEG_TO_RAD, false);
             break;
         }
 
         // point to the angles given by a mavlink message
         case MAV_MOUNT_MODE_MAVLINK_TARGETING:
-            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;
-            }
+            // mavlink targets are stored while handling the incoming message
             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
-            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;
+            MountTarget rc_target;
+            get_rc_target(mnt_target.target_type, rc_target);
+            switch (mnt_target.target_type) {
+            case MountTargetType::ANGLE:
+                mnt_target.angle_rad = rc_target;
+                break;
+            case MountTargetType::RATE:
+                mnt_target.rate_rads = rc_target;
+                break;
             }
             break;
         }
 
         // point mount to a GPS point given by the mission planner
         case MAV_MOUNT_MODE_GPS_POINT:
-            IGNORE_RETURN(get_angle_target_to_roi(_angle_rad));
+            if (get_angle_target_to_roi(mnt_target.angle_rad)) {
+                mnt_target.target_type = MountTargetType::ANGLE;
+            }
             break;
 
+        // point mount to Home location
         case MAV_MOUNT_MODE_HOME_LOCATION:
-            IGNORE_RETURN(get_angle_target_to_home(_angle_rad));
+            if (get_angle_target_to_home(mnt_target.angle_rad)) {
+                mnt_target.target_type = MountTargetType::ANGLE;
+            }
             break;
 
+        // point mount to another vehicle
         case MAV_MOUNT_MODE_SYSID_TARGET:
-            IGNORE_RETURN(get_angle_target_to_sysid(_angle_rad));
+            if (get_angle_target_to_sysid(mnt_target.angle_rad)) {
+                mnt_target.target_type = MountTargetType::ANGLE;
+            }
             break;
 
         default:
             // we do not know this mode so do nothing
             break;
     }
 
-    // send latest targets to gimbal
-    control_axis(_angle_rad);
+    // send target angles or rates depending on the target type
+    switch (mnt_target.target_type) {
+        case MountTargetType::RATE:
+            update_angle_target_from_rate(mnt_target.rate_rads, mnt_target.angle_rad);
+            FALLTHROUGH;
+        case MountTargetType::ANGLE:
+            // send latest angle targets to gimbal
+            control_axis(mnt_target.angle_rad);
+            break;
+    }
 }
 
 // has_pan_control - returns true if this mount can control its pan (required for multicopters)

libraries/AP_Mount/AP_Mount_Alexmos.h

@@ -112,8 +112,6 @@ class AP_Mount_Alexmos : public AP_Mount_Backend
     // read_incoming - detect and read the header of the incoming message from the gimbal
     void read_incoming();
 
-    MountTarget _angle_rad;         // latest angle target
-
     // structure for the Serial Protocol
 
     // CMD_BOARD_INFO