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

AP_Mount: revert AP_Math class change

Browse Source
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); decode_feedback(msg);
update_state(); 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); send_control(chan);
} }

4
libraries/AP_Mount/AP_Mount_Servo.cpp
View File

@ -154,13 +154,13 @@ void AP_Mount_Servo::stabilize()
// lead filter // lead filter
const Vector3f &gyro = _frontend._ahrs.get_gyro(); 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 // 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()); 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; _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 // Compute rate of change of euler pitch angle
float pitch_rate = _frontend._ahrs.cos_pitch() * gyro.y - _frontend._ahrs.sin_roll() * gyro.z; 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; _angle_bf_output_deg.y -= degrees(pitch_rate) * _state._pitch_stb_lead;