|
|
|
@ -686,8 +686,8 @@ uint16_t CompassCalibrator::get_random(void)
@@ -686,8 +686,8 @@ 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) |
|
|
|
|
#define COMPASS_CAL_SAMPLE_SCALE_TO_FIXED(__X) ((int16_t)constrain_float(roundf(__X*8.0f), INT16_MIN, INT16_MAX)) |
|
|
|
|
#define COMPASS_CAL_SAMPLE_SCALE_TO_FLOAT(__X) (__X/8.0f) |
|
|
|
|
|
|
|
|
|
Vector3f CompassCalibrator::CompassSample::get() const { |
|
|
|
|
return Vector3f(COMPASS_CAL_SAMPLE_SCALE_TO_FLOAT(x), |
|
|
|
|