diff --git a/libraries/AP_AHRS/AP_AHRS_DCM.cpp b/libraries/AP_AHRS/AP_AHRS_DCM.cpp index 323d1bb338b27ccf897158d94e029bb2cd87007a..b6fa2e1f4596de5ac8f194fd5388c3fa605d2435 100644 --- a/libraries/AP_AHRS/AP_AHRS_DCM.cpp +++ b/libraries/AP_AHRS/AP_AHRS_DCM.cpp @@ -495,25 +495,27 @@ AP_AHRS_DCM::euler_angles(void) // average error_roll_pitch since last call float AP_AHRS_DCM::get_error_rp(void) { - float ret; if (_error_rp_count == 0) { - return 0; + // this happens when telemetry is setup on two + // serial ports + return _error_rp_last; } - ret = _error_rp_sum / _error_rp_count; + _error_rp_last = _error_rp_sum / _error_rp_count; _error_rp_sum = 0; _error_rp_count = 0; - return ret; + return _error_rp_last; } // average error_yaw since last call float AP_AHRS_DCM::get_error_yaw(void) { - float ret; if (_error_yaw_count == 0) { - return 0; + // this happens when telemetry is setup on two + // serial ports + return _error_yaw_last; } - ret = _error_yaw_sum / _error_yaw_count; + _error_yaw_last = _error_yaw_sum / _error_yaw_count; _error_yaw_sum = 0; _error_yaw_count = 0; - return ret; + return _error_yaw_last; } diff --git a/libraries/AP_AHRS/AP_AHRS_DCM.h b/libraries/AP_AHRS/AP_AHRS_DCM.h index ee5b3403ac26919bb646ab9e5696363dab62324b..5952380c7e938f9f50bef1401a1480fdf1975eff 100644 --- a/libraries/AP_AHRS/AP_AHRS_DCM.h +++ b/libraries/AP_AHRS/AP_AHRS_DCM.h @@ -75,8 +75,10 @@ private: uint16_t _renorm_val_count; float _error_rp_sum; uint16_t _error_rp_count; + float _error_rp_last; float _error_yaw_sum; uint16_t _error_yaw_count; + float _error_yaw_last; // time in millis when we last got a GPS heading uint32_t _gps_last_update; diff --git a/libraries/AP_AHRS/AP_AHRS_Quaternion.cpp b/libraries/AP_AHRS/AP_AHRS_Quaternion.cpp index 03fe02869d5e4d87ffd011e4e56050c931729239..5148aa878f4b065714d114711028d52608731894 100644 --- a/libraries/AP_AHRS/AP_AHRS_Quaternion.cpp +++ b/libraries/AP_AHRS/AP_AHRS_Quaternion.cpp @@ -383,30 +383,34 @@ void AP_AHRS_Quaternion::update(void) } -// average error in roll/pitch since last call +/* reporting of Quaternion state for MAVLink */ + +// average error_roll_pitch since last call float AP_AHRS_Quaternion::get_error_rp(void) { - float ret; if (_error_rp_count == 0) { - return 0; + // this happens when telemetry is setup on two + // serial ports + return _error_rp_last; } - ret = _error_rp_sum / _error_rp_count; + _error_rp_last = _error_rp_sum / _error_rp_count; _error_rp_sum = 0; _error_rp_count = 0; - return ret; + return _error_rp_last; } -// average error in yaw since last call +// average error_yaw since last call float AP_AHRS_Quaternion::get_error_yaw(void) { - float ret; if (_error_yaw_count == 0) { - return 0; + // this happens when telemetry is setup on two + // serial ports + return _error_yaw_last; } - ret = _error_yaw_sum / _error_yaw_count; + _error_yaw_last = _error_yaw_sum / _error_yaw_count; _error_yaw_sum = 0; _error_yaw_count = 0; - return ret; + return _error_yaw_last; } // reset attitude system diff --git a/libraries/AP_AHRS/AP_AHRS_Quaternion.h b/libraries/AP_AHRS/AP_AHRS_Quaternion.h index a5b6583903846064ea2f555a2a042cb26a6020c9..91c41e2ea40223591f34c0589ad105594a2f1c41 100644 --- a/libraries/AP_AHRS/AP_AHRS_Quaternion.h +++ b/libraries/AP_AHRS/AP_AHRS_Quaternion.h @@ -87,8 +87,10 @@ private: // estimate of error float _error_rp_sum; uint16_t _error_rp_count; + float _error_rp_last; float _error_yaw_sum; uint16_t _error_yaw_count; + float _error_yaw_last; }; #endif