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

AP_Gimbal: disable gimbal motors if copter is fliped

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();
decode_feedback(msg);
update_state();
if (_ekf.getStatus()){
if (_ekf.getStatus() && !isCopterFliped()){
send_control(chan);
}
@ -177,3 +177,7 @@ void AP_Gimbal::update_targets_from_rc()
// 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;
}

2
libraries/AP_Gimbal/AP_Gimbal.h
View File

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