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

Gimbal: formatting fixes

Browse Source 
Also isCopterFlipped returns bool instead of uint8_t
No functional change
This commit is contained in:
Randy Mackay 2015-04-20 15:51:59 +09:00
parent db6bb295c7
commit e9b2153672
2 changed files with 30 additions and 33 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

34
libraries/AP_Mount/AP_Gimbal.cpp
View File

@ -23,7 +23,6 @@ void AP_Gimbal::receive_feedback(mavlink_channel_t chan, mavlink_message_t *msg)
//::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)
{
@ -32,13 +31,13 @@ void AP_Gimbal::decode_feedback(mavlink_message_t *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.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.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.y = report_msg.joint_el;
_measurement.joint_angles.z = report_msg.joint_az;
//apply joint angle compensation
@ -55,7 +54,7 @@ void AP_Gimbal::update_state()
// get the gimbal quaternion estimate
Quaternion quatEst;
_ekf.getQuat(quatEst);
// Add the control rate vectors
gimbalRateDemVec.zero();
gimbalRateDemVec += getGimbalRateDemVecYaw(quatEst);
@ -98,11 +97,11 @@ Vector3f AP_Gimbal::getGimbalRateDemVecYaw(const Quaternion &quatEst)
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_dcm_matrix().transposed()*Vector3f(0,0,excess_rate_correction);
}else{
gimbalRateDemVecYaw -= _ahrs.get_dcm_matrix().transposed()*Vector3f(0,0,excess_rate_correction);
}
}
gimbalRateDemVecYaw += _ahrs.get_dcm_matrix().transposed()*Vector3f(0,0,excess_rate_correction);
} else {
gimbalRateDemVecYaw -= _ahrs.get_dcm_matrix().transposed()*Vector3f(0,0,excess_rate_correction);
}
}
// rotate into gimbal frame to calculate the gimbal rate vector required to keep the yaw gimbal centred
gimbalRateDemVecYaw = Tvg * gimbalRateDemVecYaw;
@ -124,7 +123,7 @@ Vector3f AP_Gimbal::getGimbalRateDemVecTilt(const Quaternion &quatEst)
//divide the demanded quaternion by the estimated to get the error
Quaternion quatErr = quatDem / quatEst;
// Convert to a delta rotation using a small angle appoximation
// Convert to a delta rotation using a small angle approximation
quatErr.normalize();
Vector3f deltaAngErr;
float scaler;
@ -175,15 +174,18 @@ void AP_Gimbal::update_target(Vector3f newTarget)
_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);Vector3f eulerEst;quatEst.to_vector312(eulerEst.x, eulerEst.y, eulerEst.z);
Quaternion quatEst;
_ekf.getQuat(quatEst);
Vector3f eulerEst;
quatEst.to_vector312(eulerEst.x, eulerEst.y, eulerEst.z);
return eulerEst;
}
uint8_t AP_Gimbal::isCopterFlipped(){
return _ahrs.cos_roll()*_ahrs.cos_pitch() < 0.5f;
bool AP_Gimbal::isCopterFlipped()
{
return (_ahrs.cos_roll()*_ahrs.cos_pitch() < 0.5f);
}

29
libraries/AP_Mount/AP_Gimbal.h
View File

@ -1,15 +1,10 @@
// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
/************************************************************
* AP_Gimbal -- library to control a 3 axis rate gimbal. *
* *
* Author: Arthur Benemann, Paul Riseborough; *
* *
* Purpose: *
* *
* Usage: *
* *
* Comments: *
* AP_Gimbal -- library to control a 3 axis rate gimbal. *
* *
* Author: Arthur Benemann, Paul Riseborough; *
* *
************************************************************/
#ifndef __AP_GIMBAL_H__
#define __AP_GIMBAL_H__
@ -53,11 +48,11 @@ public:
} _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
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:
private:
const AP_Mount::gimbal_params &_gimbalParams;
// filtered yaw rate from the vehicle
@ -71,16 +66,16 @@ private:
// 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;
float const yawErrorLimit;
uint8_t _sysid;
uint8_t _compid;
uint8_t _sysid;
uint8_t _compid;
void send_control(mavlink_channel_t chan);
void update_state();
void decode_feedback(mavlink_message_t *msg);
uint8_t isCopterFlipped();
bool isCopterFlipped();
// Control loop functions
Vector3f getGimbalRateDemVecYaw(const Quaternion &quatEst);