AP_Mount: add scripting backend

libraries/AP_Mount/AP_Mount.cpp

@@ -12,6 +12,7 @@
 #include "AP_Mount_SToRM32_serial.h"
 #include "AP_Mount_Gremsy.h"
 #include "AP_Mount_Siyi.h"
+#include "AP_Mount_Scripting.h"
 #include <stdio.h>
 #include <AP_Math/location.h>
 #include <SRV_Channel/SRV_Channel.h>
@@ -118,6 +119,13 @@ void AP_Mount::init()
             _num_instances++;
 #endif // HAL_MOUNT_SIYI_ENABLED
 
+#if HAL_MOUNT_SCRIPTING_ENABLED
+        // check for Scripting gimbal
+        } else if (mount_type == Mount_Type_Scripting) {
+            _backends[instance] = new AP_Mount_Scripting(*this, _params[instance], instance);
+            _num_instances++;
+#endif // HAL_MOUNT_SCRIPTING_ENABLED
+
         }
 
         // init new instance
@@ -428,6 +436,39 @@ bool AP_Mount::pre_arm_checks(char *failure_msg, uint8_t failure_msg_len)
     return true;
 }
 
+// accessors for scripting backends
+bool AP_Mount::get_rate_target(uint8_t instance, float& roll_degs, float& pitch_degs, float& yaw_degs, bool& yaw_is_earth_frame)
+{
+    if (!check_instance(instance)) {
+        return false;
+    }
+    return _backends[instance]->get_rate_target(roll_degs, pitch_degs, yaw_degs, yaw_is_earth_frame);
+}
+
+bool AP_Mount::get_angle_target(uint8_t instance, float& roll_deg, float& pitch_deg, float& yaw_deg, bool& yaw_is_earth_frame)
+{
+    if (!check_instance(instance)) {
+        return false;
+    }
+    return _backends[instance]->get_angle_target(roll_deg, pitch_deg, yaw_deg, yaw_is_earth_frame);
+}
+
+bool AP_Mount::get_location_target(uint8_t instance, Location& target_loc)
+{
+    if (!check_instance(instance)) {
+        return false;
+    }
+    return _backends[instance]->get_location_target(target_loc);
+}
+
+void AP_Mount::set_attitude_euler(uint8_t instance, float roll_deg, float pitch_deg, float yaw_bf_deg)
+{
+    if (!check_instance(instance)) {
+        return;
+    }
+    _backends[instance]->set_attitude_euler(roll_deg, pitch_deg, yaw_bf_deg);
+}
+
 // point at system ID sysid
 void AP_Mount::set_target_sysid(uint8_t instance, uint8_t sysid)
 {

libraries/AP_Mount/AP_Mount.h

@@ -49,6 +49,7 @@ class AP_Mount_SToRM32;
 class AP_Mount_SToRM32_serial;
 class AP_Mount_Gremsy;
 class AP_Mount_Siyi;
+class AP_Mount_Scripting;
 
 /*
   This is a workaround to allow the MAVLink backend access to the
@@ -66,6 +67,7 @@ class AP_Mount
     friend class AP_Mount_SToRM32_serial;
     friend class AP_Mount_Gremsy;
     friend class AP_Mount_Siyi;
+    friend class AP_Mount_Scripting;
 
 public:
     AP_Mount();
@@ -89,6 +91,7 @@ public:
         Mount_Type_Gremsy = 6,          /// Gremsy gimbal using MAVLink v2 Gimbal protocol
         Mount_Type_BrushlessPWM = 7,    /// Brushless (stabilized) gimbal using PWM protocol
         Mount_Type_Siyi = 8,            /// Siyi gimbal using custom serial protocol
+        Mount_Type_Scripting = 9,       /// Scripting gimbal driver
     };
 
     // init - detect and initialise all mounts
@@ -164,6 +167,12 @@ public:
     // 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
+    bool get_rate_target(uint8_t instance, float& roll_degs, float& pitch_degs, float& yaw_degs, bool& yaw_is_earth_frame);
+    bool get_angle_target(uint8_t instance, float& roll_deg, float& pitch_deg, float& yaw_deg, bool& yaw_is_earth_frame);
+    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);
+
     //
     // camera controls for gimbals that include a camera
     //

libraries/AP_Mount/AP_Mount_Backend.h

@@ -104,6 +104,12 @@ public:
     // handle GIMBAL_DEVICE_ATTITUDE_STATUS message
     virtual void handle_gimbal_device_attitude_status(const mavlink_message_t &msg) {}
 
+    // accessors for scripting backends
+    virtual bool get_rate_target(float& roll_degs, float& pitch_degs, float& yaw_degs, bool& yaw_is_earth_frame) { return false; }
+    virtual bool get_angle_target(float& roll_deg, float& pitch_deg, float& yaw_deg, bool& yaw_is_earth_frame) { return false; }
+    virtual bool get_location_target(Location &target_loc) { return false; }
+    virtual void set_attitude_euler(float roll_deg, float pitch_deg, float yaw_bf_deg) {};
+
     //
     // camera controls for gimbals that include a camera
     //

libraries/AP_Mount/AP_Mount_Params.cpp

@@ -9,7 +9,7 @@ const AP_Param::GroupInfo AP_Mount_Params::var_info[] = {
     // @Param: _TYPE
     // @DisplayName: Mount Type
     // @Description: Mount Type
-    // @Values: 0:None, 1:Servo, 2:3DR Solo, 3:Alexmos Serial, 4:SToRM32 MAVLink, 5:SToRM32 Serial, 6:Gremsy, 7:BrushlessPWM, 8:Siyi
+    // @Values: 0:None, 1:Servo, 2:3DR Solo, 3:Alexmos Serial, 4:SToRM32 MAVLink, 5:SToRM32 Serial, 6:Gremsy, 7:BrushlessPWM, 8:Siyi, 9:Scripting
     // @RebootRequired: True
     // @User: Standard
     AP_GROUPINFO_FLAGS("_TYPE", 1, AP_Mount_Params, type, 0, AP_PARAM_FLAG_ENABLE),

libraries/AP_Mount/AP_Mount_Scripting.cpp

@@ -0,0 +1,233 @@
+#include "AP_Mount_Scripting.h"
+
+#if HAL_MOUNT_SCRIPTING_ENABLED
+#include <AP_HAL/AP_HAL.h>
+#include <AP_AHRS/AP_AHRS.h>
+#include <GCS_MAVLink/GCS.h>
+
+extern const AP_HAL::HAL& hal;
+
+#define AP_MOUNT_SCRIPTING_TIMEOUT_MS    1000   // scripting mount becomes unhealthy after 1sec with no updates
+
+#define AP_MOUNT_SCRIPTING_DEBUG 0
+#define debug(fmt, args ...) do { if (AP_MOUNT_SCRIPTING_DEBUG) { GCS_SEND_TEXT(MAV_SEVERITY_INFO, "Siyi: " fmt, ## args); } } while (0)
+
+// update mount position - should be called periodically
+void AP_Mount_Scripting::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: {
+            const Vector3f &angle_bf_target = _params.retract_angles.get();
+            target_angle_rad.roll = ToRad(angle_bf_target.x);
+            target_angle_rad.pitch = ToRad(angle_bf_target.y);
+            target_angle_rad.yaw = ToRad(angle_bf_target.z);
+            target_angle_rad.yaw_is_ef = false;
+            target_angle_rad_valid = true;
+
+            // mark other targets as invalid
+            target_rate_rads_valid = false;
+            target_loc_valid = false;
+            break;
+        }
+
+        // move mount to a neutral position, typically pointing forward
+        case MAV_MOUNT_MODE_NEUTRAL: {
+            const Vector3f &angle_bf_target = _params.neutral_angles.get();
+            target_angle_rad.roll = ToRad(angle_bf_target.x);
+            target_angle_rad.pitch = ToRad(angle_bf_target.y);
+            target_angle_rad.yaw = ToRad(angle_bf_target.z);
+            target_angle_rad.yaw_is_ef = false;
+            target_angle_rad_valid = true;
+
+            // mark other targets as invalid
+            target_rate_rads_valid = false;
+            target_loc_valid = 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:
+                target_angle_rad = mavt_target.angle_rad;
+                target_angle_rad_valid = true;
+                target_rate_rads_valid = false;
+                target_loc_valid = false;
+                break;
+            case MountTargetType::RATE:
+                target_rate_rads = mavt_target.rate_rads;
+                target_rate_rads_valid = true;
+                target_angle_rad_valid = false;
+                target_loc_valid = false;
+                break;
+            }
+            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)) {
+                target_rate_rads = rc_target;
+                target_rate_rads_valid = true;
+                target_angle_rad_valid = false;
+                target_loc_valid = false;
+            } else if (get_rc_angle_target(rc_target)) {
+                target_angle_rad = rc_target;
+                target_angle_rad_valid = true;
+                target_rate_rads_valid = false;
+                target_loc_valid = false;
+            }
+            break;
+        }
+
+        // point mount towards a GPS point
+        case MAV_MOUNT_MODE_GPS_POINT: {
+            target_loc_valid = _roi_target_set;
+            if (target_loc_valid) {
+                target_loc = _roi_target;
+                target_angle_rad_valid = get_angle_target_to_location(target_loc, target_angle_rad);
+            } else {
+                target_angle_rad_valid = false;
+            }
+            target_rate_rads_valid = false;
+            break;
+        }
+
+        // point mount towards home
+        case MAV_MOUNT_MODE_HOME_LOCATION: {
+            target_loc_valid = AP::ahrs().home_is_set();
+            if (target_loc_valid) {
+                target_loc = AP::ahrs().get_home();
+                target_angle_rad_valid = get_angle_target_to_home(target_angle_rad);
+            } else {
+                target_angle_rad_valid = false;
+            }
+            target_rate_rads_valid = false;
+            break;
+        }
+
+        // point mount towards another vehicle
+        case MAV_MOUNT_MODE_SYSID_TARGET: {
+            target_loc_valid = _target_sysid_location_set;
+            if (target_loc_valid) {
+                target_loc = _target_sysid_location;
+                target_angle_rad_valid = get_angle_target_to_location(target_loc, target_angle_rad);
+            } else {
+                target_angle_rad_valid = false;
+            }
+            target_rate_rads_valid = false;
+            break;
+        }
+
+        default:
+            // we do not know this mode so raise internal error
+            INTERNAL_ERROR(AP_InternalError::error_t::flow_of_control);
+            break;
+    }
+}
+
+// return true if healthy
+bool AP_Mount_Scripting::healthy() const
+{
+    // healthy if scripting backend has updated actual angles recently
+    return (AP_HAL::millis() - last_update_ms <= AP_MOUNT_SCRIPTING_TIMEOUT_MS);
+}
+
+// take a picture.  returns true on success
+bool AP_Mount_Scripting::take_picture()
+{
+    picture_count++;
+    recording_video = false;
+    return true;
+}
+
+// start or stop video recording.  returns true on success
+// set start_recording = true to start record, false to stop recording
+bool AP_Mount_Scripting::record_video(bool start_recording)
+{
+    recording_video = start_recording;
+    return true;
+}
+
+// set camera zoom step.  returns true on success
+// zoom out = -1, hold = 0, zoom in = 1
+bool AP_Mount_Scripting::set_zoom_step(int8_t zoom_step)
+{
+    manual_zoom_step = zoom_step;
+    return true;
+}
+
+// set focus in, out or hold.  returns true on success
+// focus in = -1, focus hold = 0, focus out = 1
+bool AP_Mount_Scripting::set_manual_focus_step(int8_t focus_step)
+{
+    manual_focus_step = focus_step;
+    auto_focus_active = false;
+    return true;
+}
+
+// auto focus.  returns true on success
+bool AP_Mount_Scripting::set_auto_focus()
+{
+    manual_focus_step = 0;
+    auto_focus_active = true;
+    return true;
+}
+
+// accessors for scripting backends
+bool AP_Mount_Scripting::get_rate_target(float& roll_degs, float& pitch_degs, float& yaw_degs, bool& yaw_is_earth_frame)
+{
+    if (target_rate_rads_valid) {
+        roll_degs = degrees(target_rate_rads.roll);
+        pitch_degs = degrees(target_rate_rads.pitch);
+        yaw_degs = degrees(target_rate_rads.yaw);
+        yaw_is_earth_frame = target_rate_rads.yaw_is_ef;
+        return true;
+    }
+    return false;
+}
+
+bool AP_Mount_Scripting::get_angle_target(float& roll_deg, float& pitch_deg, float& yaw_deg, bool& yaw_is_earth_frame)
+{
+    if (target_angle_rad_valid) {
+        roll_deg = degrees(target_angle_rad.roll);
+        pitch_deg = degrees(target_angle_rad.pitch);
+        yaw_deg = degrees(target_angle_rad.yaw);
+        yaw_is_earth_frame = target_angle_rad.yaw_is_ef;
+        return true;
+    }
+    return false;
+}
+
+// return target location if available
+// returns true if a target location is available and fills in target_loc argument
+bool AP_Mount_Scripting::get_location_target(Location &_target_loc)
+{
+    if (target_loc_valid) {
+        _target_loc = target_loc;
+        return true;
+    }
+    return false;
+}
+
+// update mount's actual angles (to be called by script communicating with mount)
+void AP_Mount_Scripting::set_attitude_euler(float roll_deg, float pitch_deg, float yaw_bf_deg)
+{
+    last_update_ms = AP_HAL::millis();
+    current_angle_deg.x = roll_deg;
+    current_angle_deg.y = pitch_deg;
+    current_angle_deg.z = yaw_bf_deg;
+}
+
+// get attitude as a quaternion.  returns true on success
+bool AP_Mount_Scripting::get_attitude_quaternion(Quaternion& att_quat)
+{
+    // construct quaternion
+    att_quat.from_euler(radians(current_angle_deg.x), radians(current_angle_deg.y), radians(current_angle_deg.z));
+    return true;
+}
+
+#endif // HAL_MOUNT_SCRIPTING_ENABLED

libraries/AP_Mount/AP_Mount_Scripting.h

@@ -0,0 +1,93 @@
+/*
+  Scripting mount/gimbal driver
+ */
+
+#pragma once
+
+#include "AP_Mount_Backend.h"
+
+#ifndef HAL_MOUNT_SCRIPTING_ENABLED
+#define HAL_MOUNT_SCRIPTING_ENABLED HAL_MOUNT_ENABLED && AP_SCRIPTING_ENABLED
+#endif
+
+#if HAL_MOUNT_SCRIPTING_ENABLED
+
+#include <AP_HAL/AP_HAL.h>
+#include <AP_Math/AP_Math.h>
+#include <AP_Common/AP_Common.h>
+
+class AP_Mount_Scripting : public AP_Mount_Backend
+{
+
+public:
+    // Constructor
+    using AP_Mount_Backend::AP_Mount_Backend;
+
+    /* Do not allow copies */
+    CLASS_NO_COPY(AP_Mount_Scripting);
+
+    // init - performs any required initialisation for this instance
+    void init() override {};
+
+    // update mount position - should be called periodically
+    void update() override;
+
+    // return true if healthy
+    bool healthy() const override;
+
+    // has_pan_control - returns true if this mount can control its pan (required for multicopters)
+    bool has_pan_control() const override { return yaw_range_valid(); };
+
+    // take a picture.  returns true on success
+    bool take_picture() override;
+
+    // start or stop video recording.  returns true on success
+    // set start_recording = true to start record, false to stop recording
+    bool record_video(bool start_recording) override;
+
+    // set camera zoom step.  returns true on success
+    // zoom out = -1, hold = 0, zoom in = 1
+    bool set_zoom_step(int8_t zoom_step) override;
+
+    // set focus in, out or hold.  returns true on success
+    // focus in = -1, focus hold = 0, focus out = 1
+    bool set_manual_focus_step(int8_t focus_step) override;
+
+    // auto focus.  returns true on success
+    bool set_auto_focus() override;
+
+    // accessors for scripting backends
+    bool get_rate_target(float& roll_degs, float& pitch_degs, float& yaw_degs, bool& yaw_is_earth_frame) override;
+    bool get_angle_target(float& roll_deg, float& pitch_deg, float& yaw_deg, bool& yaw_is_earth_frame) override;
+    bool get_location_target(Location& _target_loc) override;
+    void set_attitude_euler(float roll_deg, float pitch_deg, float yaw_bf_deg) override;
+
+protected:
+
+    // get attitude as a quaternion.  returns true on success
+    bool get_attitude_quaternion(Quaternion& att_quat) override;
+
+private:
+
+    // internal variables
+    uint32_t last_update_ms;        // system time of last call to one of the get_ methods.  Used for health reporting
+    Vector3f current_angle_deg;     // current gimbal angles in degrees (x=roll, y=pitch, z=yaw)
+
+    MountTarget target_rate_rads;   // rate target in rad/s
+    bool target_rate_rads_valid;    // true if _target_rate_degs holds a valid rate target
+
+    MountTarget target_angle_rad;   // angle target in radians
+    bool target_angle_rad_valid;    // true if _target_rate_degs holds a valid rate target
+
+    Location target_loc;            // target location
+    bool target_loc_valid;          // true if target_loc holds a valid target location
+
+    // camera related internal variables
+    uint16_t picture_count;         // number of pictures that have been taken (or at least requested)
+    bool recording_video;           // true when record video has been requested
+    int8_t manual_zoom_step;        // manual zoom step.  zoom out = -1, hold = 0, zoom in = 1
+    int8_t manual_focus_step;       // manual focus step.  focus in = -1, focus hold = 0, focus out = 1
+    bool auto_focus_active;         // auto focus active
+};
+
+#endif // HAL_MOUNT_SIYISERIAL_ENABLED