Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

AP_Gimbal: add a radio failsafe state to the gimbal

This commit is contained in:
Arthur Benemann 2015-03-10 14:48:42 -07:00 committed by Randy Mackay
parent 7cf883a61c
commit 534790634b
2 changed files with 12 additions and 4 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

9
libraries/AP_Gimbal/AP_Gimbal.cpp
View File

@ -150,19 +150,24 @@ void AP_Gimbal::send_control(mavlink_channel_t chan)
gimbalRateDemVec.x, gimbalRateDemVec.y, gimbalRateDemVec.z);
}
void AP_Gimbal::update_failsafe(uint8_t rc_failsafe)
{
_rc_failsafe = rc_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()
{
float new_tilt = angle_input_rad(RC_Channel::rc_channel(tilt_rc_in-1), _tilt_angle_min, _tilt_angle_max);
float new_tilt = (_rc_failsafe)?0.0f:angle_input_rad(RC_Channel::rc_channel(tilt_rc_in-1), _tilt_angle_min, _tilt_angle_max);
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);
_angle_ef_target_rad.y += tilt_change;

7
libraries/AP_Gimbal/AP_Gimbal.h
View File

@ -37,14 +37,15 @@ public:
tilt_rc_in(6),
_tilt_angle_min(-45.0f),
_tilt_angle_max(0.0f),
_max_tilt_rate(0.5f)
_max_tilt_rate(0.5f),
_rc_failsafe(true)
{
AP_Param::setup_object_defaults(this, var_info);
_sysid = sysid;
_compid = compid;
}
// MAVLink methods
void update_failsafe(uint8_t rc_failsafe);
void receive_feedback(mavlink_channel_t chan, mavlink_message_t *msg);
// hook for eeprom variables
@ -89,6 +90,8 @@ private:
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);