Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-12 02:48:28 -04:00
Code Issues Packages Projects Releases Wiki Activity

AP_Gimbal: disable gimbal motors if copter is fliped

Browse Source
This commit is contained in:
Arthur Benemann 2015-03-10 15:48:15 -07:00 committed by Randy Mackay
parent ebaf1e28b1
commit 4bdae02cfd
2 changed files with 7 additions and 1 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

6
libraries/AP_Gimbal/AP_Gimbal.cpp
View File

@ -22,7 +22,7 @@ void AP_Gimbal::receive_feedback(mavlink_channel_t chan, mavlink_message_t *msg)
update_targets_from_rc(); update_targets_from_rc();
decode_feedback(msg); decode_feedback(msg);
update_state(); update_state();
if (_ekf.getStatus()){ if (_ekf.getStatus() && !isCopterFliped()){
send_control(chan); send_control(chan);
} }
@ -177,3 +177,7 @@ void AP_Gimbal::update_targets_from_rc()
// Update tilt // Update tilt
_angle_ef_target_rad.y = constrain_float(_angle_ef_target_rad.y + tilt_change,_tilt_angle_min,_tilt_angle_max); _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;
}

2
libraries/AP_Gimbal/AP_Gimbal.h
View File

@ -97,6 +97,8 @@ private:
void decode_feedback(mavlink_message_t *msg); void decode_feedback(mavlink_message_t *msg);
void update_targets_from_rc(); void update_targets_from_rc();
uint8_t isCopterFliped();
// Control loop functions // Control loop functions
Vector3f getGimbalRateDemVecYaw(Quaternion quatEst); Vector3f getGimbalRateDemVecYaw(Quaternion quatEst);
Vector3f getGimbalRateDemVecTilt(Quaternion quatEst); Vector3f getGimbalRateDemVecTilt(Quaternion quatEst);