Skip to content

Commit

Permalink
AP_Gimbal: disable gimbal motors if copter is fliped
Browse files Browse the repository at this point in the history
  • Loading branch information
arthurbenemann committed Mar 10, 2015
1 parent 02c3981 commit 9e791de
Show file tree
Hide file tree
Showing 2 changed files with 7 additions and 1 deletion.
6 changes: 5 additions & 1 deletion libraries/AP_Gimbal/AP_Gimbal.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -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);
}

Expand Down Expand Up @@ -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 changes: 2 additions & 0 deletions libraries/AP_Gimbal/AP_Gimbal.h
Original file line number Diff line number Diff line change
Expand Up @@ -97,6 +97,8 @@ class AP_Gimbal
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);
Expand Down

0 comments on commit 9e791de

Please sign in to comment.