AP_Gimbal: clean-up for AP_Mount merge

This commit is contained in:
Arthur Benemann 2015-03-20 10:32:38 -07:00 committed by Randy Mackay
parent 4ad3e786a5
commit ee8c92c850
2 changed files with 15 additions and 59 deletions

View File

@ -2,24 +2,15 @@
#include <stdio.h>
#include <AP_Common.h>
#include <AP_Progmem.h>
#include <AP_Param.h>
#include <AP_Gimbal.h>
#include <GCS.h>
#include <GCS_MAVLink.h>
#include <AP_SmallEKF.h>
const AP_Param::GroupInfo AP_Gimbal::var_info[] PROGMEM = {
AP_GROUPEND
};
uint16_t feedback_error_count;
static float K_gimbalRate = 5.0f;
static float angRateLimit = 0.5f;
void AP_Gimbal::receive_feedback(mavlink_channel_t chan, mavlink_message_t *msg)
{
update_targets_from_rc();
decode_feedback(msg);
update_state();
if (_ekf.getStatus() && !isCopterFliped()){
@ -168,32 +159,11 @@ void AP_Gimbal::send_control(mavlink_channel_t chan)
}
void AP_Gimbal::update_failsafe(uint8_t rc_failsafe)
void AP_Gimbal::update_failsafe(uint8_t failsafe)
{
_rc_failsafe = rc_failsafe;
_failsafe = failsafe;
}
// returns the angle (radians) that the RC_Channel input is receiving
float angle_input_rad(RC_Channel* rc, float angle_min, float angle_max)
{
float input =rc->norm_input();
float angle = input*(angle_max - angle_min) + angle_min;
return radians(angle);
}
// update_targets_from_rc - updates angle targets using input from receiver
void AP_Gimbal::update_targets_from_rc()
{
// Get new tilt angle
float new_tilt = (_rc_failsafe)?0.0f:angle_input_rad(RC_Channel::rc_channel(tilt_rc_in-1), _tilt_angle_min, _tilt_angle_max);
// Low-pass filter
new_tilt = _angle_ef_target_rad.y + 0.09f*(new_tilt - _angle_ef_target_rad.y);
// Slew-rate constrain
float max_change_rads =_max_tilt_rate * _measurament.delta_time;
float tilt_change = constrain_float(new_tilt - _angle_ef_target_rad.y,-max_change_rads,+max_change_rads);
// Update tilt
_angle_ef_target_rad.y = constrain_float(_angle_ef_target_rad.y + tilt_change,_tilt_angle_min,_tilt_angle_max);
}
uint8_t AP_Gimbal::isCopterFliped(){
return fabs(_ahrs.roll)>1.0f || fabs(_ahrs.pitch)>1.0f;

View File

@ -28,29 +28,21 @@ class AP_Gimbal
public:
//Constructor
AP_Gimbal(const AP_AHRS_NavEKF &ahrs, uint8_t sysid, uint8_t compid) :
_ahrs(ahrs),
_ekf(ahrs),
_ahrs(ahrs),
_joint_offsets(0.0f,0.0f,0.0f),
vehicleYawRateFilt(0.0f),
yawRateFiltPole(10.0f),
yawErrorLimit(0.1f),
tilt_rc_in(6),
_tilt_angle_min(-45.0f),
_tilt_angle_max(0.0f),
_max_tilt_rate(0.5f),
_rc_failsafe(true)
_failsafe(true)
{
AP_Param::setup_object_defaults(this, var_info);
_sysid = sysid;
_compid = compid;
}
void update_failsafe(uint8_t rc_failsafe);
void update_failsafe(uint8_t failsafe);
void receive_feedback(mavlink_channel_t chan, mavlink_message_t *msg);
// hook for eeprom variables
static const struct AP_Param::GroupInfo var_info[];
struct Measurament {
float delta_time;
Vector3f delta_angles;
@ -59,10 +51,15 @@ public:
} _measurament;
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:
// These are corrections (in radians) applied to the to the gimbal joint (x,y,z = roll,pitch,yaw) measurements
Vector3f const _joint_offsets;
// filtered yaw rate from the vehicle
float vehicleYawRateFilt;
@ -76,26 +73,15 @@ private:
// reducing this makes the gimbal respond more to vehicle yaw disturbances
float const yawErrorLimit;
// These are corrections (in radians) applied to the to the gimbal joint (x,y,z = roll,pitch,yaw) measurements
Vector3f const _joint_offsets;
// status of the RC signal
uint8_t _failsafe;
uint8_t const tilt_rc_in;
float const _tilt_angle_min; // min tilt in 0.01 degree units
float const _tilt_angle_max; // max tilt in 0.01 degree units
float const _max_tilt_rate; // max tilt rate in rad/s
const AP_AHRS_NavEKF &_ahrs; // Main EKF
uint8_t _sysid;
uint8_t _compid;
uint8_t _rc_failsafe; // status of the RC signal
void send_control(mavlink_channel_t chan);
void update_state();
void decode_feedback(mavlink_message_t *msg);
void update_targets_from_rc();
uint8_t isCopterFliped();