diff --git a/libraries/AP_Math/AP_Math.h b/libraries/AP_Math/AP_Math.h index 9c64feeaf5..126a64a8e8 100644 --- a/libraries/AP_Math/AP_Math.h +++ b/libraries/AP_Math/AP_Math.h @@ -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); diff --git a/libraries/AP_Math/edc.cpp b/libraries/AP_Math/edc.cpp new file mode 100644 index 0000000000..7d93dddd73 --- /dev/null +++ b/libraries/AP_Math/edc.cpp @@ -0,0 +1,56 @@ +/* + * Copyright (C) 2010 Swift Navigation Inc. + * Contact: Fergus Noble + * + * 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; +} \ No newline at end of file diff --git a/libraries/AP_Math/edc.h b/libraries/AP_Math/edc.h new file mode 100644 index 0000000000..b8773a0f8a --- /dev/null +++ b/libraries/AP_Math/edc.h @@ -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 . + */ + +// Copyright (C) 2010 Swift Navigation Inc. +// Contact: Fergus Noble + +#ifndef __EDC_H_ +#define __EDC_H_ + +#include + +#include + +uint16_t crc16_ccitt(const uint8_t *buf, uint32_t len, uint16_t crc); + +#endif /* __EDC_H_ */ diff --git a/libraries/AP_Math/examples/location/location.pde b/libraries/AP_Math/examples/location/location.pde index b8aa140555..b670277804 100644 --- a/libraries/AP_Math/examples/location/location.pde +++ b/libraries/AP_Math/examples/location/location.pde @@ -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){} diff --git a/libraries/AP_Math/location.cpp b/libraries/AP_Math/location.cpp index 009bc8a84f..f827530d75 100644 --- a/libraries/AP_Math/location.cpp +++ b/libraries/AP_Math/location.cpp @@ -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 + diff --git a/libraries/AP_Math/matrix3.cpp b/libraries/AP_Math/matrix3.cpp index 8d6a606c1c..4c30a28193 100644 --- a/libraries/AP_Math/matrix3.cpp +++ b/libraries/AP_Math/matrix3.cpp @@ -65,7 +65,7 @@ void Matrix3::to_euler(float *roll, float *pitch, float *yaw) const template void Matrix3::rotate(const Vector3 &g) { - Matrix3f temp_matrix; + Matrix3 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::rotate(const Vector3 &g) template void Matrix3::rotateXY(const Vector3 &g) { - Matrix3f temp_matrix; + Matrix3 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::rotateXY(const Vector3 &g) template void Matrix3::rotateXYinv(const Vector3 &g) { - Matrix3f temp_matrix; + Matrix3 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 Matrix3::mul_transpose(const Vector3 &v) c template Matrix3 Matrix3::operator *(const Matrix3 &m) const; template Matrix3 Matrix3::transposed(void) const; template Vector2 Matrix3::mulXY(const Vector3 &v) const; + +#if HAL_CPU_CLASS >= HAL_CPU_CLASS_75 +template void Matrix3::zero(void); +template void Matrix3::rotate(const Vector3 &g); +template void Matrix3::rotateXY(const Vector3 &g); +template void Matrix3::rotateXYinv(const Vector3 &g); +template void Matrix3::from_euler(float roll, float pitch, float yaw); +template void Matrix3::to_euler(float *roll, float *pitch, float *yaw) const; +template Vector3 Matrix3::operator *(const Vector3 &v) const; +template Vector3 Matrix3::mul_transpose(const Vector3 &v) const; +template Matrix3 Matrix3::operator *(const Matrix3 &m) const; +template Matrix3 Matrix3::transposed(void) const; +template Vector2 Matrix3::mulXY(const Vector3 &v) const; +#endif diff --git a/libraries/AP_Math/matrix3.h b/libraries/AP_Math/matrix3.h index b8cd12d851..da8fb985af 100644 --- a/libraries/AP_Math/matrix3.h +++ b/libraries/AP_Math/matrix3.h @@ -154,19 +154,19 @@ public: // extract x column Vector3 colx(void) const { - return Vector3f(a.x, b.x, c.x); + return Vector3(a.x, b.x, c.x); } // extract y column Vector3 coly(void) const { - return Vector3f(a.y, b.y, c.y); + return Vector3(a.y, b.y, c.y); } // extract z column Vector3 colz(void) const { - return Vector3f(a.z, b.z, c.z); + return Vector3(a.z, b.z, c.z); } // multiplication by another Matrix3 @@ -226,5 +226,8 @@ typedef Matrix3 Matrix3ui; typedef Matrix3 Matrix3l; typedef Matrix3 Matrix3ul; typedef Matrix3 Matrix3f; +#if HAL_CPU_CLASS >= HAL_CPU_CLASS_75 + typedef Matrix3 Matrix3d; +#endif #endif // MATRIX3_H diff --git a/libraries/AP_Math/vector3.cpp b/libraries/AP_Math/vector3.cpp index 04382dd50b..b08a03dd30 100644 --- a/libraries/AP_Math/vector3.cpp +++ b/libraries/AP_Math/vector3.cpp @@ -373,3 +373,26 @@ template bool Vector3::operator !=(const Vector3 &v) const; template bool Vector3::is_nan(void) const; template bool Vector3::is_inf(void) const; template float Vector3::angle(const Vector3 &v) const; + +#if HAL_CPU_CLASS >= HAL_CPU_CLASS_75 +template void Vector3::rotate(enum Rotation); +template float Vector3::length(void) const; +template Vector3 Vector3::operator %(const Vector3 &v) const; +template double Vector3::operator *(const Vector3 &v) const; +template Vector3 Vector3::operator *(const Matrix3 &m) const; +template Matrix3 Vector3::mul_rowcol(const Vector3 &v) const; +template Vector3 &Vector3::operator *=(const double num); +template Vector3 &Vector3::operator /=(const double num); +template Vector3 &Vector3::operator -=(const Vector3 &v); +template Vector3 &Vector3::operator +=(const Vector3 &v); +template Vector3 Vector3::operator /(const double num) const; +template Vector3 Vector3::operator *(const double num) const; +template Vector3 Vector3::operator +(const Vector3 &v) const; +template Vector3 Vector3::operator -(const Vector3 &v) const; +template Vector3 Vector3::operator -(void) const; +template bool Vector3::operator ==(const Vector3 &v) const; +template bool Vector3::operator !=(const Vector3 &v) const; +template bool Vector3::is_nan(void) const; +template bool Vector3::is_inf(void) const; +template float Vector3::angle(const Vector3 &v) const; +#endif diff --git a/libraries/AP_Math/vector3.h b/libraries/AP_Math/vector3.h index 56e0de90a7..c7cc7b36da 100644 --- a/libraries/AP_Math/vector3.h +++ b/libraries/AP_Math/vector3.h @@ -214,5 +214,8 @@ typedef Vector3 Vector3ui; typedef Vector3 Vector3l; typedef Vector3 Vector3ul; typedef Vector3 Vector3f; +#if HAL_CPU_CLASS >= HAL_CPU_CLASS_75 + typedef Vector3 Vector3d; +#endif #endif // VECTOR3_H