mirror of https://github.com/ArduPilot/ardupilot
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
This commit is contained in:
parent
20941c15c3
commit
071f89df2e
|
@ -495,25 +495,27 @@ AP_AHRS_DCM::euler_angles(void)
|
||||||
// average error_roll_pitch since last call
|
// average error_roll_pitch since last call
|
||||||
float AP_AHRS_DCM::get_error_rp(void)
|
float AP_AHRS_DCM::get_error_rp(void)
|
||||||
{
|
{
|
||||||
float ret;
|
|
||||||
if (_error_rp_count == 0) {
|
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_sum = 0;
|
||||||
_error_rp_count = 0;
|
_error_rp_count = 0;
|
||||||
return ret;
|
return _error_rp_last;
|
||||||
}
|
}
|
||||||
|
|
||||||
// average error_yaw since last call
|
// average error_yaw since last call
|
||||||
float AP_AHRS_DCM::get_error_yaw(void)
|
float AP_AHRS_DCM::get_error_yaw(void)
|
||||||
{
|
{
|
||||||
float ret;
|
|
||||||
if (_error_yaw_count == 0) {
|
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_sum = 0;
|
||||||
_error_yaw_count = 0;
|
_error_yaw_count = 0;
|
||||||
return ret;
|
return _error_yaw_last;
|
||||||
}
|
}
|
||||||
|
|
|
@ -75,8 +75,10 @@ private:
|
||||||
uint16_t _renorm_val_count;
|
uint16_t _renorm_val_count;
|
||||||
float _error_rp_sum;
|
float _error_rp_sum;
|
||||||
uint16_t _error_rp_count;
|
uint16_t _error_rp_count;
|
||||||
|
float _error_rp_last;
|
||||||
float _error_yaw_sum;
|
float _error_yaw_sum;
|
||||||
uint16_t _error_yaw_count;
|
uint16_t _error_yaw_count;
|
||||||
|
float _error_yaw_last;
|
||||||
|
|
||||||
// time in millis when we last got a GPS heading
|
// time in millis when we last got a GPS heading
|
||||||
uint32_t _gps_last_update;
|
uint32_t _gps_last_update;
|
||||||
|
|
|
@ -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 AP_AHRS_Quaternion::get_error_rp(void)
|
||||||
{
|
{
|
||||||
float ret;
|
|
||||||
if (_error_rp_count == 0) {
|
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_sum = 0;
|
||||||
_error_rp_count = 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 AP_AHRS_Quaternion::get_error_yaw(void)
|
||||||
{
|
{
|
||||||
float ret;
|
|
||||||
if (_error_yaw_count == 0) {
|
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_sum = 0;
|
||||||
_error_yaw_count = 0;
|
_error_yaw_count = 0;
|
||||||
return ret;
|
return _error_yaw_last;
|
||||||
}
|
}
|
||||||
|
|
||||||
// reset attitude system
|
// reset attitude system
|
||||||
|
|
|
@ -87,8 +87,10 @@ private:
|
||||||
// estimate of error
|
// estimate of error
|
||||||
float _error_rp_sum;
|
float _error_rp_sum;
|
||||||
uint16_t _error_rp_count;
|
uint16_t _error_rp_count;
|
||||||
|
float _error_rp_last;
|
||||||
float _error_yaw_sum;
|
float _error_yaw_sum;
|
||||||
uint16_t _error_yaw_count;
|
uint16_t _error_yaw_count;
|
||||||
|
float _error_yaw_last;
|
||||||
};
|
};
|
||||||
|
|
||||||
#endif
|
#endif
|
||||||
|
|
Loading…
Reference in New Issue