Jonathan Challinger
9 years ago
committed by
Randy Mackay
11 changed files with 1153 additions and 422 deletions
@ -1,212 +0,0 @@
@@ -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 <stdio.h> |
||||
#include <AP_Common/AP_Common.h> |
||||
#include <GCS_MAVLink/GCS.h> |
||||
#include <AP_NavEKF/AP_SmallEKF.h> |
||||
#include <AP_Math/AP_Math.h> |
||||
|
||||
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
|
||||
@ -1,95 +0,0 @@
@@ -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 <AP_HAL/AP_HAL.h> |
||||
#include <AP_AHRS/AP_AHRS.h> |
||||
|
||||
#if AP_AHRS_NAVEKF_AVAILABLE |
||||
|
||||
#include <AP_Math/AP_Math.h> |
||||
#include <AP_Common/AP_Common.h> |
||||
#include <AP_GPS/AP_GPS.h> |
||||
#include <GCS_MAVLink/GCS_MAVLink.h> |
||||
#include "AP_Mount.h" |
||||
#include <AP_NavEKF/AP_SmallEKF.h> |
||||
#include <AP_NavEKF/AP_NavEKF.h> |
||||
|
||||
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__
|
||||
@ -0,0 +1,489 @@
@@ -0,0 +1,489 @@
|
||||
// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
|
||||
|
||||
#include <AP_HAL/AP_HAL.h> |
||||
#include <AP_AHRS/AP_AHRS.h> |
||||
#if AP_AHRS_NAVEKF_AVAILABLE |
||||
|
||||
#include "SoloGimbal.h" |
||||
|
||||
#include <stdio.h> |
||||
#include <GCS_MAVLink/GCS.h> |
||||
|
||||
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
|
||||
@ -0,0 +1,160 @@
@@ -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 <AP_HAL/AP_HAL.h> |
||||
#include <AP_AHRS/AP_AHRS.h> |
||||
#if AP_AHRS_NAVEKF_AVAILABLE |
||||
|
||||
#include "AP_Mount.h" |
||||
#include "SoloGimbalEKF.h" |
||||
#include <AP_Math/AP_Math.h> |
||||
#include <AP_Common/AP_Common.h> |
||||
#include <AP_GPS/AP_GPS.h> |
||||
#include <GCS_MAVLink/GCS_MAVLink.h> |
||||
#include <AP_NavEKF/AP_NavEKF.h> |
||||
#include <AP_AccelCal/AP_AccelCal.h> |
||||
|
||||
#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__
|
||||
@ -0,0 +1,278 @@
@@ -0,0 +1,278 @@
|
||||
#include "SoloGimbal_Parameters.h" |
||||
#include <AP_HAL/AP_HAL.h> |
||||
|
||||
#include <stdio.h> |
||||
|
||||
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<MAVLINK_GIMBAL_NUM_TRACKED_PARAMS; i++) { |
||||
if (_params[i].state != GMB_PARAMSTATE_NOT_YET_READ) { |
||||
_params[i].state = GMB_PARAMSTATE_FETCH_AGAIN; |
||||
} |
||||
} |
||||
} |
||||
|
||||
bool SoloGimbal_Parameters::initialized() |
||||
{ |
||||
for(uint8_t i=0; i<MAVLINK_GIMBAL_NUM_TRACKED_PARAMS; i++) { |
||||
if(_params[i].state == GMB_PARAMSTATE_NOT_YET_READ) { |
||||
return false; |
||||
} |
||||
} |
||||
return true; |
||||
} |
||||
|
||||
bool SoloGimbal_Parameters::received_all() |
||||
{ |
||||
for(uint8_t i=0; i<MAVLINK_GIMBAL_NUM_TRACKED_PARAMS; i++) { |
||||
if(_params[i].state == GMB_PARAMSTATE_NOT_YET_READ || _params[i].state == GMB_PARAMSTATE_FETCH_AGAIN) { |
||||
return false; |
||||
} |
||||
} |
||||
return true; |
||||
} |
||||
|
||||
void SoloGimbal_Parameters::get_param(gmb_param_t param, float& value, float def_val) { |
||||
if (!_params[param].seen) { |
||||
value = def_val; |
||||
} else { |
||||
value = _params[param].value; |
||||
} |
||||
} |
||||
|
||||
void SoloGimbal_Parameters::set_param(gmb_param_t param, float value) { |
||||
if ((_params[param].state == GMB_PARAMSTATE_CONSISTENT && param != GMB_PARAM_GMB_FLASH && _params[param].value == value) || _params[param].state == GMB_PARAMSTATE_NONEXISTANT) { |
||||
return; |
||||
} |
||||
|
||||
_params[param].state = GMB_PARAMSTATE_ATTEMPTING_TO_SET; |
||||
_params[param].value = value; |
||||
mavlink_msg_param_set_send(_chan, 0, MAV_COMP_ID_GIMBAL, get_param_name(param), _params[param].value, MAV_PARAM_TYPE_REAL32); |
||||
|
||||
_last_request_ms = AP_HAL::millis(); |
||||
} |
||||
|
||||
void SoloGimbal_Parameters::update() |
||||
{ |
||||
uint32_t tnow_ms = AP_HAL::millis(); |
||||
|
||||
// retry initial param retrieval
|
||||
if(!received_all()){ |
||||
if (tnow_ms-_last_request_ms > _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<MAVLINK_GIMBAL_NUM_TRACKED_PARAMS; i++) { |
||||
if (!_params[i].seen) { |
||||
_params[i].fetch_attempts++; |
||||
} |
||||
} |
||||
} |
||||
} |
||||
|
||||
// retry param_set
|
||||
for(uint8_t i=0; i<MAVLINK_GIMBAL_NUM_TRACKED_PARAMS; i++) { |
||||
if (_params[i].state == GMB_PARAMSTATE_ATTEMPTING_TO_SET && tnow_ms - _last_request_ms > _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<MAVLINK_GIMBAL_NUM_TRACKED_PARAMS; i++) { |
||||
if (!_params[i].seen && _params[i].fetch_attempts > _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; i<MAVLINK_GIMBAL_NUM_TRACKED_PARAMS; i++) { |
||||
if (_params[i].state == GMB_PARAMSTATE_ATTEMPTING_TO_SET) { |
||||
done = false; |
||||
break; |
||||
} |
||||
} |
||||
|
||||
if (done) { |
||||
_flashing_step = GMB_PARAM_FLASHING_WAITING_FOR_ACK; |
||||
set_param(GMB_PARAM_GMB_FLASH,69.0f); |
||||
} |
||||
} |
||||
} |
||||
|
||||
void SoloGimbal_Parameters::handle_param_value(DataFlash_Class *dataflash, mavlink_message_t *msg) |
||||
{ |
||||
mavlink_param_value_t packet; |
||||
mavlink_msg_param_value_decode(msg, &packet); |
||||
|
||||
if (dataflash != NULL) { |
||||
dataflash->Log_Write_Parameter(packet.param_id, packet.param_value); |
||||
} |
||||
|
||||
for(uint8_t i=0; i<MAVLINK_GIMBAL_NUM_TRACKED_PARAMS; i++) { |
||||
if (!strcmp(packet.param_id, get_param_name((gmb_param_t)i))) { |
||||
_params[i].seen = true; |
||||
switch(_params[i].state) { |
||||
case GMB_PARAMSTATE_NONEXISTANT: |
||||
case GMB_PARAMSTATE_NOT_YET_READ: |
||||
case GMB_PARAMSTATE_FETCH_AGAIN: |
||||
_params[i].value = packet.param_value; |
||||
_params[i].state = GMB_PARAMSTATE_CONSISTENT; |
||||
break; |
||||
case GMB_PARAMSTATE_CONSISTENT: |
||||
_params[i].value = packet.param_value; |
||||
break; |
||||
case GMB_PARAMSTATE_ATTEMPTING_TO_SET: |
||||
if (i == GMB_PARAM_GMB_FLASH) { |
||||
if (_flashing_step == GMB_PARAM_FLASHING_WAITING_FOR_ACK && packet.param_value == 1) { |
||||
_flashing_step = GMB_PARAM_NOT_FLASHING; |
||||
} |
||||
_params[i].value = 0; |
||||
_params[i].state = GMB_PARAMSTATE_CONSISTENT; |
||||
} else if (packet.param_value == _params[i].value) { |
||||
_params[i].state = GMB_PARAMSTATE_CONSISTENT; |
||||
} |
||||
break; |
||||
} |
||||
break; |
||||
} |
||||
} |
||||
} |
||||
|
||||
Vector3f SoloGimbal_Parameters::get_accel_bias() |
||||
{ |
||||
Vector3f ret; |
||||
get_param(GMB_PARAM_GMB_OFF_ACC_X,ret.x); |
||||
get_param(GMB_PARAM_GMB_OFF_ACC_Y,ret.y); |
||||
get_param(GMB_PARAM_GMB_OFF_ACC_Z,ret.z); |
||||
return ret; |
||||
} |
||||
Vector3f SoloGimbal_Parameters::get_accel_gain() |
||||
{ |
||||
Vector3f ret; |
||||
get_param(GMB_PARAM_GMB_GN_ACC_X,ret.x,1.0f); |
||||
get_param(GMB_PARAM_GMB_GN_ACC_Y,ret.y,1.0f); |
||||
get_param(GMB_PARAM_GMB_GN_ACC_Z,ret.z,1.0f); |
||||
return ret; |
||||
} |
||||
|
||||
void SoloGimbal_Parameters::set_accel_bias(const Vector3f& bias) |
||||
{ |
||||
set_param(GMB_PARAM_GMB_OFF_ACC_X, bias.x); |
||||
set_param(GMB_PARAM_GMB_OFF_ACC_Y, bias.y); |
||||
set_param(GMB_PARAM_GMB_OFF_ACC_Z, bias.z); |
||||
} |
||||
|
||||
void SoloGimbal_Parameters::set_accel_gain(const Vector3f& gain) |
||||
{ |
||||
set_param(GMB_PARAM_GMB_GN_ACC_X, gain.x); |
||||
set_param(GMB_PARAM_GMB_GN_ACC_Y, gain.y); |
||||
set_param(GMB_PARAM_GMB_GN_ACC_Z, gain.z); |
||||
} |
||||
|
||||
Vector3f SoloGimbal_Parameters::get_gyro_bias() |
||||
{ |
||||
Vector3f ret; |
||||
get_param(GMB_PARAM_GMB_OFF_GYRO_X,ret.x); |
||||
get_param(GMB_PARAM_GMB_OFF_GYRO_Y,ret.y); |
||||
get_param(GMB_PARAM_GMB_OFF_GYRO_Z,ret.z); |
||||
return ret; |
||||
} |
||||
|
||||
void SoloGimbal_Parameters::set_gyro_bias(const Vector3f& bias) |
||||
{ |
||||
set_param(GMB_PARAM_GMB_OFF_GYRO_X,bias.x); |
||||
set_param(GMB_PARAM_GMB_OFF_GYRO_Y,bias.y); |
||||
set_param(GMB_PARAM_GMB_OFF_GYRO_Z,bias.z); |
||||
} |
||||
|
||||
Vector3f SoloGimbal_Parameters::get_joint_bias() |
||||
{ |
||||
Vector3f ret; |
||||
get_param(GMB_PARAM_GMB_OFF_JNT_X,ret.x); |
||||
get_param(GMB_PARAM_GMB_OFF_JNT_Y,ret.y); |
||||
get_param(GMB_PARAM_GMB_OFF_JNT_Z,ret.z); |
||||
return ret; |
||||
} |
||||
float SoloGimbal_Parameters::get_K_rate() |
||||
{ |
||||
float ret; |
||||
get_param(GMB_PARAM_GMB_K_RATE,ret); |
||||
return ret; |
||||
} |
||||
|
||||
void SoloGimbal_Parameters::flash() |
||||
{ |
||||
_flashing_step = GMB_PARAM_FLASHING_WAITING_FOR_SET; |
||||
} |
||||
|
||||
bool SoloGimbal_Parameters::flashing() |
||||
{ |
||||
return _flashing_step != GMB_PARAM_NOT_FLASHING; |
||||
} |
||||
@ -0,0 +1,94 @@
@@ -0,0 +1,94 @@
|
||||
#ifndef __SOLOGIMBAL_PARAMETERS__ |
||||
#define __SOLOGIMBAL_PARAMETERS__ |
||||
#include <AP_Math/AP_Math.h> |
||||
#include <AP_Common/AP_Common.h> |
||||
#include <GCS_MAVLink/GCS_MAVLink.h> |
||||
#include <DataFlash/DataFlash.h> |
||||
|
||||
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__
|
||||
Loading…
Reference in new issue