From 5b834330cb5176f9cd1dacb85143d349529e1db4 Mon Sep 17 00:00:00 2001 From: Jonathan Challinger Date: Wed, 30 Dec 2015 18:19:52 -0800 Subject: [PATCH] AP_Mount: merge SoloGimbal from solo master --- libraries/AP_Mount/AP_Gimbal.cpp | 212 -------- libraries/AP_Mount/AP_Gimbal.h | 95 ---- libraries/AP_Mount/AP_Mount.cpp | 116 ++--- libraries/AP_Mount/AP_Mount.h | 27 +- libraries/AP_Mount/AP_Mount_Backend.h | 6 + ...nt_MAVLink.cpp => AP_Mount_SoloGimbal.cpp} | 73 ++- ..._Mount_MAVLink.h => AP_Mount_SoloGimbal.h} | 25 +- libraries/AP_Mount/SoloGimbal.cpp | 489 ++++++++++++++++++ libraries/AP_Mount/SoloGimbal.h | 160 ++++++ libraries/AP_Mount/SoloGimbal_Parameters.cpp | 278 ++++++++++ libraries/AP_Mount/SoloGimbal_Parameters.h | 94 ++++ 11 files changed, 1153 insertions(+), 422 deletions(-) delete mode 100644 libraries/AP_Mount/AP_Gimbal.cpp delete mode 100644 libraries/AP_Mount/AP_Gimbal.h rename libraries/AP_Mount/{AP_Mount_MAVLink.cpp => AP_Mount_SoloGimbal.cpp} (53%) rename libraries/AP_Mount/{AP_Mount_MAVLink.h => AP_Mount_SoloGimbal.h} (69%) create mode 100644 libraries/AP_Mount/SoloGimbal.cpp create mode 100644 libraries/AP_Mount/SoloGimbal.h create mode 100644 libraries/AP_Mount/SoloGimbal_Parameters.cpp create mode 100644 libraries/AP_Mount/SoloGimbal_Parameters.h diff --git a/libraries/AP_Mount/AP_Gimbal.cpp b/libraries/AP_Mount/AP_Gimbal.cpp deleted file mode 100644 index 3bc29b592a..0000000000 --- a/libraries/AP_Mount/AP_Gimbal.cpp +++ /dev/null @@ -1,212 +0,0 @@ -// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*- - -#include "AP_Gimbal.h" - -#if AP_AHRS_NAVEKF_AVAILABLE - -#include -#include -#include -#include -#include - -void AP_Gimbal::receive_feedback(mavlink_channel_t chan, mavlink_message_t *msg) -{ - decode_feedback(msg); - update_state(); - if (_ekf.getStatus() && !isCopterFlipped() && !is_zero(_gimbalParams.K_gimbalRate)){ - 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", _measurement.joint_angles.x,_measurement.joint_angles.y,_measurement.joint_angles.z); - //::printf("delta_ang=(%+1.3f %+1.3f %+1.3f)\t",_measurement.delta_angles.x,_measurement.delta_angles.y,_measurement.delta_angles.z); - //::printf("delta_vel=(%+1.3f %+1.3f %+1.3f)\t",_measurement.delta_velocity.x,_measurement.delta_velocity.y,_measurement.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_msg_gimbal_report_decode(msg, &_report_msg); - - _measurement.delta_time = _report_msg.delta_time; - _measurement.delta_angles.x = _report_msg.delta_angle_x; - _measurement.delta_angles.y = _report_msg.delta_angle_y; - _measurement.delta_angles.z = _report_msg.delta_angle_z; - _measurement.delta_velocity.x = _report_msg.delta_velocity_x, - _measurement.delta_velocity.y = _report_msg.delta_velocity_y; - _measurement.delta_velocity.z = _report_msg.delta_velocity_z; - _measurement.joint_angles.x = _report_msg.joint_roll; - _measurement.joint_angles.y = _report_msg.joint_el; - _measurement.joint_angles.z = _report_msg.joint_az; - - //apply joint angle compensation - _measurement.joint_angles -= _gimbalParams.joint_angles_offsets; - _measurement.delta_velocity -= _gimbalParams.delta_velocity_offsets; - _measurement.delta_angles -= _gimbalParams.delta_angles_offsets; -} - -/* - send a gimbal report to the GCS for display purposes - */ -void AP_Gimbal::send_report(mavlink_channel_t chan) const -{ - mavlink_msg_gimbal_report_send(chan, - 0, 0, // send as broadcast - _report_msg.delta_time, - _report_msg.delta_angle_x, - _report_msg.delta_angle_y, - _report_msg.delta_angle_z, - _report_msg.delta_velocity_x, - _report_msg.delta_velocity_y, - _report_msg.delta_velocity_z, - _report_msg.joint_roll, - _report_msg.joint_el, - _report_msg.joint_az); -} - -void AP_Gimbal::update_state() -{ - // Run the gimbal attitude and gyro bias estimator - _ekf.RunEKF(_measurement.delta_time, _measurement.delta_angles, _measurement.delta_velocity, _measurement.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(const Quaternion &quatEst) -{ - // Define rotation from vehicle to gimbal using a 312 rotation sequence - Matrix3f Tvg; - float cosPhi = cosf(_measurement.joint_angles.x); - float cosTheta = cosf(_measurement.joint_angles.y); - float sinPhi = sinf(_measurement.joint_angles.x); - float sinTheta = sinf(_measurement.joint_angles.y); - float sinPsi = sinf(_measurement.joint_angles.z); - float cosPsi = cosf(_measurement.joint_angles.z); - Tvg[0][0] = cosTheta*cosPsi-sinPsi*sinPhi*sinTheta; - Tvg[1][0] = -sinPsi*cosPhi; - Tvg[2][0] = cosPsi*sinTheta+cosTheta*sinPsi*sinPhi; - Tvg[0][1] = cosTheta*sinPsi+cosPsi*sinPhi*sinTheta; - Tvg[1][1] = cosPsi*cosPhi; - Tvg[2][1] = sinPsi*sinTheta-cosTheta*cosPsi*sinPhi; - Tvg[0][2] = -sinTheta*cosPhi; - Tvg[1][2] = sinPhi; - Tvg[2][2] = cosTheta*cosPhi; - - // 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 = - _gimbalParams.K_gimbalRate * _measurement.joint_angles.z; - - // Get filtered vehicle turn rate in earth frame - vehicleYawRateFilt = (1.0f - yawRateFiltPole * _measurement.delta_time) * vehicleYawRateFilt + yawRateFiltPole * _measurement.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 = _gimbalParams.K_gimbalRate * yawErrorLimit; - float vehicle_rate_mag_ef = vehicle_rate_ef.length(); - float excess_rate_correction = fabsf(vehicle_rate_mag_ef) - maxRate; - if (vehicle_rate_mag_ef > maxRate) { - if (vehicle_rate_ef.z>0.0f){ - gimbalRateDemVecYaw += _ahrs.get_rotation_body_to_ned().transposed()*Vector3f(0,0,excess_rate_correction); - } else { - gimbalRateDemVecYaw -= _ahrs.get_rotation_body_to_ned().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(const Quaternion &quatEst) -{ - // Calculate the gimbal 321 Euler angle estimates relative to earth frame - Vector3f eulerEst = quatEst.to_vector312(); - - // Calculate a demanded quaternion using the demanded roll and pitch and estimated yaw (yaw is slaved to the vehicle) - Quaternion quatDem; - quatDem.from_vector312( _angle_ef_target_rad.x, - _angle_ef_target_rad.y, - eulerEst.z); - - //divide the demanded quaternion by the estimated to get the error - Quaternion quatErr = quatDem / quatEst; - - // Convert to a delta rotation using a small angle approximation - quatErr.normalize(); - Vector3f deltaAngErr; - float scaler; - if (quatErr[0] >= 0.0f) { - scaler = 2.0f; - } else { - scaler = -2.0f; - } - deltaAngErr.x = scaler * quatErr[1]; - deltaAngErr.y = scaler * quatErr[2]; - deltaAngErr.z = scaler * quatErr[3]; - - // multiply the angle error vector by a gain to calculate a demanded gimbal rate required to control tilt - Vector3f gimbalRateDemVecTilt = deltaAngErr * _gimbalParams.K_gimbalRate; - return gimbalRateDemVecTilt; -} - -Vector3f AP_Gimbal::getGimbalRateDemVecForward(const 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 / _measurement.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, mavlink_system.sysid, _compid, - gimbalRateDemVec.x, gimbalRateDemVec.y, gimbalRateDemVec.z); -} - -void AP_Gimbal::update_target(Vector3f newTarget) -{ - // Low-pass filter - _angle_ef_target_rad.y = _angle_ef_target_rad.y + 0.02f*(newTarget.y - _angle_ef_target_rad.y); - // Update tilt - _angle_ef_target_rad.y = constrain_float(_angle_ef_target_rad.y,radians(-90),radians(0)); -} - -Vector3f AP_Gimbal::getGimbalEstimateEF() -{ - Quaternion quatEst; - _ekf.getQuat(quatEst); - return quatEst.to_vector312(); -} - -bool AP_Gimbal::isCopterFlipped() -{ - return (_ahrs.cos_roll()*_ahrs.cos_pitch() < 0.5f); -} - -#endif // AP_AHRS_NAVEKF_AVAILABLE diff --git a/libraries/AP_Mount/AP_Gimbal.h b/libraries/AP_Mount/AP_Gimbal.h deleted file mode 100644 index aa9d362275..0000000000 --- a/libraries/AP_Mount/AP_Gimbal.h +++ /dev/null @@ -1,95 +0,0 @@ -// -*- 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; * -* * -************************************************************/ -#ifndef __AP_GIMBAL_H__ -#define __AP_GIMBAL_H__ - -#include -#include - -#if AP_AHRS_NAVEKF_AVAILABLE - -#include -#include -#include -#include -#include "AP_Mount.h" -#include -#include - -class AP_Gimbal -{ -public: - //Constructor - AP_Gimbal(const AP_AHRS_NavEKF &ahrs, uint8_t compid, const AP_Mount::gimbal_params &gimbalParams) : - _ekf(ahrs), - _ahrs(ahrs), - _gimbalParams(gimbalParams), - vehicleYawRateFilt(0.0f), - yawRateFiltPole(10.0f), - yawErrorLimit(0.1f) - { - _compid = compid; - memset(&_report_msg, 0, sizeof(_report_msg)); - } - - void update_target(Vector3f newTarget); - void receive_feedback(mavlink_channel_t chan, mavlink_message_t *msg); - void send_report(mavlink_channel_t chan) const; - - Vector3f getGimbalEstimateEF(); - - struct Measurament { - float delta_time; - Vector3f delta_angles; - Vector3f delta_velocity; - Vector3f joint_angles; - } _measurement; - - SmallEKF _ekf; // state of small EKF for gimbal - const AP_AHRS_NavEKF &_ahrs; // Main EKF - Vector3f gimbalRateDemVec; // degrees/s - Vector3f _angle_ef_target_rad; // desired earth-frame roll, tilt and pan angles in radians - -private: - const AP_Mount::gimbal_params &_gimbalParams; - - // 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; - - uint8_t _compid; - - mavlink_gimbal_report_t _report_msg; - - void send_control(mavlink_channel_t chan); - void update_state(); - void decode_feedback(mavlink_message_t *msg); - - bool isCopterFlipped(); - - // Control loop functions - Vector3f getGimbalRateDemVecYaw(const Quaternion &quatEst); - Vector3f getGimbalRateDemVecTilt(const Quaternion &quatEst); - Vector3f getGimbalRateDemVecForward(const Quaternion &quatEst); - Vector3f getGimbalRateDemVecGyroBias(); - -}; - -#endif // AP_AHRS_NAVEKF_AVAILABLE - -#endif // __AP_MOUNT_H__ diff --git a/libraries/AP_Mount/AP_Mount.cpp b/libraries/AP_Mount/AP_Mount.cpp index 3671b98f51..9becf150ba 100644 --- a/libraries/AP_Mount/AP_Mount.cpp +++ b/libraries/AP_Mount/AP_Mount.cpp @@ -5,7 +5,7 @@ #include "AP_Mount.h" #include "AP_Mount_Backend.h" #include "AP_Mount_Servo.h" -#include "AP_Mount_MAVLink.h" +#include "AP_Mount_SoloGimbal.h" #include "AP_Mount_Alexmos.h" #include "AP_Mount_SToRM32.h" #include "AP_Mount_SToRM32_serial.h" @@ -200,80 +200,15 @@ const AP_Param::GroupInfo AP_Mount::var_info[] = { // @User: Standard AP_GROUPINFO("_TYPE", 19, AP_Mount, state[0]._type, 0), - // @Param: _OFF_JNT_X - // @DisplayName: MAVLink Mount's roll angle offsets - // @Description: MAVLink Mount's roll angle offsets - // @Units: radians - // @Range: 0 0.5 - // @User: Advanced - - // @Param: _OFF_JNT_Y - // @DisplayName: MAVLink Mount's pitch angle offsets - // @Description: MAVLink Mount's pitch angle offsets - // @Units: radians - // @Range: 0 0.5 - // @User: Advanced - - // @Param: _OFF_JNT_Z - // @DisplayName: MAVLink Mount's yaw angle offsets - // @Description: MAVLink Mount's yaw angle offsets - // @Units: radians - // @Range: 0 0.5 - // @User: Advanced - AP_GROUPINFO("_OFF_JNT", 20, AP_Mount, state[0]._gimbalParams.joint_angles_offsets, 0), - - // @Param: _OFF_ACC_X - // @DisplayName: MAVLink Mount's roll velocity offsets - // @Description: MAVLink Mount's roll velocity offsets - // @Units: m/s - // @Range: 0 2 - // @User: Advanced - - // @Param: _OFF_ACC_Y - // @DisplayName: MAVLink Mount's pitch velocity offsets - // @Description: MAVLink Mount's pitch velocity offsets - // @Units: m/s - // @Range: 0 2 - // @User: Advanced - - // @Param: _OFF_ACC_Z - // @DisplayName: MAVLink Mount's yaw velocity offsets - // @Description: MAVLink Mount's yaw velocity offsets - // @Units: m/s - // @Range: 0 2 - // @User: Advanced - AP_GROUPINFO("_OFF_ACC", 21, AP_Mount, state[0]._gimbalParams.delta_velocity_offsets, 0), - - // @Param: _OFF_GYRO_X - // @DisplayName: MAVLink Mount's roll gyro offsets - // @Description: MAVLink Mount's roll gyro offsets - // @Units: radians/sec - // @Range: 0 0.5 - // @User: Advanced - - // @Param: _OFF_GYRO_Y - // @DisplayName: MAVLink Mount's pitch gyro offsets - // @Description: MAVLink Mount's pitch gyro offsets - // @Units: radians/sec - // @Range: 0 0.5 - // @User: Advanced - - // @Param: _OFF_GYRO_Z - // @DisplayName: MAVLink Mount's yaw gyro offsets - // @Description: MAVLink Mount's yaw gyro offsets - // @Units: radians/sec - // @Range: 0 0.5 - // @User: Advanced - AP_GROUPINFO("_OFF_GYRO", 22, AP_Mount, state[0]._gimbalParams.delta_angles_offsets, 0), - - // @Param: _K_RATE - // @DisplayName: MAVLink Mount's rate gain - // @Description: MAVLink Mount's rate gain - // @Range: 0 10 - // @User: Advanced - AP_GROUPINFO("_K_RATE", 23, AP_Mount, state[0]._gimbalParams.K_gimbalRate, 5.0f), - - // 20 ~ 24 reserved for future parameters + // 20 formerly _OFF_JNT + + // 21 formerly _OFF_ACC + + // 22 formerly _OFF_GYRO + + // 23 formerly _K_RATE + + // 24 is AVAILABLE #if AP_MOUNT_MAX_INSTANCES > 1 // @Param: 2_DEFLT_MODE @@ -475,13 +410,15 @@ AP_Mount::AP_Mount(const AP_AHRS_TYPE &ahrs, const struct Location ¤t_loc) } // init - detect and initialise all mounts -void AP_Mount::init(const AP_SerialManager& serial_manager) +void AP_Mount::init(DataFlash_Class *dataflash, const AP_SerialManager& serial_manager) { // check init has not been called before if (_num_instances != 0) { return; } + _dataflash = dataflash; + // default mount to servo mount if rc output channels to control roll, tilt or pan have been defined if (!state[0]._type.configured()) { if (RC_Channel_aux::function_assigned(RC_Channel_aux::Aux_servo_function_t::k_mount_pan) || @@ -508,8 +445,8 @@ void AP_Mount::init(const AP_SerialManager& serial_manager) #if AP_AHRS_NAVEKF_AVAILABLE // check for MAVLink mounts - } else if (mount_type == Mount_Type_MAVLink) { - _backends[instance] = new AP_Mount_MAVLink(*this, state[instance], instance); + } else if (mount_type == Mount_Type_SoloGimbal) { + _backends[instance] = new AP_Mount_SoloGimbal(*this, state[instance], instance); _num_instances++; #endif @@ -551,6 +488,17 @@ void AP_Mount::update() } } +// used for gimbals that need to read INS data at full rate +void AP_Mount::update_fast() +{ + // update each instance + for (uint8_t instance=0; instanceupdate_fast(); + } + } +} + // get_mount_type - returns the type of mount AP_Mount::MountType AP_Mount::get_mount_type(uint8_t instance) const { @@ -674,7 +622,17 @@ void AP_Mount::handle_gimbal_report(mavlink_channel_t chan, mavlink_message_t *m if (_backends[instance] != NULL) { _backends[instance]->handle_gimbal_report(chan, msg); } - } + } +} + +// handle PARAM_VALUE +void AP_Mount::handle_param_value(mavlink_message_t *msg) +{ + for (uint8_t instance=0; instancehandle_param_value(msg); + } + } } // send a GIMBAL_REPORT message to the GCS diff --git a/libraries/AP_Mount/AP_Mount.h b/libraries/AP_Mount/AP_Mount.h index 3608319ba0..766e2dab25 100644 --- a/libraries/AP_Mount/AP_Mount.h +++ b/libraries/AP_Mount/AP_Mount.h @@ -29,6 +29,7 @@ #include #include #include +#include // maximum number of mounts #define AP_MOUNT_MAX_INSTANCES 1 @@ -36,7 +37,7 @@ // declare backend classes class AP_Mount_Backend; class AP_Mount_Servo; -class AP_Mount_MAVLink; +class AP_Mount_SoloGimbal; class AP_Mount_Alexmos; class AP_Mount_SToRM32; class AP_Mount_SToRM32_serial; @@ -51,7 +52,7 @@ class AP_Mount // declare backends as friends friend class AP_Mount_Backend; friend class AP_Mount_Servo; - friend class AP_Mount_MAVLink; + friend class AP_Mount_SoloGimbal; friend class AP_Mount_Alexmos; friend class AP_Mount_SToRM32; friend class AP_Mount_SToRM32_serial; @@ -62,28 +63,24 @@ public: enum MountType { Mount_Type_None = 0, /// no mount Mount_Type_Servo = 1, /// servo controlled mount - Mount_Type_MAVLink = 2, /// MAVLink controlled mount + Mount_Type_SoloGimbal = 2, /// Solo's gimbal Mount_Type_Alexmos = 3, /// Alexmos mount Mount_Type_SToRM32 = 4, /// SToRM32 mount using MAVLink protocol Mount_Type_SToRM32_serial = 5 /// SToRM32 mount using custom serial protocol }; - struct gimbal_params { - AP_Vector3f delta_angles_offsets; - AP_Vector3f delta_velocity_offsets; - AP_Vector3f joint_angles_offsets; - AP_Float K_gimbalRate; - }; - // Constructor AP_Mount(const AP_AHRS_TYPE &ahrs, const struct Location ¤t_loc); // init - detect and initialise all mounts - void init(const AP_SerialManager& serial_manager); + void init(DataFlash_Class *dataflash, const AP_SerialManager& serial_manager); // update - give mount opportunity to update servos. should be called at 10hz or higher void update(); + // used for gimbals that need to read INS data at full rate + void update_fast(); + // get_mount_type - returns the type of mount AP_Mount::MountType get_mount_type() const { return get_mount_type(_primary); } AP_Mount::MountType get_mount_type(uint8_t instance) const; @@ -126,6 +123,9 @@ public: void control_msg(mavlink_message_t* msg) { control_msg(_primary, msg); } void control_msg(uint8_t instance, mavlink_message_t* msg); + // handle a PARAM_VALUE message + void handle_param_value(mavlink_message_t *msg); + // handle a GIMBAL_REPORT message void handle_gimbal_report(mavlink_channel_t chan, mavlink_message_t *msg); @@ -182,10 +182,9 @@ protected: MAV_MOUNT_MODE _mode; // current mode (see MAV_MOUNT_MODE enum) struct Location _roi_target; // roi target location - - struct gimbal_params _gimbalParams; - } state[AP_MOUNT_MAX_INSTANCES]; + + DataFlash_Class *_dataflash; }; #endif // __AP_MOUNT_H__ diff --git a/libraries/AP_Mount/AP_Mount_Backend.h b/libraries/AP_Mount/AP_Mount_Backend.h index 221f310fec..4795e1133d 100644 --- a/libraries/AP_Mount/AP_Mount_Backend.h +++ b/libraries/AP_Mount/AP_Mount_Backend.h @@ -44,6 +44,9 @@ public: // update mount position - should be called periodically virtual void update() = 0; + // used for gimbals that need to read INS data at full rate + virtual void update_fast() {} + // has_pan_control - returns true if this mount can control it's pan (required for multicopters) virtual bool has_pan_control() const = 0; @@ -71,6 +74,9 @@ public: // handle a GIMBAL_REPORT message virtual void handle_gimbal_report(mavlink_channel_t chan, mavlink_message_t *msg) {} + // handle a PARAM_VALUE message + virtual void handle_param_value(mavlink_message_t *msg) {} + // send a GIMBAL_REPORT message to the GCS virtual void send_gimbal_report(mavlink_channel_t chan) {} diff --git a/libraries/AP_Mount/AP_Mount_MAVLink.cpp b/libraries/AP_Mount/AP_Mount_SoloGimbal.cpp similarity index 53% rename from libraries/AP_Mount/AP_Mount_MAVLink.cpp rename to libraries/AP_Mount/AP_Mount_SoloGimbal.cpp index d76b68ed94..8cd07db032 100644 --- a/libraries/AP_Mount/AP_Mount_MAVLink.cpp +++ b/libraries/AP_Mount/AP_Mount_SoloGimbal.cpp @@ -1,30 +1,40 @@ // -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*- -#include "AP_Mount_MAVLink.h" +#include +#include #if AP_AHRS_NAVEKF_AVAILABLE + +#include "AP_Mount_SoloGimbal.h" +#include "SoloGimbal.h" +#include #include -#include -#include "AP_Gimbal.h" #if MOUNT_DEBUG #include #endif -AP_Mount_MAVLink::AP_Mount_MAVLink(AP_Mount &frontend, AP_Mount::mount_state &state, uint8_t instance) : +extern const AP_HAL::HAL& hal; + +AP_Mount_SoloGimbal::AP_Mount_SoloGimbal(AP_Mount &frontend, AP_Mount::mount_state &state, uint8_t instance) : AP_Mount_Backend(frontend, state, instance), _initialised(false), - _gimbal(frontend._ahrs, MAV_COMP_ID_GIMBAL, _state._gimbalParams) + _gimbal(frontend._ahrs) {} // init - performs any required initialisation for this instance -void AP_Mount_MAVLink::init(const AP_SerialManager& serial_manager) +void AP_Mount_SoloGimbal::init(const AP_SerialManager& serial_manager) { _initialised = true; set_mode((enum MAV_MOUNT_MODE)_state._default_mode.get()); } +void AP_Mount_SoloGimbal::update_fast() +{ + _gimbal.update_fast(); +} + // update mount position - should be called periodically -void AP_Mount_MAVLink::update() +void AP_Mount_SoloGimbal::update() { // exit immediately if not initialised if (!_initialised) { @@ -35,25 +45,36 @@ void AP_Mount_MAVLink::update() switch(get_mode()) { // move mount to a "retracted" position. we do not implement a separate servo based retract mechanism case MAV_MOUNT_MODE_RETRACT: + _gimbal.set_lockedToBody(true); break; // move mount to a neutral position, typically pointing forward case MAV_MOUNT_MODE_NEUTRAL: + { + _gimbal.set_lockedToBody(false); + 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: + _gimbal.set_lockedToBody(false); // 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: + _gimbal.set_lockedToBody(false); // update targets using pilot's rc inputs update_targets_from_rc(); break; // point mount to a GPS point given by the mission planner case MAV_MOUNT_MODE_GPS_POINT: + _gimbal.set_lockedToBody(false); 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); } @@ -66,14 +87,14 @@ void AP_Mount_MAVLink::update() } // has_pan_control - returns true if this mount can control it's pan (required for multicopters) -bool AP_Mount_MAVLink::has_pan_control() const +bool AP_Mount_SoloGimbal::has_pan_control() const { // we do not have yaw control return false; } // set_mode - sets mount's mode -void AP_Mount_MAVLink::set_mode(enum MAV_MOUNT_MODE mode) +void AP_Mount_SoloGimbal::set_mode(enum MAV_MOUNT_MODE mode) { // exit immediately if not initialised if (!_initialised) { @@ -85,27 +106,49 @@ void AP_Mount_MAVLink::set_mode(enum MAV_MOUNT_MODE mode) } // status_msg - called to allow mounts to send their status to GCS using the MOUNT_STATUS message -void AP_Mount_MAVLink::status_msg(mavlink_channel_t chan) +void AP_Mount_SoloGimbal::status_msg(mavlink_channel_t chan) { - Vector3f est = _gimbal.getGimbalEstimateEF(); - mavlink_msg_mount_status_send(chan, 0, 0, degrees(est.y)*100, degrees(est.x)*100, degrees(est.z)*100); + if (_gimbal.aligned()) { + mavlink_msg_mount_status_send(chan, 0, 0, degrees(_angle_ef_target_rad.y)*100, degrees(_angle_ef_target_rad.x)*100, degrees(_angle_ef_target_rad.z)*100); + } } /* handle a GIMBAL_REPORT message */ -void AP_Mount_MAVLink::handle_gimbal_report(mavlink_channel_t chan, mavlink_message_t *msg) +void AP_Mount_SoloGimbal::handle_gimbal_report(mavlink_channel_t chan, mavlink_message_t *msg) { _gimbal.update_target(_angle_ef_target_rad); _gimbal.receive_feedback(chan,msg); + + if(!_params_saved && _frontend._dataflash != NULL && _frontend._dataflash->logging_started()) { + _gimbal.fetch_params(); //last parameter save might not be stored in dataflash so retry + _params_saved = true; + } + + if (_gimbal.get_log_dt() > 1.0f/25.0f) { + _gimbal.write_logs(_frontend._dataflash); + } +} + +void AP_Mount_SoloGimbal::handle_param_value(mavlink_message_t *msg) +{ + _gimbal.handle_param_value(_frontend._dataflash, msg); +} + +/* + handle a GIMBAL_REPORT message + */ +void AP_Mount_SoloGimbal::handle_gimbal_torque_report(mavlink_channel_t chan, mavlink_message_t *msg) +{ + _gimbal.disable_torque_report(); } /* send a GIMBAL_REPORT message to the GCS */ -void AP_Mount_MAVLink::send_gimbal_report(mavlink_channel_t chan) +void AP_Mount_SoloGimbal::send_gimbal_report(mavlink_channel_t chan) { - _gimbal.send_report(chan); } #endif // AP_AHRS_NAVEKF_AVAILABLE diff --git a/libraries/AP_Mount/AP_Mount_MAVLink.h b/libraries/AP_Mount/AP_Mount_SoloGimbal.h similarity index 69% rename from libraries/AP_Mount/AP_Mount_MAVLink.h rename to libraries/AP_Mount/AP_Mount_SoloGimbal.h index fd62a72e32..ebce708e12 100644 --- a/libraries/AP_Mount/AP_Mount_MAVLink.h +++ b/libraries/AP_Mount/AP_Mount_SoloGimbal.h @@ -4,8 +4,9 @@ MAVLink enabled mount backend class */ -#ifndef __AP_MOUNT_MAVLINK_H__ -#define __AP_MOUNT_MAVLINK_H__ +#ifndef __AP_MOUNT_SOLOGIMBAL_H__ +#define __AP_MOUNT_SOLOGIMBAL_H__ + #include #include @@ -17,14 +18,15 @@ #include #include #include "AP_Mount_Backend.h" -#include "AP_Gimbal.h" +#include "SoloGimbal.h" + -class AP_Mount_MAVLink : public AP_Mount_Backend +class AP_Mount_SoloGimbal : public AP_Mount_Backend { public: // Constructor - AP_Mount_MAVLink(AP_Mount &frontend, AP_Mount::mount_state &state, uint8_t instance); + AP_Mount_SoloGimbal(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); @@ -43,17 +45,26 @@ public: // handle a GIMBAL_REPORT message virtual void handle_gimbal_report(mavlink_channel_t chan, mavlink_message_t *msg); + virtual void handle_gimbal_torque_report(mavlink_channel_t chan, mavlink_message_t *msg); + virtual void handle_param_value(mavlink_message_t *msg); // send a GIMBAL_REPORT message to the GCS virtual void send_gimbal_report(mavlink_channel_t chan); + virtual void update_fast(); + private: // internal variables bool _initialised; // true once the driver has been initialised - AP_Gimbal _gimbal; + // Write a gimbal measurament and estimation data packet + void Log_Write_Gimbal(SoloGimbal &gimbal); + + bool _params_saved; + + SoloGimbal _gimbal; }; #endif // AP_AHRS_NAVEKF_AVAILABLE -#endif // __AP_MOUNT_MAVLINK_H__ +#endif // __AP_MOUNT_SOLOGIMBAL_H__ diff --git a/libraries/AP_Mount/SoloGimbal.cpp b/libraries/AP_Mount/SoloGimbal.cpp new file mode 100644 index 0000000000..a358fbf077 --- /dev/null +++ b/libraries/AP_Mount/SoloGimbal.cpp @@ -0,0 +1,489 @@ +// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*- + +#include +#include +#if AP_AHRS_NAVEKF_AVAILABLE + +#include "SoloGimbal.h" + +#include +#include + +extern const AP_HAL::HAL& hal; + +bool SoloGimbal::present() +{ + if (_state != GIMBAL_STATE_NOT_PRESENT && AP_HAL::millis()-_last_report_msg_ms > 3000) { + // gimbal went away + _state = GIMBAL_STATE_NOT_PRESENT; + return false; + } + + return _state != GIMBAL_STATE_NOT_PRESENT; +} + +bool SoloGimbal::aligned() +{ + return present() && _state == GIMBAL_STATE_PRESENT_RUNNING; +} + +gimbal_mode_t SoloGimbal::get_mode() +{ + if ((_gimbalParams.initialized() && _gimbalParams.get_K_rate()==0.0f) || (_ahrs.get_rotation_body_to_ned().c.z < 0 && !(_lockedToBody || _calibrator.running()))) { + return GIMBAL_MODE_IDLE; + } else if (!_ekf.getStatus()) { + return GIMBAL_MODE_POS_HOLD; + } else if (_calibrator.running() || _lockedToBody) { + return GIMBAL_MODE_POS_HOLD_FF; + } else { + return GIMBAL_MODE_STABILIZE; + } +} + +void SoloGimbal::receive_feedback(mavlink_channel_t chan, mavlink_message_t *msg) +{ + mavlink_gimbal_report_t report_msg; + mavlink_msg_gimbal_report_decode(msg, &report_msg); + uint32_t tnow_ms = AP_HAL::millis(); + _last_report_msg_ms = tnow_ms; + + _gimbalParams.set_channel(chan); + + if (report_msg.target_system != 1) { + _state = GIMBAL_STATE_NOT_PRESENT; + } + + switch(_state) { + case GIMBAL_STATE_NOT_PRESENT: + // gimbal was just connected or we just rebooted, transition to PRESENT_INITIALIZING + _gimbalParams.reset(); + _gimbalParams.set_param(GMB_PARAM_GMB_SYSID, 1); + _state = GIMBAL_STATE_PRESENT_INITIALIZING; + break; + + case GIMBAL_STATE_PRESENT_INITIALIZING: + _gimbalParams.update(); + if (_gimbalParams.initialized()) { + // parameters done initializing, finalize initialization and transition to aligning + extract_feedback(report_msg); + _ang_vel_mag_filt = 20; + _filtered_joint_angles = _measurement.joint_angles; + _vehicle_to_gimbal_quat_filt.from_vector312(_filtered_joint_angles.x,_filtered_joint_angles.y,_filtered_joint_angles.z); + _ekf.reset(); + _state = GIMBAL_STATE_PRESENT_ALIGNING; + } + break; + + case GIMBAL_STATE_PRESENT_ALIGNING: + _gimbalParams.update(); + extract_feedback(report_msg); + update_estimators(); + if (_ekf.getStatus()) { + // EKF done aligning, transition to running + _state = GIMBAL_STATE_PRESENT_RUNNING; + } + break; + + case GIMBAL_STATE_PRESENT_RUNNING: + _gimbalParams.update(); + extract_feedback(report_msg); + update_estimators(); + break; + } + + send_controls(chan); +} + +void SoloGimbal::send_controls(mavlink_channel_t chan) +{ + if (_state == GIMBAL_STATE_PRESENT_RUNNING) { + // get the gimbal quaternion estimate + Quaternion quatEst; + _ekf.getQuat(quatEst); + + // run rate controller + _ang_vel_dem_rads.zero(); + switch(get_mode()) { + case GIMBAL_MODE_POS_HOLD_FF: { + _ang_vel_dem_rads += get_ang_vel_dem_body_lock(); + _ang_vel_dem_rads += get_ang_vel_dem_gyro_bias(); + float _ang_vel_dem_radsLen = _ang_vel_dem_rads.length(); + if (_ang_vel_dem_radsLen > radians(400)) { + _ang_vel_dem_rads *= radians(400)/_ang_vel_dem_radsLen; + } + mavlink_msg_gimbal_control_send(chan, mavlink_system.sysid, _compid, + _ang_vel_dem_rads.x, _ang_vel_dem_rads.y, _ang_vel_dem_rads.z); + break; + } + case GIMBAL_MODE_STABILIZE: { + _ang_vel_dem_rads += get_ang_vel_dem_yaw(quatEst); + _ang_vel_dem_rads += get_ang_vel_dem_tilt(quatEst); + _ang_vel_dem_rads += get_ang_vel_dem_feedforward(quatEst); + _ang_vel_dem_rads += get_ang_vel_dem_gyro_bias(); + float ang_vel_dem_norm = _ang_vel_dem_rads.length(); + if (ang_vel_dem_norm > radians(400)) { + _ang_vel_dem_rads *= radians(400)/ang_vel_dem_norm; + } + mavlink_msg_gimbal_control_send(chan, mavlink_system.sysid, _compid, + _ang_vel_dem_rads.x, _ang_vel_dem_rads.y, _ang_vel_dem_rads.z); + break; + } + default: + case GIMBAL_MODE_IDLE: + case GIMBAL_MODE_POS_HOLD: + break; + } + } + + // set GMB_POS_HOLD + if (get_mode() == GIMBAL_MODE_POS_HOLD) { + _gimbalParams.set_param(GMB_PARAM_GMB_POS_HOLD, 1); + } else { + _gimbalParams.set_param(GMB_PARAM_GMB_POS_HOLD, 0); + } + + // set GMB_MAX_TORQUE + float max_torque; + _gimbalParams.get_param(GMB_PARAM_GMB_MAX_TORQUE, max_torque, 0); + if (max_torque != _max_torque && max_torque != 0) { + _max_torque = max_torque; + } + + if (!hal.util->get_soft_armed() || joints_near_limits()) { + _gimbalParams.set_param(GMB_PARAM_GMB_MAX_TORQUE, _max_torque); + } else { + _gimbalParams.set_param(GMB_PARAM_GMB_MAX_TORQUE, 0); + } +} + +void SoloGimbal::extract_feedback(const mavlink_gimbal_report_t& report_msg) +{ + _measurement.delta_time = report_msg.delta_time; + _measurement.delta_angles.x = report_msg.delta_angle_x; + _measurement.delta_angles.y = report_msg.delta_angle_y; + _measurement.delta_angles.z = report_msg.delta_angle_z; + _measurement.delta_velocity.x = report_msg.delta_velocity_x, + _measurement.delta_velocity.y = report_msg.delta_velocity_y; + _measurement.delta_velocity.z = report_msg.delta_velocity_z; + _measurement.joint_angles.x = report_msg.joint_roll; + _measurement.joint_angles.y = report_msg.joint_el; + _measurement.joint_angles.z = report_msg.joint_az; + + if (_calibrator.get_status() == ACCEL_CAL_COLLECTING_SAMPLE) { + _calibrator.new_sample(_measurement.delta_velocity,_measurement.delta_time); + } + + _measurement.delta_angles -= _gimbalParams.get_gyro_bias() * _measurement.delta_time; + _measurement.joint_angles -= _gimbalParams.get_joint_bias(); + _measurement.delta_velocity -= _gimbalParams.get_accel_bias() * _measurement.delta_time; + Vector3f accel_gain = _gimbalParams.get_accel_gain(); + _measurement.delta_velocity.x *= accel_gain.x == 0.0f ? 1.0f : accel_gain.x; + _measurement.delta_velocity.y *= accel_gain.y == 0.0f ? 1.0f : accel_gain.y; + _measurement.delta_velocity.z *= accel_gain.z == 0.0f ? 1.0f : accel_gain.z; + + // update _ang_vel_mag_filt, used for accel sample readiness + Vector3f ang_vel = _measurement.delta_angles / _measurement.delta_time; + Vector3f ekf_gyro_bias; + _ekf.getGyroBias(ekf_gyro_bias); + ang_vel -= ekf_gyro_bias; + float alpha = constrain_float(_measurement.delta_time/(_measurement.delta_time+0.5f),0.0f,1.0f); + _ang_vel_mag_filt += (ang_vel.length()-_ang_vel_mag_filt)*alpha; + _ang_vel_mag_filt = MIN(_ang_vel_mag_filt,20.0f); + + // get complementary filter inputs + _vehicle_to_gimbal_quat.from_vector312(_measurement.joint_angles.x,_measurement.joint_angles.y,_measurement.joint_angles.z); + + // update log deltas + _log_dt += _measurement.delta_time; + _log_del_ang += _measurement.delta_angles; + _log_del_vel += _measurement.delta_velocity; +} + +void SoloGimbal::update_estimators() +{ + if (_state == GIMBAL_STATE_NOT_PRESENT || _state == GIMBAL_STATE_PRESENT_INITIALIZING) { + return; + } + + // Run the gimbal attitude and gyro bias estimator + _ekf.RunEKF(_measurement.delta_time, _measurement.delta_angles, _measurement.delta_velocity, _measurement.joint_angles); + update_joint_angle_est(); +} + +void SoloGimbal::readVehicleDeltaAngle(uint8_t ins_index, Vector3f &dAng) { + const AP_InertialSensor &ins = _ahrs.get_ins(); + + if (ins_index < ins.get_gyro_count()) { + if (!ins.get_delta_angle(ins_index,dAng)) { + dAng = ins.get_gyro(ins_index) / ins.get_sample_rate(); + } + } +} + +void SoloGimbal::update_fast() { + const AP_InertialSensor &ins = _ahrs.get_ins(); + + if (ins.get_gyro_health(0) && ins.get_gyro_health(1)) { + // dual gyro mode - average first two gyros + Vector3f dAng; + readVehicleDeltaAngle(0, dAng); + _vehicle_delta_angles += dAng*0.5f; + readVehicleDeltaAngle(1, dAng); + _vehicle_delta_angles += dAng*0.5f; + } else { + // single gyro mode - one of the first two gyros are unhealthy or don't exist + // just read primary gyro + Vector3f dAng; + readVehicleDeltaAngle(ins.get_primary_gyro(), dAng); + _vehicle_delta_angles += dAng; + } +} + +void SoloGimbal::update_joint_angle_est() +{ + static const float tc = 1.0f; + float dt = _measurement.delta_time; + float alpha = constrain_float(dt/(dt+tc),0.0f,1.0f); + + Matrix3f Tvg; // vehicle frame to gimbal frame + _vehicle_to_gimbal_quat.inverse().rotation_matrix(Tvg); + + Vector3f delta_angle_bias; + _ekf.getGyroBias(delta_angle_bias); + delta_angle_bias *= dt; + + Vector3f joint_del_ang; + gimbal_ang_vel_to_joint_rates((_measurement.delta_angles-delta_angle_bias) - Tvg*_vehicle_delta_angles, joint_del_ang); + + _filtered_joint_angles += joint_del_ang; + _filtered_joint_angles += (_measurement.joint_angles-_filtered_joint_angles)*alpha; + + _vehicle_to_gimbal_quat_filt.from_vector312(_filtered_joint_angles.x,_filtered_joint_angles.y,_filtered_joint_angles.z); + + _vehicle_delta_angles.zero(); +} + +Vector3f SoloGimbal::get_ang_vel_dem_yaw(const Quaternion &quatEst) +{ + static const float tc = 0.1f; + static const float yawErrorLimit = radians(5.7f); + float dt = _measurement.delta_time; + float alpha = dt/(dt+tc); + + Matrix3f Tve = _ahrs.get_rotation_body_to_ned(); + Matrix3f Teg; + quatEst.inverse().rotation_matrix(Teg); + + + //_vehicle_yaw_rate_ef_filt = _ahrs.get_yaw_rate_earth(); + + // filter the vehicle yaw rate to remove noise + _vehicle_yaw_rate_ef_filt += (_ahrs.get_yaw_rate_earth() - _vehicle_yaw_rate_ef_filt) * alpha; + + float yaw_rate_ff = 0; + + // calculate an earth-frame yaw rate feed-forward that prevents gimbal from exceeding the maximum yaw error + if (_vehicle_yaw_rate_ef_filt > _gimbalParams.get_K_rate()*yawErrorLimit) { + yaw_rate_ff = _vehicle_yaw_rate_ef_filt-_gimbalParams.get_K_rate()*yawErrorLimit; + } else if (_vehicle_yaw_rate_ef_filt < -_gimbalParams.get_K_rate()*yawErrorLimit) { + yaw_rate_ff = _vehicle_yaw_rate_ef_filt+_gimbalParams.get_K_rate()*yawErrorLimit; + } + + // filter the feed-forward to remove noise + //_yaw_rate_ff_ef_filt += (yaw_rate_ff - _yaw_rate_ff_ef_filt) * alpha; + + Vector3f gimbalRateDemVecYaw; + gimbalRateDemVecYaw.z = yaw_rate_ff - _gimbalParams.get_K_rate() * _filtered_joint_angles.z / constrain_float(Tve.c.z,0.5f,1.0f); + gimbalRateDemVecYaw.z /= constrain_float(Tve.c.z,0.5f,1.0f); + + // rotate the rate demand into gimbal frame + gimbalRateDemVecYaw = Teg * gimbalRateDemVecYaw; + + return gimbalRateDemVecYaw; +} + +Vector3f SoloGimbal::get_ang_vel_dem_tilt(const Quaternion &quatEst) +{ + // Calculate the gimbal 321 Euler angle estimates relative to earth frame + Vector3f eulerEst = quatEst.to_vector312(); + + // Calculate a demanded quaternion using the demanded roll and pitch and estimated yaw (yaw is slaved to the vehicle) + Quaternion quatDem; + quatDem.from_vector312( _att_target_euler_rad.x, + _att_target_euler_rad.y, + eulerEst.z); + + //divide the demanded quaternion by the estimated to get the error + Quaternion quatErr = quatDem / quatEst; + + // Convert to a delta rotation + quatErr.normalize(); + Vector3f deltaAngErr; + quatErr.to_axis_angle(deltaAngErr); + + // multiply the angle error vector by a gain to calculate a demanded gimbal rate required to control tilt + Vector3f gimbalRateDemVecTilt = deltaAngErr * _gimbalParams.get_K_rate(); + return gimbalRateDemVecTilt; +} + +Vector3f SoloGimbal::get_ang_vel_dem_feedforward(const 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 = _att_target_euler_rad.y - lastDem; + lastDem = _att_target_euler_rad.y; + + Vector3f gimbalRateDemVecForward; + gimbalRateDemVecForward.y = delta / _measurement.delta_time; + return gimbalRateDemVecForward; +} + +Vector3f SoloGimbal::get_ang_vel_dem_gyro_bias() +{ + Vector3f gyroBias; + _ekf.getGyroBias(gyroBias); + return gyroBias + _gimbalParams.get_gyro_bias(); +} + +Vector3f SoloGimbal::get_ang_vel_dem_body_lock() +{ + // Define rotation from vehicle to gimbal using a 312 rotation sequence + Matrix3f Tvg; + _vehicle_to_gimbal_quat_filt.inverse().rotation_matrix(Tvg); + + // multiply the joint angles by a gain to calculate a rate vector required to keep the joints centred + Vector3f gimbalRateDemVecBodyLock; + gimbalRateDemVecBodyLock = _filtered_joint_angles * -_gimbalParams.get_K_rate(); + + joint_rates_to_gimbal_ang_vel(gimbalRateDemVecBodyLock, gimbalRateDemVecBodyLock); + + // Add a feedforward term from vehicle gyros + gimbalRateDemVecBodyLock += Tvg * _ahrs.get_gyro(); + + return gimbalRateDemVecBodyLock; +} + +void SoloGimbal::update_target(Vector3f newTarget) +{ + // Low-pass filter + _att_target_euler_rad.y = _att_target_euler_rad.y + 0.02f*(newTarget.y - _att_target_euler_rad.y); + // Update tilt + _att_target_euler_rad.y = constrain_float(_att_target_euler_rad.y,radians(-90),radians(0)); +} + +void SoloGimbal::write_logs(DataFlash_Class* dataflash) +{ + if (dataflash == NULL) return; + + uint32_t tstamp = AP_HAL::millis(); + Vector3f eulerEst; + + Quaternion quatEst; + _ekf.getQuat(quatEst); + quatEst.to_euler(eulerEst.x, eulerEst.y, eulerEst.z); + + struct log_Gimbal1 pkt1 = { + LOG_PACKET_HEADER_INIT(LOG_GIMBAL1_MSG), + time_ms : tstamp, + delta_time : _log_dt, + delta_angles_x : _log_del_ang.x, + delta_angles_y : _log_del_ang.y, + delta_angles_z : _log_del_ang.z, + delta_velocity_x : _log_del_vel.x, + delta_velocity_y : _log_del_vel.y, + delta_velocity_z : _log_del_vel.z, + joint_angles_x : _measurement.joint_angles.x, + joint_angles_y : _measurement.joint_angles.y, + joint_angles_z : _measurement.joint_angles.z + }; + dataflash->WriteBlock(&pkt1, sizeof(pkt1)); + + struct log_Gimbal2 pkt2 = { + LOG_PACKET_HEADER_INIT(LOG_GIMBAL2_MSG), + time_ms : tstamp, + est_sta : (uint8_t) _ekf.getStatus(), + est_x : eulerEst.x, + est_y : eulerEst.y, + est_z : eulerEst.z, + rate_x : _ang_vel_dem_rads.x, + rate_y : _ang_vel_dem_rads.y, + rate_z : _ang_vel_dem_rads.z, + target_x: _att_target_euler_rad.x, + target_y: _att_target_euler_rad.y, + target_z: _att_target_euler_rad.z + }; + dataflash->WriteBlock(&pkt2, sizeof(pkt2)); + + _log_dt = 0; + _log_del_ang.zero(); + _log_del_vel.zero(); +} + +bool SoloGimbal::joints_near_limits() +{ + return fabsf(_measurement.joint_angles.x) > radians(40) || _measurement.joint_angles.y > radians(45) || _measurement.joint_angles.y < -radians(135); +} + +AccelCalibrator* SoloGimbal::_acal_get_calibrator(uint8_t instance) +{ + if(instance==0 && (present() || _calibrator.get_status() == ACCEL_CAL_SUCCESS)) { + return &_calibrator; + } else { + return NULL; + } +} + +bool SoloGimbal::_acal_get_ready_to_sample() +{ + return _ang_vel_mag_filt < radians(10); +} + +bool SoloGimbal::_acal_get_saving() +{ + return _gimbalParams.flashing(); +} + +void SoloGimbal::_acal_save_calibrations() +{ + if (_calibrator.get_status() != ACCEL_CAL_SUCCESS) { + return; + } + Vector3f bias; + Vector3f gain; + _calibrator.get_calibration(bias,gain); + _gimbalParams.set_accel_bias(bias); + _gimbalParams.set_accel_gain(gain); + _gimbalParams.flash(); +} + +void SoloGimbal::gimbal_ang_vel_to_joint_rates(const Vector3f& ang_vel, Vector3f& joint_rates) +{ + float sin_theta = sinf(_measurement.joint_angles.y); + float cos_theta = cosf(_measurement.joint_angles.y); + + float sin_phi = sinf(_measurement.joint_angles.x); + float cos_phi = cosf(_measurement.joint_angles.x); + float sec_phi = 1.0f/cos_phi; + float tan_phi = sin_phi/cos_phi; + + joint_rates.x = ang_vel.x*cos_theta+ang_vel.z*sin_theta; + joint_rates.y = ang_vel.x*sin_theta*tan_phi-ang_vel.z*cos_theta*tan_phi+ang_vel.y; + joint_rates.z = sec_phi*(ang_vel.z*cos_theta-ang_vel.x*sin_theta); +} + +void SoloGimbal::joint_rates_to_gimbal_ang_vel(const Vector3f& joint_rates, Vector3f& ang_vel) +{ + float sin_theta = sinf(_measurement.joint_angles.y); + float cos_theta = cosf(_measurement.joint_angles.y); + + float sin_phi = sinf(_measurement.joint_angles.x); + float cos_phi = cosf(_measurement.joint_angles.x); + + ang_vel.x = cos_theta*joint_rates.x-sin_theta*cos_phi*joint_rates.z; + ang_vel.y = joint_rates.y + sin_phi*joint_rates.z; + ang_vel.z = sin_theta*joint_rates.x+cos_theta*cos_phi*joint_rates.z; +} + +#endif // AP_AHRS_NAVEKF_AVAILABLE diff --git a/libraries/AP_Mount/SoloGimbal.h b/libraries/AP_Mount/SoloGimbal.h new file mode 100644 index 0000000000..e195505e33 --- /dev/null +++ b/libraries/AP_Mount/SoloGimbal.h @@ -0,0 +1,160 @@ +// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*- + +/************************************************************ +* SoloGimbal -- library to control a 3 axis rate gimbal. * +* * +* Author: Arthur Benemann, Paul Riseborough; * +* * +************************************************************/ +#ifndef __SOLOGIMBAL_H__ +#define __SOLOGIMBAL_H__ + +#include +#include +#if AP_AHRS_NAVEKF_AVAILABLE + +#include "AP_Mount.h" +#include "SoloGimbalEKF.h" +#include +#include +#include +#include +#include +#include + +#include "SoloGimbal_Parameters.h" + +enum gimbal_state_t { + GIMBAL_STATE_NOT_PRESENT = 0, + GIMBAL_STATE_PRESENT_INITIALIZING, + GIMBAL_STATE_PRESENT_ALIGNING, + GIMBAL_STATE_PRESENT_RUNNING +}; + +enum gimbal_mode_t { + GIMBAL_MODE_IDLE = 0, + GIMBAL_MODE_POS_HOLD, + GIMBAL_MODE_POS_HOLD_FF, + GIMBAL_MODE_STABILIZE +}; + +class SoloGimbal : AP_AccelCal_Client +{ +public: + //Constructor + SoloGimbal(const AP_AHRS_NavEKF &ahrs) : + _ekf(ahrs), + _ahrs(ahrs), + _state(GIMBAL_STATE_NOT_PRESENT), + _yaw_rate_ff_ef_filt(0.0f), + _vehicle_yaw_rate_ef_filt(0.0f), + _lockedToBody(false), + _vehicle_delta_angles(), + _vehicle_to_gimbal_quat(), + _vehicle_to_gimbal_quat_filt(), + _filtered_joint_angles(), + _max_torque(5000.0f), + _log_dt(0), + _log_del_ang(), + _log_del_vel() + { + AP_AccelCal::register_client(this); + } + + void update_target(Vector3f newTarget); + void receive_feedback(mavlink_channel_t chan, mavlink_message_t *msg); + + void update_fast(); + + bool present(); + bool aligned(); + + void set_lockedToBody(bool val) { _lockedToBody = val; } + + void write_logs(DataFlash_Class* dataflash); + + float get_log_dt() { return _log_dt; } + + void disable_torque_report() { _gimbalParams.set_param(GMB_PARAM_GMB_SND_TORQUE, 0); } + void fetch_params() { _gimbalParams.fetch_params(); } + + void handle_param_value(DataFlash_Class *dataflash, mavlink_message_t *msg) { + _gimbalParams.handle_param_value(dataflash, msg); + } + +private: + // private methods + void update_estimators(); + void send_controls(mavlink_channel_t chan); + void extract_feedback(const mavlink_gimbal_report_t& report_msg); + void update_joint_angle_est(); + + Vector3f get_ang_vel_dem_yaw(const Quaternion &quatEst); + Vector3f get_ang_vel_dem_tilt(const Quaternion &quatEst); + Vector3f get_ang_vel_dem_feedforward(const Quaternion &quatEst); + Vector3f get_ang_vel_dem_gyro_bias(); + Vector3f get_ang_vel_dem_body_lock(); + + void gimbal_ang_vel_to_joint_rates(const Vector3f& ang_vel, Vector3f& joint_rates); + void joint_rates_to_gimbal_ang_vel(const Vector3f& joint_rates, Vector3f& ang_vel); + + void readVehicleDeltaAngle(uint8_t ins_index, Vector3f &dAng); + + void _acal_save_calibrations(); + bool _acal_get_ready_to_sample(); + bool _acal_get_saving(); + AccelCalibrator* _acal_get_calibrator(uint8_t instance); + + gimbal_mode_t get_mode(); + + bool joints_near_limits(); + + // private member variables + gimbal_state_t _state; + + struct { + float delta_time; + Vector3f delta_angles; + Vector3f delta_velocity; + Vector3f joint_angles; + } _measurement; + + float _yaw_rate_ff_ef_filt; + float _vehicle_yaw_rate_ef_filt; + + static const uint8_t _compid = MAV_COMP_ID_GIMBAL; + + // joint angle filter states + Vector3f _vehicle_delta_angles; + + Quaternion _vehicle_to_gimbal_quat; + Quaternion _vehicle_to_gimbal_quat_filt; + Vector3f _filtered_joint_angles; + + uint32_t _last_report_msg_ms; + + float _max_torque; + + float _ang_vel_mag_filt; + + mavlink_channel_t _chan; + + Vector3f _ang_vel_dem_rads; // rad/s + Vector3f _att_target_euler_rad; // desired earth-frame roll, tilt and pan angles in radians + + bool _lockedToBody; + + SoloGimbalEKF _ekf; // state of small EKF for gimbal + const AP_AHRS_NavEKF &_ahrs; // Main EKF + SoloGimbal_Parameters _gimbalParams; + + AccelCalibrator _calibrator; + + float _log_dt; + Vector3f _log_del_ang; + Vector3f _log_del_vel; +}; + +#endif // AP_AHRS_NAVEKF_AVAILABLE + +#endif // __AP_MOUNT_H__ diff --git a/libraries/AP_Mount/SoloGimbal_Parameters.cpp b/libraries/AP_Mount/SoloGimbal_Parameters.cpp new file mode 100644 index 0000000000..fddb388d9a --- /dev/null +++ b/libraries/AP_Mount/SoloGimbal_Parameters.cpp @@ -0,0 +1,278 @@ +#include "SoloGimbal_Parameters.h" +#include + +#include + +extern const AP_HAL::HAL& hal; + +const uint32_t SoloGimbal_Parameters::_retry_period = 3000; +const uint8_t SoloGimbal_Parameters::_max_fetch_attempts = 5; + +SoloGimbal_Parameters::SoloGimbal_Parameters() +{ + reset(); +} + + +void SoloGimbal_Parameters::reset() +{ + memset(_params,0,sizeof(_params)); + _last_request_ms = 0; + _flashing_step = GMB_PARAM_NOT_FLASHING; +} + +const char* SoloGimbal_Parameters::get_param_name(gmb_param_t param) +{ + switch(param) { + case GMB_PARAM_GMB_OFF_ACC_X: + return "GMB_OFF_ACC_X"; + case GMB_PARAM_GMB_OFF_ACC_Y: + return "GMB_OFF_ACC_Y"; + case GMB_PARAM_GMB_OFF_ACC_Z: + return "GMB_OFF_ACC_Z"; + case GMB_PARAM_GMB_GN_ACC_X: + return "GMB_GN_ACC_X"; + case GMB_PARAM_GMB_GN_ACC_Y: + return "GMB_GN_ACC_Y"; + case GMB_PARAM_GMB_GN_ACC_Z: + return "GMB_GN_ACC_Z"; + case GMB_PARAM_GMB_OFF_GYRO_X: + return "GMB_OFF_GYRO_X"; + case GMB_PARAM_GMB_OFF_GYRO_Y: + return "GMB_OFF_GYRO_Y"; + case GMB_PARAM_GMB_OFF_GYRO_Z: + return "GMB_OFF_GYRO_Z"; + case GMB_PARAM_GMB_OFF_JNT_X: + return "GMB_OFF_JNT_X"; + case GMB_PARAM_GMB_OFF_JNT_Y: + return "GMB_OFF_JNT_Y"; + case GMB_PARAM_GMB_OFF_JNT_Z: + return "GMB_OFF_JNT_Z"; + case GMB_PARAM_GMB_K_RATE: + return "GMB_K_RATE"; + case GMB_PARAM_GMB_POS_HOLD: + return "GMB_POS_HOLD"; + case GMB_PARAM_GMB_MAX_TORQUE: + return "GMB_MAX_TORQUE"; + case GMB_PARAM_GMB_SND_TORQUE: + return "GMB_SND_TORQUE"; + case GMB_PARAM_GMB_SYSID: + return "GMB_SYSID"; + case GMB_PARAM_GMB_FLASH: + return "GMB_FLASH"; + default: + return ""; + }; +} + +void SoloGimbal_Parameters::fetch_params() +{ + for(uint8_t i=0; i _retry_period) { + _last_request_ms = tnow_ms; + mavlink_msg_param_request_list_send(_chan, 0, MAV_COMP_ID_GIMBAL); + + for(uint8_t i=0; i _retry_period) { + mavlink_msg_param_set_send(_chan, 0, MAV_COMP_ID_GIMBAL, get_param_name((gmb_param_t)i), _params[i].value, MAV_PARAM_TYPE_REAL32); + if (!_params[i].seen) { + _params[i].fetch_attempts++; + } + } + } + + // check for nonexistant parameters + for(uint8_t i=0; i _max_fetch_attempts) { + _params[i].state = GMB_PARAMSTATE_NONEXISTANT; + hal.console->printf("Gimbal parameter %s timed out\n", get_param_name((gmb_param_t)i)); + } + } + + if(_flashing_step == GMB_PARAM_FLASHING_WAITING_FOR_SET) { + bool done = true; + for(uint8_t i=0; iLog_Write_Parameter(packet.param_id, packet.param_value); + } + + for(uint8_t i=0; i +#include +#include +#include + +enum gmb_param_state_t { + GMB_PARAMSTATE_NOT_YET_READ=0, // parameter has yet to be initialized + GMB_PARAMSTATE_FETCH_AGAIN=1, // parameter is being fetched + GMB_PARAMSTATE_ATTEMPTING_TO_SET=2, // parameter is being set + GMB_PARAMSTATE_CONSISTENT=3, // parameter is consistent + GMB_PARAMSTATE_NONEXISTANT=4 // parameter does not seem to exist +}; + +enum gmb_param_t { + GMB_PARAM_GMB_OFF_ACC_X=0, + GMB_PARAM_GMB_OFF_ACC_Y, + GMB_PARAM_GMB_OFF_ACC_Z, + GMB_PARAM_GMB_GN_ACC_X, + GMB_PARAM_GMB_GN_ACC_Y, + GMB_PARAM_GMB_GN_ACC_Z, + GMB_PARAM_GMB_OFF_GYRO_X, + GMB_PARAM_GMB_OFF_GYRO_Y, + GMB_PARAM_GMB_OFF_GYRO_Z, + GMB_PARAM_GMB_OFF_JNT_X, + GMB_PARAM_GMB_OFF_JNT_Y, + GMB_PARAM_GMB_OFF_JNT_Z, + GMB_PARAM_GMB_K_RATE, + GMB_PARAM_GMB_POS_HOLD, + GMB_PARAM_GMB_MAX_TORQUE, + GMB_PARAM_GMB_SND_TORQUE, + GMB_PARAM_GMB_SYSID, + GMB_PARAM_GMB_FLASH, + MAVLINK_GIMBAL_NUM_TRACKED_PARAMS +}; + +enum gmb_flashing_step_t { + GMB_PARAM_NOT_FLASHING=0, + GMB_PARAM_FLASHING_WAITING_FOR_SET, + GMB_PARAM_FLASHING_WAITING_FOR_ACK +}; + +class SoloGimbal_Parameters +{ +public: + SoloGimbal_Parameters(); + void reset(); + + bool initialized(); + bool received_all(); + void fetch_params(); + + void get_param(gmb_param_t param, float& value, float def_val = 0.0f); + void set_param(gmb_param_t param, float value); + + void update(); + void handle_param_value(DataFlash_Class *dataflash, mavlink_message_t *msg); + + Vector3f get_accel_bias(); + Vector3f get_accel_gain(); + void set_accel_bias(const Vector3f& bias); + void set_accel_gain(const Vector3f& gain); + Vector3f get_gyro_bias(); + void set_gyro_bias(const Vector3f& bias); + Vector3f get_joint_bias(); + + float get_K_rate(); + void flash(); + bool flashing(); + + void set_channel(mavlink_channel_t chan) { _chan = chan; } + +private: + static const char* get_param_name(gmb_param_t param); + + static const uint32_t _retry_period; + static const uint8_t _max_fetch_attempts; + + struct { + float value; + gmb_param_state_t state; + uint8_t fetch_attempts; + bool seen; + } _params[MAVLINK_GIMBAL_NUM_TRACKED_PARAMS]; + + uint32_t _last_request_ms; + gmb_flashing_step_t _flashing_step; + + mavlink_channel_t _chan; +}; + + +#endif // __SOLOGIMBAL_PARAMETERS__