Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-03 14:38:30 -04:00
Code Issues Packages Projects Releases Wiki Activity

AP_Compass: correct wrapping behavior in conversion to fixed point

Browse Source
This commit is contained in:
Jonathan Challinger 2016-09-16 12:04:10 -07:00 committed by Andrew Tridgell
parent 68df5b7606
commit 0652d71a72
1 changed files with 9 additions and 8 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

17
libraries/AP_Compass/CompassCalibrator.cpp
View File

@ -686,16 +686,17 @@ uint16_t CompassCalibrator::get_random(void)
//////////// CompassSample public interface //////////////
//////////////////////////////////////////////////////////
#define COMPASS_CAL_SAMPLE_SCALE_TO_FIXED(__X) ((int16_t)constrain_float(roundf(__X*16.0f), INT16_MIN, INT16_MAX))
#define COMPASS_CAL_SAMPLE_SCALE_TO_FLOAT(__X) (__X/16.0f)
Vector3f CompassCalibrator::CompassSample::get() const {
Vector3f out;
out.x = (float)x*2048.0f/32700.0f;
out.y = (float)y*2048.0f/32700.0f;
out.z = (float)z*2048.0f/32700.0f;
return out;
return Vector3f(COMPASS_CAL_SAMPLE_SCALE_TO_FLOAT(x),
COMPASS_CAL_SAMPLE_SCALE_TO_FLOAT(y),
COMPASS_CAL_SAMPLE_SCALE_TO_FLOAT(z));
}
void CompassCalibrator::CompassSample::set(const Vector3f &in) {
x = (int16_t)(in.x*32700.0f/2048.0f + 0.5f);
y = (int16_t)(in.y*32700.0f/2048.0f + 0.5f);
z = (int16_t)(in.z*32700.0f/2048.0f + 0.5f);
x = COMPASS_CAL_SAMPLE_SCALE_TO_FIXED(in.x);
y = COMPASS_CAL_SAMPLE_SCALE_TO_FIXED(in.y);
z = COMPASS_CAL_SAMPLE_SCALE_TO_FIXED(in.z);
}