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

@@ -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;
 }

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;

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

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