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

AP_Gimbal: add a radio failsafe state to the gimbal

Browse Source
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); 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 // 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 angle_input_rad(RC_Channel* rc, float angle_min, float angle_max)
{ {
float input =rc->norm_input(); float input =rc->norm_input();
float angle = input*(angle_max - angle_min) + angle_min; float angle = input*(angle_max - angle_min) + angle_min;
return radians(angle); return radians(angle);
} }
// update_targets_from_rc - updates angle targets using input from receiver // update_targets_from_rc - updates angle targets using input from receiver
void AP_Gimbal::update_targets_from_rc() 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 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); 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; _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_rc_in(6),
_tilt_angle_min(-45.0f), _tilt_angle_min(-45.0f),
_tilt_angle_max(0.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); AP_Param::setup_object_defaults(this, var_info);
_sysid = sysid; _sysid = sysid;
_compid = compid; _compid = compid;
} }
// MAVLink methods void update_failsafe(uint8_t rc_failsafe);
void receive_feedback(mavlink_channel_t chan, mavlink_message_t *msg); void receive_feedback(mavlink_channel_t chan, mavlink_message_t *msg);
// hook for eeprom variables // hook for eeprom variables
@ -89,6 +90,8 @@ private:
uint8_t _compid; uint8_t _compid;
uint8_t _rc_failsafe; // status of the RC signal
void send_control(mavlink_channel_t chan); void send_control(mavlink_channel_t chan);
void update_state(); void update_state();
void decode_feedback(mavlink_message_t *msg); void decode_feedback(mavlink_message_t *msg);