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

AP_Gimbal: Move gimbal parameters to a structure

Browse Source
This commit is contained in:
Arthur Benemann 2015-03-30 15:46:19 -07:00 committed by Randy Mackay
parent 9b94f26583
commit cf2377d113
2 changed files with 11 additions and 26 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

20
libraries/AP_Gimbal/AP_Gimbal.cpp
View File

@ -6,13 +6,11 @@
#include <GCS.h>
#include <AP_SmallEKF.h>
static float K_gimbalRate = 5.0f;
void AP_Gimbal::receive_feedback(mavlink_channel_t chan, mavlink_message_t *msg)
{
decode_feedback(msg);
update_state();
if (_ekf.getStatus() && !isCopterFliped()){
if (_ekf.getStatus() && !isCopterFliped() && _gimbalParams.K_gimbalRate!=0.0f){
send_control(chan);
}
@ -44,7 +42,9 @@ void AP_Gimbal::decode_feedback(mavlink_message_t *msg)
_measurament.joint_angles.z = report_msg.joint_az;
//apply joint angle compensation
_measurament.joint_angles -= _joint_offsets;
_measurament.joint_angles -= _gimbalParams.joint_angles_offsets;
_measurament.delta_velocity -= _gimbalParams.delta_velocity_offsets;
_measurament.delta_angles -= _gimbalParams.delta_angles_offsets;
}
void AP_Gimbal::update_state()
@ -74,14 +74,14 @@ Vector3f AP_Gimbal::getGimbalRateDemVecYaw(Quaternion quatEst)
// multiply the yaw joint angle by a gain to calculate a demanded vehicle frame relative rate vector required to keep the yaw joint centred
Vector3f gimbalRateDemVecYaw;
gimbalRateDemVecYaw.z = - K_gimbalRate * _measurament.joint_angles.z;
gimbalRateDemVecYaw.z = - _gimbalParams.K_gimbalRate * _measurament.joint_angles.z;
// Get filtered vehicle turn rate in earth frame
vehicleYawRateFilt = (1.0f - yawRateFiltPole * _measurament.delta_time) * vehicleYawRateFilt + yawRateFiltPole * _measurament.delta_time * _ahrs.get_yaw_rate_earth();
Vector3f vehicle_rate_ef(0,0,vehicleYawRateFilt);
// calculate the maximum steady state rate error corresponding to the maximum permitted yaw angle error
float maxRate = K_gimbalRate * yawErrorLimit;
float maxRate = _gimbalParams.K_gimbalRate * yawErrorLimit;
float vehicle_rate_mag_ef = vehicle_rate_ef.length();
float excess_rate_correction = fabs(vehicle_rate_mag_ef) - maxRate;
if (vehicle_rate_mag_ef > maxRate) {
@ -126,7 +126,7 @@ Vector3f AP_Gimbal::getGimbalRateDemVecTilt(Quaternion quatEst)
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 * K_gimbalRate;
Vector3f gimbalRateDemVecTilt = deltaAngErr * _gimbalParams.K_gimbalRate;
return gimbalRateDemVecTilt;
}
@ -157,12 +157,6 @@ void AP_Gimbal::send_control(mavlink_channel_t chan)
gimbalRateDemVec.x, gimbalRateDemVec.y, gimbalRateDemVec.z);
}
void AP_Gimbal::update_failsafe(uint8_t failsafe)
{
_failsafe = failsafe;
}
void AP_Gimbal::update_target(Vector3f newTarget)
{
// Low-pass filter

17
libraries/AP_Gimbal/AP_Gimbal.h
View File

@ -21,28 +21,26 @@
#include <GCS_MAVLink.h>
#include <AP_SmallEKF.h>
#include <AP_NavEKF.h>
#include <AP_Mount.h>
class AP_Gimbal
{
public:
//Constructor
AP_Gimbal(const AP_AHRS_NavEKF &ahrs, uint8_t sysid, uint8_t compid) :
AP_Gimbal(const AP_AHRS_NavEKF &ahrs, uint8_t sysid, uint8_t compid, const AP_Mount::gimbal_params &gimbalParams) :
_ekf(ahrs),
_ahrs(ahrs),
K_gimbalRate(5.0f),
_joint_offsets(0.0f,0.0f,0.0f),
vehicleYawRateFilt(0.0f),
yawRateFiltPole(10.0f),
yawErrorLimit(0.1f),
_failsafe(true)
_gimbalParams(gimbalParams)
{
_sysid = sysid;
_compid = compid;
}
void update_target(Vector3f newTarget);
void update_failsafe(uint8_t failsafe);
void receive_feedback(mavlink_channel_t chan, mavlink_message_t *msg);
struct Measurament {
@ -58,11 +56,7 @@ public:
Vector3f _angle_ef_target_rad; // desired earth-frame roll, tilt and pan angles in radians
private:
// K gain for the pointing loop
float const K_gimbalRate;
// These are corrections (in radians) applied to the to the gimbal joint (x,y,z = roll,pitch,yaw) measurements
Vector3f const _joint_offsets;
const AP_Mount::gimbal_params &_gimbalParams;
// filtered yaw rate from the vehicle
float vehicleYawRateFilt;
@ -77,9 +71,6 @@ private:
// reducing this makes the gimbal respond more to vehicle yaw disturbances
float const yawErrorLimit;
// status of the RC signal
uint8_t _failsafe;
uint8_t _sysid;
uint8_t _compid;