mirror of
https://github.com/ArduPilot/ardupilot
synced 2025-01-03 14:38:30 -04:00
AP_Math: added quaternion helper functions and a test suite
This commit is contained in:
parent
f70dfe440d
commit
c7d5f06b21
@ -67,3 +67,33 @@ void calculate_euler_angles(Matrix3f &m, float *roll, float *pitch, float *yaw)
|
||||
*yaw = atan2(m.b.x, m.a.x);
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
// create a quaternion from Euler angles
|
||||
void quaternion_from_euler(Quaternion &q, float roll, float pitch, float yaw)
|
||||
{
|
||||
float cr2 = cos(roll/2);
|
||||
float cp2 = cos(pitch/2);
|
||||
float cy2 = cos(yaw/2);
|
||||
// the sign reversal here is due to the different conventions
|
||||
// in the madgwick quaternion code and the rest of APM
|
||||
float sr2 = -sin(roll/2);
|
||||
float sp2 = -sin(pitch/2);
|
||||
float sy2 = sin(yaw/2);
|
||||
|
||||
q.q1 = cr2*cp2*cy2 + sr2*sp2*sy2;
|
||||
q.q2 = sr2*cp2*cy2 - cr2*sp2*sy2;
|
||||
q.q3 = cr2*sp2*cy2 + sr2*cp2*sy2;
|
||||
q.q4 = cr2*cp2*sy2 - sr2*sp2*cy2;
|
||||
}
|
||||
|
||||
// create eulers from a quaternion
|
||||
void euler_from_quaternion(Quaternion &q, float *roll, float *pitch, float *yaw)
|
||||
{
|
||||
*roll = -(atan2(2.0*(q.q1*q.q2 + q.q3*q.q4),
|
||||
1 - 2.0*(q.q2*q.q2 + q.q3*q.q3)));
|
||||
// we let safe_asin() handle the singularities near 90/-90 in pitch
|
||||
*pitch = -safe_asin(2.0*(q.q1*q.q3 - q.q4*q.q2));
|
||||
*yaw = atan2(2.0*(q.q1*q.q4 + q.q2*q.q3),
|
||||
1 - 2.0*(q.q3*q.q3 + q.q4*q.q4));
|
||||
}
|
||||
|
@ -7,6 +7,7 @@
|
||||
#include "vector2.h"
|
||||
#include "vector3.h"
|
||||
#include "matrix3.h"
|
||||
#include "quaternion.h"
|
||||
#include "polygon.h"
|
||||
|
||||
// define AP_Param types AP_Vector3f and Ap_Matrix3f
|
||||
@ -24,3 +25,9 @@ void rotation_matrix_from_euler(Matrix3f &m, float roll, float pitch, float yaw)
|
||||
|
||||
// calculate euler angles from a rotation matrix
|
||||
void calculate_euler_angles(Matrix3f &m, float *roll, float *pitch, float *yaw);
|
||||
|
||||
// create a quaternion from Euler angles
|
||||
void quaternion_from_euler(Quaternion &q, float roll, float pitch, float yaw);
|
||||
|
||||
// create eulers from a quaternion
|
||||
void euler_from_quaternion(Quaternion &q, float *roll, float *pitch, float *yaw);
|
||||
|
1
libraries/AP_Math/examples/eulers/Makefile
Normal file
1
libraries/AP_Math/examples/eulers/Makefile
Normal file
@ -0,0 +1 @@
|
||||
include ../../../AP_Common/Arduino.mk
|
159
libraries/AP_Math/examples/eulers/eulers.pde
Normal file
159
libraries/AP_Math/examples/eulers/eulers.pde
Normal file
@ -0,0 +1,159 @@
|
||||
/// -*- 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);
|
||||
|
||||
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 test_euler(float roll, float pitch, float yaw)
|
||||
{
|
||||
Matrix3f m;
|
||||
float roll2, pitch2, yaw2;
|
||||
|
||||
rotation_matrix_from_euler(m, roll, pitch, yaw);
|
||||
calculate_euler_angles(m, &roll2, &pitch2, &yaw2);
|
||||
if (m.is_nan()) {
|
||||
Serial.printf("NAN matrix roll=%f pitch=%f yaw=%f\n",
|
||||
roll, pitch, yaw);
|
||||
}
|
||||
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) > 0.01 ||
|
||||
rad_diff(pitch2, pitch) > 0.01 ||
|
||||
rad_diff(yaw2, yaw) > 0.01) {
|
||||
Serial.printf("incorrect eulers roll=%f/%f pitch=%f/%f yaw=%f/%f\n",
|
||||
roll, roll2, pitch, pitch2, yaw, 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;
|
||||
|
||||
quaternion_from_euler(q, roll, pitch, yaw);
|
||||
euler_from_quaternion(q, &roll2, &pitch2, &yaw2);
|
||||
if (q.is_nan()) {
|
||||
Serial.printf("NAN quaternion roll=%f pitch=%f yaw=%f\n",
|
||||
roll, pitch, yaw);
|
||||
}
|
||||
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 (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));
|
||||
}
|
||||
} else {
|
||||
Serial.printf("correct eulers roll=%f/%f pitch=%f/%f yaw=%f/%f\n",
|
||||
ToDeg(roll), ToDeg(roll2), ToDeg(pitch), ToDeg(pitch2), ToDeg(yaw), ToDeg(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");
|
||||
}
|
||||
|
||||
/*
|
||||
euler angle tests
|
||||
*/
|
||||
void setup(void)
|
||||
{
|
||||
Serial.begin(115200);
|
||||
Serial.println("euler unit tests\n");
|
||||
test_quaternion_eulers();
|
||||
//test_matrix_eulers();
|
||||
}
|
||||
|
||||
void
|
||||
loop(void)
|
||||
{
|
||||
}
|
36
libraries/AP_Math/quaternion.h
Normal file
36
libraries/AP_Math/quaternion.h
Normal file
@ -0,0 +1,36 @@
|
||||
// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: t -*-
|
||||
|
||||
// Copyright 2012 Andrew Tridgell, all rights reserved.
|
||||
|
||||
// This library is free software; you can redistribute it and / or
|
||||
// modify it under the terms of the GNU Lesser General Public
|
||||
// License as published by the Free Software Foundation; either
|
||||
// version 2.1 of the License, or (at your option) any later version.
|
||||
|
||||
#ifndef QUATERNION_H
|
||||
#define QUATERNION_H
|
||||
|
||||
#include <math.h>
|
||||
|
||||
class Quaternion
|
||||
{
|
||||
public:
|
||||
float q1, q2, q3, q4;
|
||||
|
||||
// constructor
|
||||
Quaternion() { q1 = 1; q2 = q3 = q4 = 0; }
|
||||
|
||||
// setting constructor
|
||||
Quaternion(const float _q1, const float _q2, const float _q3, const float _q4):
|
||||
q1(_q1), q2(_q2), q3(_q3), q4(_q4) {}
|
||||
|
||||
// function call operator
|
||||
void operator ()(const float _q1, const float _q2, const float _q3, const float _q4)
|
||||
{ q1 = _q1; q2 = _q2; q3 = _q3; q4 = _q4; }
|
||||
|
||||
// check if any elements are NAN
|
||||
bool is_nan(void)
|
||||
{ return isnan(q1) || isnan(q2) || isnan(q3) || isnan(q4); }
|
||||
|
||||
};
|
||||
#endif // QUATERNION_H
|
Loading…
Reference in New Issue
Block a user