aepko/Ardupilot2
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

AP_Mount: merge SoloGimbal from solo master

Browse Source
This commit is contained in:
Jonathan Challinger 2015-12-30 18:19:52 -08:00 committed by Randy Mackay
parent eabede692e
commit 5b834330cb
11 changed files with 1149 additions and 418 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

212
libraries/AP_Mount/AP_Gimbal.cpp
View File

@ -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

95
libraries/AP_Mount/AP_Gimbal.h
View File

@ -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__

108
libraries/AP_Mount/AP_Mount.cpp
View File

@ -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
// 20 formerly _OFF_JNT
// @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
// 21 formerly _OFF_ACC
// @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),
// 22 formerly _OFF_GYRO
// @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
// 23 formerly _K_RATE
// @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
// 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 &current_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; instance<AP_MOUNT_MAX_INSTANCES; instance++) {
if (_backends[instance] != NULL) {
_backends[instance]->update_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; instance<AP_MOUNT_MAX_INSTANCES; instance++) {
if (_backends[instance] != NULL) {
_backends[instance]->handle_param_value(msg);
}
}
}
// send a GIMBAL_REPORT message to the GCS

27
libraries/AP_Mount/AP_Mount.h
View File

@ -29,6 +29,7 @@
#include <GCS_MAVLink/GCS_MAVLink.h>
#include <RC_Channel/RC_Channel.h>
#include <AP_SerialManager/AP_SerialManager.h>
#include <DataFlash/DataFlash.h>
// 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 &current_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__

6
libraries/AP_Mount/AP_Mount_Backend.h
View File

@ -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) {}

73
libraries/AP_Mount/AP_Mount_MAVLink.cpp → libraries/AP_Mount/AP_Mount_SoloGimbal.cpp
View File

@ -1,30 +1,40 @@
// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
#include "AP_Mount_MAVLink.h"
#include <AP_HAL/AP_HAL.h>
#include <AP_AHRS/AP_AHRS.h>
#if AP_AHRS_NAVEKF_AVAILABLE
#include "AP_Mount_SoloGimbal.h"
#include "SoloGimbal.h"
#include <DataFlash/DataFlash.h>
#include <GCS_MAVLink/GCS_MAVLink.h>
#include <stdio.h>
#include "AP_Gimbal.h"
#if MOUNT_DEBUG
#include <stdio.h>
#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

25
libraries/AP_Mount/AP_Mount_MAVLink.h → libraries/AP_Mount/AP_Mount_SoloGimbal.h
View File

@ -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 <AP_HAL/AP_HAL.h>
#include <AP_AHRS/AP_AHRS.h>
@ -17,14 +18,15 @@
#include <GCS_MAVLink/GCS_MAVLink.h>
#include <RC_Channel/RC_Channel.h>
#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__

489
libraries/AP_Mount/SoloGimbal.cpp Normal file
View File

@ -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

160
libraries/AP_Mount/SoloGimbal.h Normal file
View File

@ -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__

278
libraries/AP_Mount/SoloGimbal_Parameters.cpp Normal file
View File

@ -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;
}

94
libraries/AP_Mount/SoloGimbal_Parameters.h Normal file
View File

@ -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__