Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

AP_MATH: Adding WGS GPS conversions, CRC16 checks, and double-precision Vectors and Matrices

This commit is contained in:
Niels Joubert 2014-04-03 15:44:56 -07:00 committed by Andrew Tridgell
parent 161b5f99b3
commit 879eb5936b
9 changed files with 356 additions and 6 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

27
libraries/AP_Math/AP_Math.h
View File

@ -18,6 +18,7 @@
#include "matrix3.h"
#include "quaternion.h"
#include "polygon.h"
#include "edc.h"
#ifndef M_PI_F
#define M_PI_F 3.141592653589793f
@ -28,9 +29,18 @@
#ifndef M_PI_2
# define M_PI_2 1.570796326794897f
#endif
//Single precision conversions
#define DEG_TO_RAD 0.017453292519943295769236907684886f
#define RAD_TO_DEG 57.295779513082320876798154814105f
//GPS Specific double precision conversions
//The precision here does matter when using the wsg* functions for converting
//between LLH and ECEF coordinates. Test code in examlpes/location/location.pde
#if HAL_CPU_CLASS >= HAL_CPU_CLASS_75
#define DEG_TO_RAD_DOUBLE 0.0174532925199432954743716805978692718781530857086181640625 // equals to (M_PI / 180.0)
#define RAD_TO_DEG_DOUBLE 57.29577951308232286464772187173366546630859375 // equals to (180.0 / M_PI)
#endif
#define RadiansToCentiDegrees(x) ((x) * 5729.5779513082320876798154814105f)
// acceleration due to gravity in m/s/s
@ -46,6 +56,17 @@
#define LATLON_TO_M 0.01113195f
#define LATLON_TO_CM 1.113195f
// Semi-major axis of the Earth, in meters.
#define WGS84_A 6378137.0
//Inverse flattening of the Earth
#define WGS84_IF 298.257223563
// The flattening of the Earth
#define WGS84_F (1/WGS84_IF)
// Semi-minor axis of the Earth in meters
#define WGS84_B (WGS84_A*(1-WGS84_F))
// Eccentricity of the Earth
#define WGS84_E (sqrt(2*WGS84_F - WGS84_F*WGS84_F))
// define AP_Param types AP_Vector3f and Ap_Matrix3f
AP_PARAMDEFV(Matrix3f, Matrix3f, AP_PARAM_MATRIX3F);
AP_PARAMDEFV(Vector3f, Vector3f, AP_PARAM_VECTOR3F);
@ -118,6 +139,12 @@ float wrap_PI(float angle_in_radians);
*/
void print_latlon(AP_HAL::BetterStream *s, int32_t lat_or_lon);
#if HAL_CPU_CLASS >= HAL_CPU_CLASS_75
//Convert between LLH and ECEF coordinate systems
void wgsllh2ecef(const Vector3d &llh, Vector3d &ecef);
void wgsecef2llh(const Vector3d &ecef, Vector3d &llh);
#endif
// constrain a value
float constrain_float(float amt, float low, float high);
int16_t constrain_int16(int16_t amt, int16_t low, int16_t high);

56
libraries/AP_Math/edc.cpp Normal file
View File

@ -0,0 +1,56 @@
/*
* Copyright (C) 2010 Swift Navigation Inc.
* Contact: Fergus Noble <fergus@swift-nav.com>
*
* This source is subject to the license found in the file 'LICENSE' which must
* be be distributed together with this source. All other rights reserved.
*
* THIS CODE AND INFORMATION IS PROVIDED "AS IS" WITHOUT WARRANTY OF ANY KIND,
* EITHER EXPRESSED OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE IMPLIED
* WARRANTIES OF MERCHANTABILITY AND/OR FITNESS FOR A PARTICULAR PURPOSE.
*/
#include "edc.h"
/* CRC16 implementation acording to CCITT standards */
static const uint16_t crc16tab[256] PROGMEM = {
0x0000, 0x1021, 0x2042, 0x3063, 0x4084, 0x50A5, 0x60C6, 0x70E7,
0x8108, 0x9129, 0xA14A, 0xB16B, 0xC18C, 0xD1AD, 0xE1CE, 0xF1EF,
0x1231, 0x0210, 0x3273, 0x2252, 0x52B5, 0x4294, 0x72F7, 0x62D6,
0x9339, 0x8318, 0xB37B, 0xA35A, 0xD3BD, 0xC39C, 0xF3FF, 0xE3DE,
0x2462, 0x3443, 0x0420, 0x1401, 0x64E6, 0x74C7, 0x44A4, 0x5485,
0xA56A, 0xB54B, 0x8528, 0x9509, 0xE5EE, 0xF5CF, 0xC5AC, 0xD58D,
0x3653, 0x2672, 0x1611, 0x0630, 0x76D7, 0x66F6, 0x5695, 0x46B4,
0xB75B, 0xA77A, 0x9719, 0x8738, 0xF7DF, 0xE7FE, 0xD79D, 0xC7BC,
0x48C4, 0x58E5, 0x6886, 0x78A7, 0x0840, 0x1861, 0x2802, 0x3823,
0xC9CC, 0xD9ED, 0xE98E, 0xF9AF, 0x8948, 0x9969, 0xA90A, 0xB92B,
0x5AF5, 0x4AD4, 0x7AB7, 0x6A96, 0x1A71, 0x0A50, 0x3A33, 0x2A12,
0xDBFD, 0xCBDC, 0xFBBF, 0xEB9E, 0x9B79, 0x8B58, 0xBB3B, 0xAB1A,
0x6CA6, 0x7C87, 0x4CE4, 0x5CC5, 0x2C22, 0x3C03, 0x0C60, 0x1C41,
0xEDAE, 0xFD8F, 0xCDEC, 0xDDCD, 0xAD2A, 0xBD0B, 0x8D68, 0x9D49,
0x7E97, 0x6EB6, 0x5ED5, 0x4EF4, 0x3E13, 0x2E32, 0x1E51, 0x0E70,
0xFF9F, 0xEFBE, 0xDFDD, 0xCFFC, 0xBF1B, 0xAF3A, 0x9F59, 0x8F78,
0x9188, 0x81A9, 0xB1CA, 0xA1EB, 0xD10C, 0xC12D, 0xF14E, 0xE16F,
0x1080, 0x00A1, 0x30C2, 0x20E3, 0x5004, 0x4025, 0x7046, 0x6067,
0x83B9, 0x9398, 0xA3FB, 0xB3DA, 0xC33D, 0xD31C, 0xE37F, 0xF35E,
0x02B1, 0x1290, 0x22F3, 0x32D2, 0x4235, 0x5214, 0x6277, 0x7256,
0xB5EA, 0xA5CB, 0x95A8, 0x8589, 0xF56E, 0xE54F, 0xD52C, 0xC50D,
0x34E2, 0x24C3, 0x14A0, 0x0481, 0x7466, 0x6447, 0x5424, 0x4405,
0xA7DB, 0xB7FA, 0x8799, 0x97B8, 0xE75F, 0xF77E, 0xC71D, 0xD73C,
0x26D3, 0x36F2, 0x0691, 0x16B0, 0x6657, 0x7676, 0x4615, 0x5634,
0xD94C, 0xC96D, 0xF90E, 0xE92F, 0x99C8, 0x89E9, 0xB98A, 0xA9AB,
0x5844, 0x4865, 0x7806, 0x6827, 0x18C0, 0x08E1, 0x3882, 0x28A3,
0xCB7D, 0xDB5C, 0xEB3F, 0xFB1E, 0x8BF9, 0x9BD8, 0xABBB, 0xBB9A,
0x4A75, 0x5A54, 0x6A37, 0x7A16, 0x0AF1, 0x1AD0, 0x2AB3, 0x3A92,
0xFD2E, 0xED0F, 0xDD6C, 0xCD4D, 0xBDAA, 0xAD8B, 0x9DE8, 0x8DC9,
0x7C26, 0x6C07, 0x5C64, 0x4C45, 0x3CA2, 0x2C83, 0x1CE0, 0x0CC1,
0xEF1F, 0xFF3E, 0xCF5D, 0xDF7C, 0xAF9B, 0xBFBA, 0x8FD9, 0x9FF8,
0x6E17, 0x7E36, 0x4E55, 0x5E74, 0x2E93, 0x3EB2, 0x0ED1, 0x1EF0
};
uint16_t crc16_ccitt(const uint8_t *buf, uint32_t len, uint16_t crc)
{
for (uint32_t i = 0; i < len; i++)
crc = (crc << 8) ^ (uint16_t) pgm_read_word(&crc16tab[((crc >> 8) ^ *buf++) & 0x00FF]);
return crc;
}

29
libraries/AP_Math/edc.h Normal file
View File

@ -0,0 +1,29 @@
// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
/*
This program is free software: you can redistribute it and/or modify
it under the terms of the GNU General Public License as published by
the Free Software Foundation, either version 3 of the License, or
(at your option) any later version.
This program is distributed in the hope that it will be useful,
but WITHOUT ANY WARRANTY; without even the implied warranty of
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
GNU General Public License for more details.
You should have received a copy of the GNU General Public License
along with this program. If not, see <http://www.gnu.org/licenses/>.
*/
// Copyright (C) 2010 Swift Navigation Inc.
// Contact: Fergus Noble <fergus@swift-nav.com>
#ifndef __EDC_H_
#define __EDC_H_
#include <AP_Progmem.h>
#include <stdint.h>
uint16_t crc16_ccitt(const uint8_t *buf, uint32_t len, uint16_t crc);
#endif /* __EDC_H_ */

88
libraries/AP_Math/examples/location/location.pde
View File

@ -260,6 +260,92 @@ static void test_wrap_cd(void)
hal.console->printf("wrap_cd tests done\n");
}
static void test_wgs_conversion_functions(void)
{
#define D2R DEG_TO_RAD_DOUBLE
/* Maximum allowable error in quantities with units of length (in meters). */
#define MAX_DIST_ERROR_M 1e-6
/* Maximum allowable error in quantities with units of angle (in sec of arc).
* 1 second of arc on the equator is ~31 meters. */
#define MAX_ANGLE_ERROR_SEC 1e-7
#define MAX_ANGLE_ERROR_RAD (MAX_ANGLE_ERROR_SEC*(D2R/3600.0))
/* Semi-major axis. */
#define EARTH_A 6378137.0
/* Semi-minor axis. */
#define EARTH_B 6356752.31424517929553985595703125
#define NUM_COORDS 10
Vector3d llhs[NUM_COORDS];
llhs[0] = Vector3d(0, 0, 0); /* On the Equator and Prime Meridian. */
llhs[1] = Vector3d(0, 180*D2R, 0); /* On the Equator. */
llhs[2] = Vector3d(0, 90*D2R, 0); /* On the Equator. */
llhs[3] = Vector3d(0, -90*D2R, 0); /* On the Equator. */
llhs[4] = Vector3d(90*D2R, 0, 0); /* North pole. */
llhs[5] = Vector3d(-90*D2R, 0, 0); /* South pole. */
llhs[6] = Vector3d(90*D2R, 0, 22); /* 22m above the north pole. */
llhs[7] = Vector3d(-90*D2R, 0, 22); /* 22m above the south pole. */
llhs[8] = Vector3d(0, 0, 22); /* 22m above the Equator and Prime Meridian. */
llhs[9] = Vector3d(0, 180*D2R, 22); /* 22m above the Equator. */
Vector3d ecefs[NUM_COORDS];
ecefs[0] = Vector3d(EARTH_A, 0, 0);
ecefs[1] = Vector3d(-EARTH_A, 0, 0);
ecefs[2] = Vector3d(0, EARTH_A, 0);
ecefs[3] = Vector3d(0, -EARTH_A, 0);
ecefs[4] = Vector3d(0, 0, EARTH_B);
ecefs[5] = Vector3d(0, 0, -EARTH_B);
ecefs[6] = Vector3d(0, 0, (EARTH_B+22));
ecefs[7] = Vector3d(0, 0, -(EARTH_B+22));
ecefs[8] = Vector3d((22+EARTH_A), 0, 0);
ecefs[9] = Vector3d(-(22+EARTH_A), 0, 0);
hal.console->printf("TESTING wgsllh2ecef\n");
for (int i = 0; i < NUM_COORDS; i++) {
Vector3d ecef;
wgsllh2ecef(llhs[i], ecef);
double x_err = fabs(ecef[0] - ecefs[i][0]);
double y_err = fabs(ecef[1] - ecefs[i][1]);
double z_err = fabs(ecef[2] - ecefs[i][2]);
if ((x_err < MAX_DIST_ERROR_M) &&
(y_err < MAX_DIST_ERROR_M) &&
(z_err < MAX_DIST_ERROR_M)) {
hal.console->printf("passing llh to ecef test %d\n", i);
} else {
hal.console->printf("failed llh to ecef test %d: ", i);
hal.console->printf("(%f - %f) (%f - %f) (%f - %f) => %.10f %.10f %.10f\n", ecef[0], ecefs[i][0], ecef[1], ecefs[i][1], ecef[2], ecefs[i][2], x_err, y_err, z_err);
}
}
hal.console->printf("TESTING wgsecef2llh\n");
for (int i = 0; i < NUM_COORDS; i++) {
Vector3d llh;
wgsecef2llh(ecefs[i], llh);
double lat_err = fabs(llh[0] - llhs[i][0]);
double lon_err = fabs(llh[1] - llhs[i][1]);
double hgt_err = fabs(llh[2] - llhs[i][2]);
if ((lat_err < MAX_ANGLE_ERROR_RAD) &&
(lon_err < MAX_ANGLE_ERROR_RAD) &&
(hgt_err < MAX_DIST_ERROR_M)) {
hal.console->printf("passing exef to llh test %d\n", i);
} else {
hal.console->printf("failed ecef to llh test %d: ", i);
hal.console->printf("%.10f %.10f %.10f\n", lat_err, lon_err, hgt_err);
}
}
}
/*
* polygon tests
*/
@ -269,6 +355,8 @@ void setup(void)
test_offset();
test_accuracy();
test_wrap_cd();
test_wgs_conversion_functions();
hal.console->printf("ALL TESTS DONE\n");
}
void loop(void){}

107
libraries/AP_Math/location.cpp
View File

@ -241,3 +241,110 @@ void print_latlon(AP_HAL::BetterStream *s, int32_t lat_or_lon)
}
s->printf_P(PSTR("%ld.%07ld"),(long)dec_portion,(long)frac_portion);
}
#if HAL_CPU_CLASS >= HAL_CPU_CLASS_75
void wgsllh2ecef(const Vector3d &llh, Vector3d &ecef) {
double d = WGS84_E * sin(llh[0]);
double N = WGS84_A / sqrt(1. - d*d);
ecef[0] = (N + llh[2]) * cos(llh[0]) * cos(llh[1]);
ecef[1] = (N + llh[2]) * cos(llh[0]) * sin(llh[1]);
ecef[2] = ((1 - WGS84_E*WGS84_E)*N + llh[2]) * sin(llh[0]);
}
void wgsecef2llh(const Vector3d &ecef, Vector3d &llh) {
/* Distance from polar axis. */
const double p = sqrt(ecef[0]*ecef[0] + ecef[1]*ecef[1]);
/* Compute longitude first, this can be done exactly. */
if (p != 0)
llh[1] = atan2(ecef[1], ecef[0]);
else
llh[1] = 0;
/* If we are close to the pole then convergence is very slow, treat this is a
* special case. */
if (p < WGS84_A*1e-16) {
llh[0] = copysign(M_PI_2, ecef[2]);
llh[2] = fabs(ecef[2]) - WGS84_B;
return;
}
/* Caluclate some other constants as defined in the Fukushima paper. */
const double P = p / WGS84_A;
const double e_c = sqrt(1. - WGS84_E*WGS84_E);
const double Z = fabs(ecef[2]) * e_c / WGS84_A;
/* Initial values for S and C correspond to a zero height solution. */
double S = Z;
double C = e_c * P;
/* Neither S nor C can be negative on the first iteration so
* starting prev = -1 will not cause and early exit. */
double prev_C = -1;
double prev_S = -1;
double A_n, B_n, D_n, F_n;
/* Iterate a maximum of 10 times. This should be way more than enough for all
* sane inputs */
for (int i=0; i<10; i++)
{
/* Calculate some intermmediate variables used in the update step based on
* the current state. */
A_n = sqrt(S*S + C*C);
D_n = Z*A_n*A_n*A_n + WGS84_E*WGS84_E*S*S*S;
F_n = P*A_n*A_n*A_n - WGS84_E*WGS84_E*C*C*C;
B_n = 1.5*WGS84_E*S*C*C*(A_n*(P*S - Z*C) - WGS84_E*S*C);
/* Update step. */
S = D_n*F_n - B_n*S;
C = F_n*F_n - B_n*C;
/* The original algorithm as presented in the paper by Fukushima has a
* problem with numerical stability. S and C can grow very large or small
* and over or underflow a double. In the paper this is acknowledged and
* the proposed resolution is to non-dimensionalise the equations for S and
* C. However, this does not completely solve the problem. The author caps
* the solution to only a couple of iterations and in this period over or
* underflow is unlikely but as we require a bit more precision and hence
* more iterations so this is still a concern for us.
*
* As the only thing that is important is the ratio T = S/C, my solution is
* to divide both S and C by either S or C. The scaling is chosen such that
* one of S or C is scaled to unity whilst the other is scaled to a value
* less than one. By dividing by the larger of S or C we ensure that we do
* not divide by zero as only one of S or C should ever be zero.
*
* This incurs an extra division each iteration which the author was
* explicityl trying to avoid and it may be that this solution is just
* reverting back to the method of iterating on T directly, perhaps this
* bears more thought?
*/
if (S > C) {
C = C / S;
S = 1;
} else {
S = S / C;
C = 1;
}
/* Check for convergence and exit early if we have converged. */
if (fabs(S - prev_S) < 1e-16 && fabs(C - prev_C) < 1e-16) {
break;
} else {
prev_S = S;
prev_C = C;
}
}
A_n = sqrt(S*S + C*C);
llh[0] = copysign(1.0, ecef[2]) * atan(S / (e_c*C));
llh[2] = (p*e_c*C + fabs(ecef[2])*S - WGS84_A*e_c*A_n) / sqrt(e_c*e_c*C*C + S*S);
}
#endif

20
libraries/AP_Math/matrix3.cpp
View File

@ -65,7 +65,7 @@ void Matrix3<T>::to_euler(float *roll, float *pitch, float *yaw) const
template <typename T>
void Matrix3<T>::rotate(const Vector3<T> &g)
{
Matrix3f temp_matrix;
Matrix3<T> temp_matrix;
temp_matrix.a.x = a.y * g.z - a.z * g.y;
temp_matrix.a.y = a.z * g.x - a.x * g.z;
temp_matrix.a.z = a.x * g.y - a.y * g.x;
@ -84,7 +84,7 @@ void Matrix3<T>::rotate(const Vector3<T> &g)
template <typename T>
void Matrix3<T>::rotateXY(const Vector3<T> &g)
{
Matrix3f temp_matrix;
Matrix3<T> temp_matrix;
temp_matrix.a.x = -a.z * g.y;
temp_matrix.a.y = a.z * g.x;
temp_matrix.a.z = a.x * g.y - a.y * g.x;
@ -103,7 +103,7 @@ void Matrix3<T>::rotateXY(const Vector3<T> &g)
template <typename T>
void Matrix3<T>::rotateXYinv(const Vector3<T> &g)
{
Matrix3f temp_matrix;
Matrix3<T> temp_matrix;
temp_matrix.a.x = a.z * g.y;
temp_matrix.a.y = - a.z * g.x;
temp_matrix.a.z = - a.x * g.y + a.y * g.x;
@ -188,3 +188,17 @@ template Vector3<float> Matrix3<float>::mul_transpose(const Vector3<float> &v) c
template Matrix3<float> Matrix3<float>::operator *(const Matrix3<float> &m) const;
template Matrix3<float> Matrix3<float>::transposed(void) const;
template Vector2<float> Matrix3<float>::mulXY(const Vector3<float> &v) const;
#if HAL_CPU_CLASS >= HAL_CPU_CLASS_75
template void Matrix3<double>::zero(void);
template void Matrix3<double>::rotate(const Vector3<double> &g);
template void Matrix3<double>::rotateXY(const Vector3<double> &g);
template void Matrix3<double>::rotateXYinv(const Vector3<double> &g);
template void Matrix3<double>::from_euler(float roll, float pitch, float yaw);
template void Matrix3<double>::to_euler(float *roll, float *pitch, float *yaw) const;
template Vector3<double> Matrix3<double>::operator *(const Vector3<double> &v) const;
template Vector3<double> Matrix3<double>::mul_transpose(const Vector3<double> &v) const;
template Matrix3<double> Matrix3<double>::operator *(const Matrix3<double> &m) const;
template Matrix3<double> Matrix3<double>::transposed(void) const;
template Vector2<double> Matrix3<double>::mulXY(const Vector3<double> &v) const;
#endif

9
libraries/AP_Math/matrix3.h
View File

@ -154,19 +154,19 @@ public:
// extract x column
Vector3<T> colx(void) const
{
return Vector3f(a.x, b.x, c.x);
return Vector3<T>(a.x, b.x, c.x);
}
// extract y column
Vector3<T> coly(void) const
{
return Vector3f(a.y, b.y, c.y);
return Vector3<T>(a.y, b.y, c.y);
}
// extract z column
Vector3<T> colz(void) const
{
return Vector3f(a.z, b.z, c.z);
return Vector3<T>(a.z, b.z, c.z);
}
// multiplication by another Matrix3<T>
@ -226,5 +226,8 @@ typedef Matrix3<uint16_t> Matrix3ui;
typedef Matrix3<int32_t> Matrix3l;
typedef Matrix3<uint32_t> Matrix3ul;
typedef Matrix3<float> Matrix3f;
#if HAL_CPU_CLASS >= HAL_CPU_CLASS_75
typedef Matrix3<double> Matrix3d;
#endif
#endif // MATRIX3_H

23
libraries/AP_Math/vector3.cpp
View File

@ -373,3 +373,26 @@ template bool Vector3<float>::operator !=(const Vector3<float> &v) const;
template bool Vector3<float>::is_nan(void) const;
template bool Vector3<float>::is_inf(void) const;
template float Vector3<float>::angle(const Vector3<float> &v) const;
#if HAL_CPU_CLASS >= HAL_CPU_CLASS_75
template void Vector3<double>::rotate(enum Rotation);
template float Vector3<double>::length(void) const;
template Vector3<double> Vector3<double>::operator %(const Vector3<double> &v) const;
template double Vector3<double>::operator *(const Vector3<double> &v) const;
template Vector3<double> Vector3<double>::operator *(const Matrix3<double> &m) const;
template Matrix3<double> Vector3<double>::mul_rowcol(const Vector3<double> &v) const;
template Vector3<double> &Vector3<double>::operator *=(const double num);
template Vector3<double> &Vector3<double>::operator /=(const double num);
template Vector3<double> &Vector3<double>::operator -=(const Vector3<double> &v);
template Vector3<double> &Vector3<double>::operator +=(const Vector3<double> &v);
template Vector3<double> Vector3<double>::operator /(const double num) const;
template Vector3<double> Vector3<double>::operator *(const double num) const;
template Vector3<double> Vector3<double>::operator +(const Vector3<double> &v) const;
template Vector3<double> Vector3<double>::operator -(const Vector3<double> &v) const;
template Vector3<double> Vector3<double>::operator -(void) const;
template bool Vector3<double>::operator ==(const Vector3<double> &v) const;
template bool Vector3<double>::operator !=(const Vector3<double> &v) const;
template bool Vector3<double>::is_nan(void) const;
template bool Vector3<double>::is_inf(void) const;
template float Vector3<double>::angle(const Vector3<double> &v) const;
#endif

3
libraries/AP_Math/vector3.h
View File

@ -214,5 +214,8 @@ typedef Vector3<uint16_t> Vector3ui;
typedef Vector3<int32_t> Vector3l;
typedef Vector3<uint32_t> Vector3ul;
typedef Vector3<float> Vector3f;
#if HAL_CPU_CLASS >= HAL_CPU_CLASS_75
typedef Vector3<double> Vector3d;
#endif
#endif // VECTOR3_H