line source
//////////////////////////////////////////////////////////////////////////////
/// compass_trigo_test.cpp
/// Unit test for compass various operations.
/// Copyright (c) 2012-2015, JD Gascuel, HeinrichsWeikamp, all right reserved.
//////////////////////////////////////////////////////////////////////////////
// HISTORY
// 2015-05-23 jDG: Rewrite compass testing, to allow reducing code size.
extern "C" {
# include "compass.h"
}
#include <gtest/gtest.h>
//////////////////////////////////////////////////////////////////////////////
// Fake assembleur fixed point multiplies.
extern "C" Int16 compass_umul(void);
extern "C" Int16 compass_imul(void);
extern "C" Int16 compass_a, compass_b;
// The (filtered) components of the magnetometer sensor:
Int16 compass_DX_f;
Int16 compass_DY_f;
Int16 compass_DZ_f;
// Found soft-iron calibration values, deduced from already filtered values.
Int16 compass_CX_f;
Int16 compass_CY_f;
Int16 compass_CZ_f;
// The (filtered) components of the accelerometer sensor:
Int16 accel_DX_f;
Int16 accel_DY_f;
Int16 accel_DZ_f;
// The compass result value.
Int16 compass_heading;
Int16 compass_roll;
Int16 compass_pitch;
Int16 compass_a, compass_b;
Int16 compass_umul()
{
unsigned int a = compass_a;
unsigned int b = compass_b;
a *= b;
a >>= 15;
return (Int16)a;
}
Int16 compass_imul()
{
int a = compass_a;
int b = compass_b;
a *= b;
a >>= 15;
return (Int16)a;
}
//////////////////////////////////////////////////////////////////////////////
TEST(ops, multiply) {
// Check basic sign handling:
EXPECT_EQ(umul( 8000, 4000), (Int16)0x03D0); // 8000/2**15 * 4000/2**15 = 0x3D0
EXPECT_EQ(imul(-8000, 4000), (Int16)0xFC2F); // -976 = 0xFC2F
EXPECT_EQ(imul( 8000, -4000), (Int16)0xFC2F); // -976 = 0xFC2F
EXPECT_EQ(imul(-8000, -4000), (Int16)0x03D0); // +976 = 0x3D0
}
TEST(ops, divide) {
// Check basic divides:
EXPECT_EQ(udiv(32000, 32001), (Int16)32766); // 0.99997 ~ 32766
EXPECT_EQ(udiv( 4000, 8000), (Int16)16384);
EXPECT_EQ(udiv( 2000, 8000), (Int16) 8192);
EXPECT_EQ(udiv( 1000, 8000), (Int16) 4096);
EXPECT_EQ(udiv( 500, 8000), (Int16) 2048);
}
TEST(trigo, atan) {
// Check angles returned by the SINGLE QUADRANT atan() function:
EXPECT_EQ(utan(100, 100), (Int16)4501); // +1
EXPECT_EQ(utan( 90, 100), (Int16)4195); // -4
EXPECT_EQ(utan( 80, 100), (Int16)3864); // -2
EXPECT_EQ(utan( 70, 100), (Int16)3500); // +1
EXPECT_EQ(utan( 60, 100), (Int16)3099); // +3
EXPECT_EQ(utan( 50, 100), (Int16)2658); // +1
EXPECT_EQ(utan( 40, 100), (Int16)2179); // -1
EXPECT_EQ(utan( 30, 100), (Int16)1667); // -3
EXPECT_EQ(utan( 20, 100), (Int16)1127); // -4
EXPECT_EQ(utan( 10, 100), (Int16) 569); // -2
EXPECT_EQ(utan( 0, 100), (Int16) 0);
}
TEST(trigo, cosx2h2) {
// Check ONE-OCTANT pseudo-cosinus function
// Note: cosxh(x**2, x**2+y**2) is computing cos(atan(y/x))
EXPECT_EQ(cosxh(12769, 13169), (Int16)32268); // 113, 20 --> 10.0369° --> 32267 +1
EXPECT_EQ(cosxh(10000, 12500), (Int16)29310); // 100, 50 --> 26.5650° --> 29309 +1
EXPECT_EQ(cosxh(10000, 20000), (Int16)23171); // 100, 100 --> 45.0000° --> 23170 +1
EXPECT_EQ(cosxh( 2500, 12500), (Int16)14658); // 50, 100 --> 63.4349° --> 14654 +4
EXPECT_EQ(cosxh( 400, 13169), (Int16) 5718); // 20, 113 --> 79.9631° --> 5711 +7
}
TEST(trigo, sinCos) {
Int16 sin, cos;
//---- Check sincos() FIRST QUADRANT ---------------------------------
sincos( 20, 113, &sin, &cos); // 80°
EXPECT_EQ(sin, (Int16)32269); // +2
EXPECT_EQ(cos, (Int16) 5727); // +16
sincos( 50, 100, &sin, &cos); // 63°
EXPECT_EQ(sin, (Int16)29311); // +2
EXPECT_EQ(cos, (Int16)14660); // +6
sincos(100, 100, &sin, &cos); // 45°
EXPECT_EQ(sin, (Int16)23173); // +3
EXPECT_EQ(cos, (Int16)23173); // +3
sincos(100, 50, &sin, &cos); // 27°
EXPECT_EQ(sin, (Int16)14660); // +6
EXPECT_EQ(cos, (Int16)29311); // +2
sincos(113, 20, &sin, &cos); // 10°
EXPECT_EQ(sin, (Int16) 5727); // +16
EXPECT_EQ(cos, (Int16)32269); // +2
//---- Check sincos() OTHER QUADRANTS --------------------------------
sincos(-20, 113, &sin, &cos); // 90+80°
EXPECT_EQ(sin, (Int16) 32269); // +2
EXPECT_EQ(cos, (Int16) -5727); // +16
sincos(-20,-113, &sin, &cos); // 180+80°
EXPECT_EQ(sin, (Int16)-32269); // +2
EXPECT_EQ(cos, (Int16) -5727); // +16
sincos( 20,-113, &sin, &cos); // 270+80°
EXPECT_EQ(sin, (Int16)-32269); // +2
EXPECT_EQ(cos, (Int16) 5727); // +16
}