Subversion Repositories Projects

/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-

//

// Unit tests for the AP_Math euler code

//

#include <FastSerial.h>

#include <AP_Common.h>

#include <AP_Math.h>

FastSerialPort(Serial, 0);

#ifdef DESKTOP_BUILD

// all of this is needed to build with SITL

#include <DataFlash.h>

#include <APM_RC.h>

#include <GCS_MAVLink.h>

#include <Arduino_Mega_ISR_Registry.h>

#include <AP_PeriodicProcess.h>

#include <AP_ADC.h>

#include <SPI.h>

#include <I2C.h>

#include <AP_Baro.h>

#include <AP_Compass.h>

#include <AP_GPS.h>

#include <Filter.h>

Arduino_Mega_ISR_Registry isr_registry;

AP_Baro_BMP085_HIL      barometer;

AP_Compass_HIL     compass;

#endif

#include <AP_Declination.h>

static float rad_diff(float rad1, float rad2)

{

    float diff = rad1 - rad2;

    if (diff > PI) {

        diff -= 2*PI;

    }

    if (diff < -PI) {

        diff += 2*PI;

    }

    return fabs(diff);

}

static void check_result(float roll, float pitch, float yaw,

                         float roll2, float pitch2, float yaw2)

{

    if (isnan(roll2) ||

        isnan(pitch2) ||

        isnan(yaw2)) {

        Serial.printf("NAN eulers roll=%f pitch=%f yaw=%f\n",

                      roll, pitch, yaw);

    }

    if (rad_diff(roll2,roll) > ToRad(179)) {

        // reverse all 3

        roll2 += fmod(roll2+PI, 2*PI);

        pitch2 += fmod(pitch2+PI, 2*PI);

        yaw2 += fmod(yaw2+PI, 2*PI);

    }

    if (rad_diff(roll2,roll) > 0.01 ||

        rad_diff(pitch2, pitch) > 0.01 ||

        rad_diff(yaw2, yaw) > 0.01) {

        if (pitch >= PI/2 ||

            pitch <= -PI/2 ||

            ToDeg(rad_diff(pitch, PI/2)) < 1 ||

            ToDeg(rad_diff(pitch, -PI/2)) < 1) {

            // we expect breakdown at these poles

            Serial.printf("breakdown eulers roll=%f/%f pitch=%f/%f yaw=%f/%f\n",

                          ToDeg(roll), ToDeg(roll2), ToDeg(pitch), ToDeg(pitch2), ToDeg(yaw), ToDeg(yaw2));

        } else {

            Serial.printf("incorrect eulers roll=%f/%f pitch=%f/%f yaw=%f/%f\n",

                          ToDeg(roll), ToDeg(roll2), ToDeg(pitch), ToDeg(pitch2), ToDeg(yaw), ToDeg(yaw2));

        }

    }

}

static void test_euler(float roll, float pitch, float yaw)

{

    Matrix3f m;

    float roll2, pitch2, yaw2;

    m.from_euler(roll, pitch, yaw);

    m.to_euler(&roll2, &pitch2, &yaw2);

    check_result(roll, pitch, yaw, roll2, pitch2, yaw2);

}

#define ARRAY_LENGTH(x) (sizeof((x))/sizeof((x)[0]))

static const float angles[] = { 0, PI/8, PI/4, PI/2, PI,

                                -PI/8, -PI/4, -PI/2, -PI};

void test_matrix_eulers(void)

{

    uint8_t i, j, k;

    uint8_t N = ARRAY_LENGTH(angles);

    Serial.println("rotation matrix unit tests\n");

    for (i=0; i<N; i++)

        for (j=0; j<N; j++)

            for (k=0; k<N; k++)

                test_euler(angles[i], angles[j], angles[k]);

    Serial.println("tests done\n");

}

static void test_quaternion(float roll, float pitch, float yaw)

{

    Quaternion q;

    float roll2, pitch2, yaw2;

    q.from_euler(roll, pitch, yaw);

    q.to_euler(&roll2, &pitch2, &yaw2);

    check_result(roll, pitch, yaw, roll2, pitch2, yaw2);

}

void test_quaternion_eulers(void)

{

    uint8_t i, j, k;

    uint8_t N = ARRAY_LENGTH(angles);

    Serial.println("quaternion unit tests\n");

    test_quaternion(PI/4, 0, 0);

    test_quaternion(0, PI/4, 0);

    test_quaternion(0, 0, PI/4);

    test_quaternion(-PI/4, 0, 0);

    test_quaternion(0, -PI/4, 0);

    test_quaternion(0, 0, -PI/4);

    test_quaternion(-PI/4, 1, 1);

    test_quaternion(1, -PI/4, 1);

    test_quaternion(1, 1, -PI/4);

    test_quaternion(ToRad(89), 0, 0.1);

    test_quaternion(0, ToRad(89), 0.1);

    test_quaternion(0.1, 0, ToRad(89));

    test_quaternion(ToRad(91), 0, 0.1);

    test_quaternion(0, ToRad(91), 0.1);

    test_quaternion(0.1, 0, ToRad(91));

    for (i=0; i<N; i++)

        for (j=0; j<N; j++)

            for (k=0; k<N; k++)

                test_quaternion(angles[i], angles[j], angles[k]);

    Serial.println("tests done\n");

}

static void test_conversion(float roll, float pitch, float yaw)

{

    Quaternion q;

    Matrix3f m, m2;

    float roll2, pitch2, yaw2;

    float roll3, pitch3, yaw3;

    q.from_euler(roll, pitch, yaw);

    q.to_euler(&roll2, &pitch2, &yaw2);

    check_result(roll, pitch, yaw, roll2, pitch2, yaw2);

    q.rotation_matrix(m);

    m.to_euler(&roll2, &pitch2, &yaw2);

    m2.from_euler(roll, pitch, yaw);

    m2.to_euler(&roll3, &pitch3, &yaw3);

    if (m.is_nan()) {

        Serial.printf("NAN matrix roll=%f pitch=%f yaw=%f\n",

                      roll, pitch, yaw);

    }

    check_result(roll, pitch, yaw, roll2, pitch2, yaw2);

    check_result(roll, pitch, yaw, roll3, pitch3, yaw3);

}

void test_conversions(void)

{

    uint8_t i, j, k;

    uint8_t N = ARRAY_LENGTH(angles);

    Serial.println("matrix/quaternion tests\n");

    test_conversion(1, 1.1, 1.2);

    test_conversion(1, -1.1, 1.2);

    test_conversion(1, -1.1, -1.2);

    test_conversion(-1, 1.1, -1.2);

    test_conversion(-1, 1.1, 1.2);

    for (i=0; i<N; i++)

        for (j=0; j<N; j++)

            for (k=0; k<N; k++)

                test_conversion(angles[i], angles[j], angles[k]);

    Serial.println("tests done\n");

}

void test_frame_transforms(void)

{

    Vector3f v, v2;

    Quaternion q;

    Matrix3f m;

    Serial.println("frame transform tests\n");

    q.from_euler(ToRad(90), 0, 0);

    v2 = v = Vector3f(0, 0, 1);

    q.earth_to_body(v2);

    printf("%f %f %f\n", v2.x, v2.y, v2.z);

}

// generate a random float between -1 and 1

static float rand_num(void)

{

	float ret = ((unsigned)random()) % 2000000;

	return (ret - 1.0e6) / 1.0e6;

}

void test_matrix_rotate(void)

{

    Matrix3f m1, m2, diff;

    Vector3f r;

    m1.identity();

    m2.identity();

    r.x = rand_num();

    r.y = rand_num();

    r.z = rand_num();

    for (uint16_t i = 0; i<1000; i++) {

        // old method

        Matrix3f temp_matrix;

        temp_matrix.a.x = 0;

        temp_matrix.a.y = -r.z;

        temp_matrix.a.z =  r.y;

        temp_matrix.b.x =  r.z;

        temp_matrix.b.y = 0;

        temp_matrix.b.z = -r.x;

        temp_matrix.c.x = -r.y;

        temp_matrix.c.y =  r.x;

        temp_matrix.c.z = 0;

        temp_matrix = m1 * temp_matrix;

        m1 += temp_matrix;

        // new method

        m2.rotate(r);

        // check they behave in the same way

        diff = m1 - m2;

        float err = diff.a.length() + diff.b.length() + diff.c.length();

        if (err > 0) {

            Serial.printf("ERROR: i=%u err=%f\n", (unsigned)i, err);

        }

    }

}

/*

  euler angle tests

 */

void setup(void)

{

    Serial.begin(115200);

    Serial.println("euler unit tests\n");

    test_conversion(0, PI, 0);

    test_frame_transforms();

    test_conversions();

    test_quaternion_eulers();

    test_matrix_eulers();

    test_matrix_rotate();

}

void

loop(void)

{

}