Mount: SToRM32 mount backend

libraries/AP_Mount/AP_Mount_SToRM32.cpp

@@ -0,0 +1,139 @@
+// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
+
+#include <AP_Mount_SToRM32.h>
+#include <AP_HAL.h>
+#include <GCS_MAVLink.h>
+#include <stdio.h>
+
+extern const AP_HAL::HAL& hal;
+
+AP_Mount_SToRM32::AP_Mount_SToRM32(AP_Mount &frontend, AP_Mount::mount_state &state, uint8_t instance) :
+    AP_Mount_Backend(frontend, state, instance),
+    _initialised(false),
+    _chan(MAVLINK_COMM_0),
+    _last_send(0)
+{}
+
+// init - performs any required initialisation for this instance
+void AP_Mount_SToRM32::init(const AP_SerialManager& serial_manager)
+{
+    // get_mavlink_channel for MAVLink2
+    if (serial_manager.get_mavlink_channel(AP_SerialManager::SerialProtocol_MAVLink2, _chan)) {
+        _initialised = true;
+        set_mode((enum MAV_MOUNT_MODE)_state._default_mode.get());
+    }
+}
+
+// update mount position - should be called periodically
+void AP_Mount_SToRM32::update()
+{
+    // exit immediately if not initialised
+    if (!_initialised) {
+        return;
+    }
+
+    // flag to trigger sending target angles to gimbal
+    bool resend_now = false;
+
+    // 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 &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);
+            }
+            break;
+
+        // move mount to a neutral position, typically pointing forward
+        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);
+            }
+            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
+            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();
+            resend_now = true;
+            break;
+
+        // point mount to a GPS point given by the mission planner
+        case MAV_MOUNT_MODE_GPS_POINT:
+            if(_frontend._ahrs.get_gps().status() >= AP_GPS::GPS_OK_FIX_2D) {
+                calc_angle_to_location(_state._roi_target, _angle_ef_target_rad, true, true);
+                resend_now = true;
+            }
+            break;
+
+        default:
+            // we do not know this mode so do nothing
+            break;
+    }
+
+    // resend target angles at least once per second
+    if (resend_now || ((hal.scheduler->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);
+    }
+}
+
+// has_pan_control - returns true if this mount can control it's pan (required for multicopters)
+bool AP_Mount_SToRM32::has_pan_control() const
+{
+    // we do not have yaw control
+    return false;
+}
+
+// set_mode - sets mount's mode
+void AP_Mount_SToRM32::set_mode(enum MAV_MOUNT_MODE mode)
+{
+    // exit immediately if not initialised
+    if (!_initialised) {
+        return;
+    }
+
+    // record the mode change
+    _state._mode = mode;
+}
+
+// status_msg - called to allow mounts to send their status to GCS using the MOUNT_STATUS message
+void AP_Mount_SToRM32::status_msg(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.x)*100, ToDeg(_angle_ef_target_rad.y)*100, ToDeg(_angle_ef_target_rad.z)*100);
+}
+
+// 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)
+{
+    // exit immediately if not initialised
+    if (!_initialised) {
+        return;
+    }
+
+    // send command_long command containing a do_mount_control command
+    mavlink_msg_command_long_send(_chan,
+                                  AP_MOUNT_STORM32_SYSID,
+                                  AP_MOUNT_STORM32_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,
+                                  0, 0, 0,  // param4 ~ param6 unused
+                                  mount_mode);
+
+    // store time of send
+    _last_send = hal.scheduler->millis();
+}

libraries/AP_Mount/AP_Mount_SToRM32.h

@@ -0,0 +1,58 @@
+// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
+
+/*
+  SToRM32 mount backend class
+ */
+
+#ifndef __AP_MOUNT_STORM32_H__
+#define __AP_MOUNT_STORM32_H__
+
+#include <AP_HAL.h>
+#include <AP_AHRS.h>
+
+#include <AP_Math.h>
+#include <AP_Common.h>
+#include <AP_GPS.h>
+#include <GCS_MAVLink.h>
+#include <RC_Channel.h>
+#include <AP_Mount_Backend.h>
+
+#define AP_MOUNT_STORM32_SYSID      71      // hardcoded system id
+#define AP_MOUNT_STORM32_COMPID     67      // hard coded component id for communicating with the gimbal
+
+#define AP_MOUNT_STORM32_RESEND_MS  1000    // resend angle targets to gimbal once per second
+
+class AP_Mount_SToRM32 : public AP_Mount_Backend
+{
+
+public:
+    // Constructor
+    AP_Mount_SToRM32(AP_Mount &frontend, AP_Mount::mount_state &state, uint8_t instance);
+
+    // init - performs any required initialisation for this instance
+    virtual void init(const AP_SerialManager& serial_manager);
+
+    // update mount position - should be called periodically
+    virtual void update();
+
+    // has_pan_control - returns true if this mount can control it's pan (required for multicopters)
+    virtual bool has_pan_control() const;
+
+    // set_mode - sets mount's mode
+    virtual void set_mode(enum MAV_MOUNT_MODE mode);
+
+    // status_msg - called to allow mounts to send their status to GCS using the MOUNT_STATUS message
+    virtual void status_msg(mavlink_channel_t chan);
+
+private:
+
+    // 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);
+
+    // internal variables
+    bool _initialised;              // true once the driver has been initialised
+    mavlink_channel_t _chan;        // mavlink channel used to communicate with gimbal.  Currently hard-coded to Telem2
+    uint32_t _last_send;            // system time of last do_mount_control sent to gimbal
+};
+
+#endif // __AP_MOUNT_STORM32_H__