From 071f89df2e8a166c95a37628ede196b931b60968 Mon Sep 17 00:00:00 2001
From: Andrew Tridgell <tridge@samba.org>
Date: Thu, 29 Mar 2012 08:57:53 +1100
Subject: [PATCH] AHRS: fixed error_yaw reporting with 2 MAVLink connections
when a user first connects with USB, and later switches to the
telemetry port without restarting we were getting zero for error_yaw
in the logs, as AHRS.get_error_yaw() was being called twice.
This ensures we give the last value after the counter is reset
---
libraries/AP_AHRS/AP_AHRS_DCM.cpp | 18 ++++++++++--------
libraries/AP_AHRS/AP_AHRS_DCM.h | 2 ++
libraries/AP_AHRS/AP_AHRS_Quaternion.cpp | 24 ++++++++++++++----------
libraries/AP_AHRS/AP_AHRS_Quaternion.h | 2 ++
4 files changed, 28 insertions(+), 18 deletions(-)
diff --git a/libraries/AP_AHRS/AP_AHRS_DCM.cpp b/libraries/AP_AHRS/AP_AHRS_DCM.cpp
index 323d1bb33..b6fa2e1f4 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 ee5b3403a..5952380c7 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 03fe02869..5148aa878 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 a5b658390..91c41e2ea 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
--
GitLab