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

AP_Mount: revert AP_Math class change

This commit is contained in:
Andrew Tridgell 2015-05-05 12:35:37 +10:00
parent 0b897e04bb
commit 7c9e3d4b58
2 changed files with 3 additions and 3 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
libraries/AP_Mount/AP_Gimbal.cpp
View File

@ -14,7 +14,7 @@ void AP_Gimbal::receive_feedback(mavlink_channel_t chan, mavlink_message_t *msg)
{
decode_feedback(msg);
update_state();
if (_ekf.getStatus() && !isCopterFlipped() && !AP_Math::is_zero(_gimbalParams.K_gimbalRate)){
if (_ekf.getStatus() && !isCopterFlipped() && !is_zero(_gimbalParams.K_gimbalRate)){
send_control(chan);
}

4
libraries/AP_Mount/AP_Mount_Servo.cpp
View File

@ -154,13 +154,13 @@ void AP_Mount_Servo::stabilize()
// lead filter
const Vector3f &gyro = _frontend._ahrs.get_gyro();
if (_state._stab_roll && !AP_Math::is_zero(_state._roll_stb_lead) && fabsf(_frontend._ahrs.pitch) < (float)M_PI/3.0f) {
if (_state._stab_roll && !is_zero(_state._roll_stb_lead) && fabsf(_frontend._ahrs.pitch) < (float)M_PI/3.0f) {
// Compute rate of change of euler roll angle
float roll_rate = gyro.x + (_frontend._ahrs.sin_pitch() / _frontend._ahrs.cos_pitch()) * (gyro.y * _frontend._ahrs.sin_roll() + gyro.z * _frontend._ahrs.cos_roll());
_angle_bf_output_deg.x -= degrees(roll_rate) * _state._roll_stb_lead;
}
if (_state._stab_tilt && !AP_Math::is_zero(_state._pitch_stb_lead)) {
if (_state._stab_tilt && !is_zero(_state._pitch_stb_lead)) {
// Compute rate of change of euler pitch angle
float pitch_rate = _frontend._ahrs.cos_pitch() * gyro.y - _frontend._ahrs.sin_roll() * gyro.z;
_angle_bf_output_deg.y -= degrees(pitch_rate) * _state._pitch_stb_lead;