AP_Gimbal: initial implementation of the rate controlled gimbal library

libraries/AP_Gimbal/AP_Gimbal.cpp

@@ -0,0 +1,169 @@
+// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
+
+#include <stdio.h>
+#include <AP_Common.h>
+#include <AP_Progmem.h>
+#include <AP_Param.h>
+#include <AP_Gimbal.h>
+#include <GCS.h>
+#include <GCS_MAVLink.h>
+#include <AP_SmallEKF.h>
+
+const AP_Param::GroupInfo AP_Gimbal::var_info[] PROGMEM = {
+    AP_GROUPEND
+};
+
+uint16_t feedback_error_count;
+static float K_gimbalRate = 5.0f;
+static float angRateLimit = 0.5f;
+
+void AP_Gimbal::receive_feedback(mavlink_channel_t chan, mavlink_message_t *msg)
+{
+    update_targets_from_rc();
+    decode_feedback(msg);
+    update_state();
+    if (_ekf.getStatus()){
+        send_control(chan);
+    }
+
+    Quaternion quatEst;_ekf.getQuat(quatEst);Vector3f eulerEst;quatEst.to_euler(eulerEst.x, eulerEst.y, eulerEst.z);
+    //::printf("est=%1.1f %1.1f %1.1f %d\t", eulerEst.x,eulerEst.y,eulerEst.z,_ekf.getStatus());    
+    //::printf("joint_angles=(%+1.2f %+1.2f %+1.2f)\t", _measurament.joint_angles.x,_measurament.joint_angles.y,_measurament.joint_angles.z);
+    //::printf("delta_ang=(%+1.3f %+1.3f %+1.3f)\t",_measurament.delta_angles.x,_measurament.delta_angles.y,_measurament.delta_angles.z);     
+    //::printf("delta_vel=(%+1.3f %+1.3f %+1.3f)\t",_measurament.delta_velocity.x,_measurament.delta_velocity.y,_measurament.delta_velocity.z);     
+    //::printf("rate=(%+1.3f %+1.3f %+1.3f)\t",gimbalRateDemVec.x,gimbalRateDemVec.y,gimbalRateDemVec.z);
+    //::printf("target=(%+1.3f %+1.3f %+1.3f)\t",_angle_ef_target_rad.x,_angle_ef_target_rad.y,_angle_ef_target_rad.z);
+    //::printf("\n");
+}
+    
+
+void AP_Gimbal::decode_feedback(mavlink_message_t *msg)
+{
+    mavlink_gimbal_report_t report_msg;
+    mavlink_msg_gimbal_report_decode(msg, &report_msg);
+
+    _measurament.delta_time = report_msg.delta_time;
+    _measurament.delta_angles.x = report_msg.delta_angle_x;
+    _measurament.delta_angles.y = report_msg.delta_angle_y,
+    _measurament.delta_angles.z = report_msg.delta_angle_z;
+    _measurament.delta_velocity.x = report_msg.delta_velocity_x,
+    _measurament.delta_velocity.y = report_msg.delta_velocity_y,
+    _measurament.delta_velocity.z = report_msg.delta_velocity_z;   
+    _measurament.joint_angles.x = report_msg.joint_roll;
+    _measurament.joint_angles.y = report_msg.joint_el,
+    _measurament.joint_angles.z = report_msg.joint_az;
+
+    //apply joint angle compensation
+    _measurament.joint_angles -= _joint_offsets;
+}
+
+void AP_Gimbal::update_state()
+{
+    // Run the gimbal attitude and gyro bias estimator
+    _ekf.RunEKF(_measurament.delta_time, _measurament.delta_angles, _measurament.delta_velocity, _measurament.joint_angles);
+
+    // get the gimbal quaternion estimate
+    Quaternion quatEst;
+    _ekf.getQuat(quatEst);
+ 
+    // Add the control rate vectors
+    gimbalRateDemVec.zero();
+    gimbalRateDemVec += getGimbalRateDemVecYaw(quatEst);
+    gimbalRateDemVec += getGimbalRateDemVecTilt(quatEst);
+    gimbalRateDemVec += getGimbalRateDemVecForward(quatEst);
+    gimbalRateDemVec += getGimbalRateDemVecGyroBias();
+}
+
+Vector3f AP_Gimbal::getGimbalRateDemVecYaw(Quaternion quatEst)
+{
+        // Define rotation from vehicle to gimbal using a 312 rotation sequence
+        Matrix3f Tvg;
+        Tvg.from_euler( _measurament.joint_angles.x,
+                        _measurament.joint_angles.y,  
+                        _measurament.joint_angles.z);
+
+        // multiply the yaw joint angle by a gain to calculate a demanded vehicle frame relative rate vector required to keep the yaw joint centred
+        Vector3f gimbalRateDemVecYaw;
+        gimbalRateDemVecYaw.z = - K_gimbalRate * _measurament.joint_angles.z;
+
+        // Get filtered vehicle turn rate in earth frame
+        vehicleYawRateFilt = (1.0f - yawRateFiltPole * _measurament.delta_time) * vehicleYawRateFilt + yawRateFiltPole * _measurament.delta_time * _ahrs.get_yaw_rate_earth();
+        Vector3f vehicle_rate_ef(0,0,vehicleYawRateFilt);
+
+         // calculate the maximum steady state rate error corresponding to the maximum permitted yaw angle error
+        float maxRate = K_gimbalRate * yawErrorLimit;
+        float vehicle_rate_mag_ef = vehicle_rate_ef.length();
+        float excess_rate_correction = fabs(vehicle_rate_mag_ef) - maxRate; 
+        if (vehicle_rate_mag_ef > maxRate) {
+            if (vehicle_rate_ef.z>0.0f){
+                gimbalRateDemVecYaw += _ahrs.get_dcm_matrix().transposed()*Vector3f(0,0,excess_rate_correction);    
+            }else{
+                gimbalRateDemVecYaw -= _ahrs.get_dcm_matrix().transposed()*Vector3f(0,0,excess_rate_correction);    
+            }            
+        }        
+
+        // rotate into gimbal frame to calculate the gimbal rate vector required to keep the yaw gimbal centred
+        gimbalRateDemVecYaw = Tvg * gimbalRateDemVecYaw;
+        return gimbalRateDemVecYaw;
+}
+
+Vector3f AP_Gimbal::getGimbalRateDemVecTilt(Quaternion quatEst)
+{
+        // Calculate the gimbal 321 Euler angle estimates relative to earth frame
+        Vector3f eulerEst;
+        quatEst.to_euler(eulerEst.x, eulerEst.y, eulerEst.z);
+
+        //TODO receive target from AP_Mount
+        Vector3f vectorError;
+        vectorError.x = eulerEst.x;
+        vectorError.y = eulerEst.y - _angle_ef_target_rad.y;
+        vectorError.z = 0;
+
+        Vector3f gimbalRateDemVecTilt = - vectorError * K_gimbalRate;
+        return gimbalRateDemVecTilt;
+}
+
+Vector3f AP_Gimbal::getGimbalRateDemVecForward(Quaternion quatEst)
+{
+        // quaternion demanded at the previous time step
+        static float lastDem;
+
+        // calculate the delta rotation from the last to the current demand where the demand does not incorporate the copters yaw rotation
+        float delta = _angle_ef_target_rad.y - lastDem;
+        lastDem = _angle_ef_target_rad.y;
+
+        Vector3f gimbalRateDemVecForward;
+        gimbalRateDemVecForward.y = delta / _measurament.delta_time;
+        return gimbalRateDemVecForward;
+}
+
+Vector3f AP_Gimbal::getGimbalRateDemVecGyroBias()
+{
+    Vector3f gyroBias;
+    _ekf.getGyroBias(gyroBias);
+    return gyroBias;
+}
+
+void AP_Gimbal::send_control(mavlink_channel_t chan)
+{
+    mavlink_msg_gimbal_control_send(chan,_sysid, _compid, 
+        gimbalRateDemVec.x, gimbalRateDemVec.y, gimbalRateDemVec.z);
+}
+
+// returns the angle (radians) that the RC_Channel input is receiving
+float angle_input_rad(RC_Channel* rc, float angle_min, float angle_max)
+{
+    float input =rc->norm_input();
+    float angle = input*(angle_max - angle_min) + angle_min;
+    
+    return radians(angle);
+}
+
+// update_targets_from_rc - updates angle targets using input from receiver
+void AP_Gimbal::update_targets_from_rc()
+{
+    float new_tilt = angle_input_rad(RC_Channel::rc_channel(tilt_rc_in-1), _tilt_angle_min, _tilt_angle_max);
+    float max_change_rads =_max_tilt_rate * _measurament.delta_time;
+    float tilt_change = constrain_float(new_tilt - _angle_ef_target_rad.y,-max_change_rads,+max_change_rads);
+    _angle_ef_target_rad.y += tilt_change;
+}

libraries/AP_Gimbal/AP_Gimbal.h

@@ -0,0 +1,105 @@
+// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
+
+/************************************************************
+* AP_Gimbal -- library to control a 3 axis rate gimbal.		*
+*															*
+* Author:  Arthur Benemann, Paul Riseborough;									*
+*															*
+* Purpose:                          						*
+*															*
+* Usage:	           										*
+*															*
+* Comments:                         						*
+************************************************************/
+#ifndef __AP_GIMBAL_H__
+#define __AP_GIMBAL_H__
+
+#include <AP_Math.h>
+#include <AP_Common.h>
+#include <AP_GPS.h>
+#include <AP_AHRS.h>
+#include <GCS_MAVLink.h>
+#include <AP_SmallEKF.h>
+#include <AP_NavEKF.h>
+
+
+class AP_Gimbal
+{
+public:
+    //Constructor
+    AP_Gimbal(const AP_AHRS_NavEKF &ahrs, uint8_t sysid, uint8_t compid) :
+        _ahrs(ahrs),
+        _ekf(ahrs),
+        _joint_offsets(0.0f,0.0f,0.0f),
+        vehicleYawRateFilt(0.0f),
+        yawRateFiltPole(10.0f),
+        yawErrorLimit(0.1f),
+        tilt_rc_in(6),
+        _tilt_angle_min(-45.0f),
+        _tilt_angle_max(0.0f),
+        _max_tilt_rate(0.5f)
+    {
+        AP_Param::setup_object_defaults(this, var_info);
+        _sysid = sysid;
+        _compid = compid;
+    }
+        
+    // MAVLink methods
+    void    receive_feedback(mavlink_channel_t chan, mavlink_message_t *msg);
+
+    // hook for eeprom variables
+    static const struct AP_Param::GroupInfo        var_info[];
+
+    struct Measurament {
+        float delta_time;
+        Vector3f delta_angles;
+        Vector3f delta_velocity;
+        Vector3f joint_angles;
+    } _measurament;
+
+    SmallEKF    _ekf;                   // state of small EKF for gimbal
+    Vector3f    gimbalRateDemVec;       // degrees/s   
+    Vector3f    _angle_ef_target_rad;   // desired earth-frame roll, tilt and pan angles in radians
+
+private:  
+    // filtered yaw rate from the vehicle
+    float vehicleYawRateFilt;
+
+    // circular frequency (rad/sec) constant of filter applied to forward path vehicle yaw rate
+    // this frequency must not be larger than the update rate (Hz).
+    // reducing it makes the gimbal yaw less responsive to vehicle yaw
+    // increasing it makes the gimbal yawe more responsive to vehicle yaw
+    float const yawRateFiltPole;
+
+    // amount of yaw angle that we permit the gimbal to lag the vehicle when operating in slave mode
+    // reducing this makes the gimbal respond more to vehicle yaw disturbances
+    float const yawErrorLimit;
+
+    // These are corrections (in radians) applied to the to the gimbal joint (x,y,z = roll,pitch,yaw) measurements
+    Vector3f const _joint_offsets;
+
+
+    uint8_t const tilt_rc_in; 
+    float const _tilt_angle_min;   // min tilt in 0.01 degree units
+    float const _tilt_angle_max;     // max tilt in 0.01 degree units
+    float const _max_tilt_rate;          // max tilt rate in rad/s
+    
+    const AP_AHRS_NavEKF    &_ahrs;     //  Main EKF    
+    uint8_t _sysid;                     
+    uint8_t _compid;
+
+
+    void send_control(mavlink_channel_t chan);
+    void update_state();
+    void decode_feedback(mavlink_message_t *msg);
+    void update_targets_from_rc();
+
+    // Control loop functions
+    Vector3f getGimbalRateDemVecYaw(Quaternion quatEst);
+    Vector3f getGimbalRateDemVecTilt(Quaternion quatEst);
+    Vector3f getGimbalRateDemVecForward(Quaternion quatEst);
+    Vector3f getGimbalRateDemVecGyroBias();
+
+};
+
+#endif // __AP_MOUNT_H__