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 ////////////// //////////// 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 CompassCalibrator::CompassSample::get() const {
Vector3f out; return Vector3f(COMPASS_CAL_SAMPLE_SCALE_TO_FLOAT(x),
out.x = (float)x*2048.0f/32700.0f; COMPASS_CAL_SAMPLE_SCALE_TO_FLOAT(y),
out.y = (float)y*2048.0f/32700.0f; COMPASS_CAL_SAMPLE_SCALE_TO_FLOAT(z));
out.z = (float)z*2048.0f/32700.0f;
return out;
} }
void CompassCalibrator::CompassSample::set(const Vector3f &in) { void CompassCalibrator::CompassSample::set(const Vector3f &in) {
x = (int16_t)(in.x*32700.0f/2048.0f + 0.5f); x = COMPASS_CAL_SAMPLE_SCALE_TO_FIXED(in.x);
y = (int16_t)(in.y*32700.0f/2048.0f + 0.5f); y = COMPASS_CAL_SAMPLE_SCALE_TO_FIXED(in.y);
z = (int16_t)(in.z*32700.0f/2048.0f + 0.5f); z = COMPASS_CAL_SAMPLE_SCALE_TO_FIXED(in.z);
} }