flight/libraries/CoordinateConversions.c

   1 /**
   2  ******************************************************************************
   3  *
   4  * @file       CoordinateConversions.c
   5  * @author     The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
   6  * @brief      General conversions with different coordinate systems.
   7  *             - all angles in deg
   8  *             - distances in meters
   9  *             - altitude above WGS-84 elipsoid
  10  *
  11  * @see        The GNU Public License (GPL) Version 3
  12  *
  13  *****************************************************************************/
  14 /*
  15  * This program is free software; you can redistribute it and/or modify
  16  * it under the terms of the GNU General Public License as published by
  17  * the Free Software Foundation; either version 3 of the License, or
  18  * (at your option) any later version.
  19  *
  20  * This program is distributed in the hope that it will be useful, but
  21  * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY
  22  * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License
  23  * for more details.
  24  *
  25  * You should have received a copy of the GNU General Public License along
  26  * with this program; if not, write to the Free Software Foundation, Inc.,
  27  * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
  28  */
  29
  30 #include <math.h>
  31 #include <stdint.h>
  32 #include <pios_math.h>
  33 #include "CoordinateConversions.h"
  34
  35 #define MIN_ALLOWABLE_MAGNITUDE 1e-30f
  36
  37 // ****** convert Lat,Lon,Alt to ECEF  ************
  38 void LLA2ECEF(int32_t LLAi[3], double ECEF[3])
  39 {
  40     const double a  = 6378137.0d; // Equatorial Radius
  41     const double e  = 8.1819190842622e-2d; // Eccentricity
  42     const double e2 = e * e; // Eccentricity squared
  43     double sinLat, sinLon, cosLat, cosLon;
  44     double N;
  45     double LLA[3]   = {
  46         (double)LLAi[0] * 1e-7d,
  47         (double)LLAi[1] * 1e-7d,
  48         (double)LLAi[2] * 1e-4d
  49     };
  50
  51     sinLat = sin(DEG2RAD_D(LLA[0]));
  52     sinLon = sin(DEG2RAD_D(LLA[1]));
  53     cosLat = cos(DEG2RAD_D(LLA[0]));
  54     cosLon = cos(DEG2RAD_D(LLA[1]));
  55
  56     N = a / sqrt(1.0d - e2 * sinLat * sinLat); // prime vertical radius of curvature
  57
  58     ECEF[0] = (N + LLA[2]) * cosLat * cosLon;
  59     ECEF[1] = (N + LLA[2]) * cosLat * sinLon;
  60     ECEF[2] = ((1.0d - e2) * N + LLA[2]) * sinLat;
  61 }
  62
  63 // ****** convert ECEF to Lat,Lon,Alt (ITERATIVE!) *********
  64 uint16_t ECEF2LLA(double ECEF[3], float LLA[3])
  65 {
  66     /**
  67      * LLA parameter is used to prime the iteration.
  68      * A position within 1 meter of the specified LLA
  69      * will be calculated within at most 3 iterations.
  70      * If unknown: Call with any valid LLA coordinate
  71      * will compute within at most 5 iterations.
  72      * Suggestion: [0,0,0]
  73      **/
  74
  75     const double a = 6378137.0f; // Equatorial Radius
  76     const double e = 8.1819190842622e-2f; // Eccentricity
  77     double x = ECEF[0], y = ECEF[1], z = ECEF[2];
  78     double Lat, N, NplusH, delta, esLat;
  79     uint16_t iter;
  80
  81 #define MAX_ITER 10 // should not take more than 5 for valid coordinates
  82 #define ACCURACY 1.0e-11d // used to be e-14, but we don't need sub micrometer exact calculations
  83
  84     LLA[1] = (float)RAD2DEG_D(atan2(y, x));
  85     Lat    = DEG2RAD_D((double)LLA[0]);
  86     esLat  = e * sin(Lat);
  87     N = a / sqrt(1 - esLat * esLat);
  88     NplusH = N + (double)LLA[2];
  89     delta  = 1;
  90     iter   = 0;
  91
  92     while (((delta > ACCURACY) || (delta < -ACCURACY))
  93            && (iter < MAX_ITER)) {
  94         delta  = Lat - atan(z / (sqrt(x * x + y * y) * (1 - (N * e * e / NplusH))));
  95         Lat    = Lat - delta;
  96         esLat  = e * sin(Lat);
  97         N      = a / sqrt(1 - esLat * esLat);
  98         NplusH = sqrt(x * x + y * y) / cos(Lat);
  99         iter  += 1;
 100     }
 101
 102     LLA[0] = RAD2DEG_D(Lat);
 103     LLA[2] = NplusH - N;
 104
 105     return iter < MAX_ITER;
 106 }
 107
 108 // ****** find ECEF to NED rotation matrix ********
 109 void RneFromLLA(int32_t LLAi[3], float Rne[3][3])
 110 {
 111     float sinLat, sinLon, cosLat, cosLon;
 112
 113     sinLat    = sinf(DEG2RAD((float)LLAi[0] * 1e-7f));
 114     sinLon    = sinf(DEG2RAD((float)LLAi[1] * 1e-7f));
 115     cosLat    = cosf(DEG2RAD((float)LLAi[0] * 1e-7f));
 116     cosLon    = cosf(DEG2RAD((float)LLAi[1] * 1e-7f));
 117
 118     Rne[0][0] = -sinLat * cosLon;
 119     Rne[0][1] = -sinLat * sinLon;
 120     Rne[0][2] = cosLat;
 121     Rne[1][0] = -sinLon;
 122     Rne[1][1] = cosLon;
 123     Rne[1][2] = 0;
 124     Rne[2][0] = -cosLat * cosLon;
 125     Rne[2][1] = -cosLat * sinLon;
 126     Rne[2][2] = -sinLat;
 127 }
 128
 129 // ****** find roll, pitch, yaw from quaternion ********
 130 void Quaternion2RPY(const float q[4], float rpy[3])
 131 {
 132     float R13, R11, R12, R23, R33;
 133     float q0s = q[0] * q[0];
 134     float q1s = q[1] * q[1];
 135     float q2s = q[2] * q[2];
 136     float q3s = q[3] * q[3];
 137
 138     R13    = 2.0f * (q[1] * q[3] - q[0] * q[2]);
 139     R11    = q0s + q1s - q2s - q3s;
 140     R12    = 2.0f * (q[1] * q[2] + q[0] * q[3]);
 141     R23    = 2.0f * (q[2] * q[3] + q[0] * q[1]);
 142     R33    = q0s - q1s - q2s + q3s;
 143
 144     rpy[1] = RAD2DEG(asinf(-R13)); // pitch always between -pi/2 to pi/2
 145     rpy[2] = RAD2DEG(atan2f(R12, R11));
 146     rpy[0] = RAD2DEG(atan2f(R23, R33));
 147
 148     // TODO: consider the cases where |R13| ~= 1, |pitch| ~= pi/2
 149 }
 150
 151 // ****** find quaternion from roll, pitch, yaw ********
 152 void RPY2Quaternion(const float rpy[3], float q[4])
 153 {
 154     float phi, theta, psi;
 155     float cphi, sphi, ctheta, stheta, cpsi, spsi;
 156
 157     phi    = DEG2RAD(rpy[0] / 2);
 158     theta  = DEG2RAD(rpy[1] / 2);
 159     psi    = DEG2RAD(rpy[2] / 2);
 160     cphi   = cosf(phi);
 161     sphi   = sinf(phi);
 162     ctheta = cosf(theta);
 163     stheta = sinf(theta);
 164     cpsi   = cosf(psi);
 165     spsi   = sinf(psi);
 166
 167     q[0]   = cphi * ctheta * cpsi + sphi * stheta * spsi;
 168     q[1]   = sphi * ctheta * cpsi - cphi * stheta * spsi;
 169     q[2]   = cphi * stheta * cpsi + sphi * ctheta * spsi;
 170     q[3]   = cphi * ctheta * spsi - sphi * stheta * cpsi;
 171
 172     if (q[0] < 0) { // q0 always positive for uniqueness
 173         q[0] = -q[0];
 174         q[1] = -q[1];
 175         q[2] = -q[2];
 176         q[3] = -q[3];
 177     }
 178 }
 179
 180 // ** Find Rbe, that rotates a vector from earth fixed to body frame, from quaternion **
 181 void Quaternion2R(float q[4], float Rbe[3][3])
 182 {
 183     const float q0s = q[0] * q[0], q1s = q[1] * q[1], q2s = q[2] * q[2], q3s = q[3] * q[3];
 184
 185     Rbe[0][0] = q0s + q1s - q2s - q3s;
 186     Rbe[0][1] = 2 * (q[1] * q[2] + q[0] * q[3]);
 187     Rbe[0][2] = 2 * (q[1] * q[3] - q[0] * q[2]);
 188     Rbe[1][0] = 2 * (q[1] * q[2] - q[0] * q[3]);
 189     Rbe[1][1] = q0s - q1s + q2s - q3s;
 190     Rbe[1][2] = 2 * (q[2] * q[3] + q[0] * q[1]);
 191     Rbe[2][0] = 2 * (q[1] * q[3] + q[0] * q[2]);
 192     Rbe[2][1] = 2 * (q[2] * q[3] - q[0] * q[1]);
 193     Rbe[2][2] = q0s - q1s - q2s + q3s;
 194 }
 195
 196
 197 // ** Find first row of Rbe, that rotates a vector from earth fixed to body frame, from quaternion **
 198 // ** This vector corresponds to the fuselage/roll vector xB **
 199 void QuaternionC2xB(const float q0, const float q1, const float q2, const float q3, float x[3])
 200 {
 201     const float q0s = q0 * q0, q1s = q1 * q1, q2s = q2 * q2, q3s = q3 * q3;
 202
 203     x[0] = q0s + q1s - q2s - q3s;
 204     x[1] = 2 * (q1 * q2 + q0 * q3);
 205     x[2] = 2 * (q1 * q3 - q0 * q2);
 206 }
 207
 208
 209 void Quaternion2xB(const float q[4], float x[3])
 210 {
 211     QuaternionC2xB(q[0], q[1], q[2], q[3], x);
 212 }
 213
 214
 215 // ** Find second row of Rbe, that rotates a vector from earth fixed to body frame, from quaternion **
 216 // ** This vector corresponds to the spanwise/pitch vector yB **
 217 void QuaternionC2yB(const float q0, const float q1, const float q2, const float q3, float y[3])
 218 {
 219     const float q0s = q0 * q0, q1s = q1 * q1, q2s = q2 * q2, q3s = q3 * q3;
 220
 221     y[0] = 2 * (q1 * q2 - q0 * q3);
 222     y[1] = q0s - q1s + q2s - q3s;
 223     y[2] = 2 * (q2 * q3 + q0 * q1);
 224 }
 225
 226
 227 void Quaternion2yB(const float q[4], float y[3])
 228 {
 229     QuaternionC2yB(q[0], q[1], q[2], q[3], y);
 230 }
 231
 232
 233 // ** Find third row of Rbe, that rotates a vector from earth fixed to body frame, from quaternion **
 234 // ** This vector corresponds to the vertical/yaw vector zB **
 235 void QuaternionC2zB(const float q0, const float q1, const float q2, const float q3, float z[3])
 236 {
 237     const float q0s = q0 * q0, q1s = q1 * q1, q2s = q2 * q2, q3s = q3 * q3;
 238
 239     z[0] = 2 * (q1 * q3 + q0 * q2);
 240     z[1] = 2 * (q2 * q3 - q0 * q1);
 241     z[2] = q0s - q1s - q2s + q3s;
 242 }
 243
 244
 245 void Quaternion2zB(const float q[4], float z[3])
 246 {
 247     QuaternionC2zB(q[0], q[1], q[2], q[3], z);
 248 }
 249
 250
 251 // ****** Express LLA in a local NED Base Frame ********
 252 void LLA2Base(int32_t LLAi[3], double BaseECEF[3], float Rne[3][3], float NED[3])
 253 {
 254     double ECEF[3];
 255     float diff[3];
 256
 257     LLA2ECEF(LLAi, ECEF);
 258
 259     diff[0] = (float)(ECEF[0] - BaseECEF[0]);
 260     diff[1] = (float)(ECEF[1] - BaseECEF[1]);
 261     diff[2] = (float)(ECEF[2] - BaseECEF[2]);
 262
 263     NED[0]  = Rne[0][0] * diff[0] + Rne[0][1] * diff[1] + Rne[0][2] * diff[2];
 264     NED[1]  = Rne[1][0] * diff[0] + Rne[1][1] * diff[1] + Rne[1][2] * diff[2];
 265     NED[2]  = Rne[2][0] * diff[0] + Rne[2][1] * diff[1] + Rne[2][2] * diff[2];
 266 }
 267
 268 // ****** Express ECEF in a local NED Base Frame ********
 269 void ECEF2Base(double ECEF[3], double BaseECEF[3], float Rne[3][3], float NED[3])
 270 {
 271     float diff[3];
 272
 273     diff[0] = (float)(ECEF[0] - BaseECEF[0]);
 274     diff[1] = (float)(ECEF[1] - BaseECEF[1]);
 275     diff[2] = (float)(ECEF[2] - BaseECEF[2]);
 276
 277     NED[0]  = Rne[0][0] * diff[0] + Rne[0][1] * diff[1] + Rne[0][2] * diff[2];
 278     NED[1]  = Rne[1][0] * diff[0] + Rne[1][1] * diff[1] + Rne[1][2] * diff[2];
 279     NED[2]  = Rne[2][0] * diff[0] + Rne[2][1] * diff[1] + Rne[2][2] * diff[2];
 280 }
 281
 282 // ****** convert Rotation Matrix to Quaternion ********
 283 // ****** if R converts from e to b, q is rotation from e to b ****
 284 void R2Quaternion(float R[3][3], float q[4])
 285 {
 286     float m[4], mag;
 287     uint8_t index, i;
 288
 289     m[0]  = 1 + R[0][0] + R[1][1] + R[2][2];
 290     m[1]  = 1 + R[0][0] - R[1][1] - R[2][2];
 291     m[2]  = 1 - R[0][0] + R[1][1] - R[2][2];
 292     m[3]  = 1 - R[0][0] - R[1][1] + R[2][2];
 293
 294     // find maximum divisor
 295     index = 0;
 296     mag   = m[0];
 297     for (i = 1; i < 4; i++) {
 298         if (m[i] > mag) {
 299             mag   = m[i];
 300             index = i;
 301         }
 302     }
 303     mag = 2 * sqrtf(mag);
 304
 305     if (index == 0) {
 306         q[0] = mag / 4;
 307         q[1] = (R[1][2] - R[2][1]) / mag;
 308         q[2] = (R[2][0] - R[0][2]) / mag;
 309         q[3] = (R[0][1] - R[1][0]) / mag;
 310     } else if (index == 1) {
 311         q[1] = mag / 4;
 312         q[0] = (R[1][2] - R[2][1]) / mag;
 313         q[2] = (R[0][1] + R[1][0]) / mag;
 314         q[3] = (R[0][2] + R[2][0]) / mag;
 315     } else if (index == 2) {
 316         q[2] = mag / 4;
 317         q[0] = (R[2][0] - R[0][2]) / mag;
 318         q[1] = (R[0][1] + R[1][0]) / mag;
 319         q[3] = (R[1][2] + R[2][1]) / mag;
 320     } else {
 321         q[3] = mag / 4;
 322         q[0] = (R[0][1] - R[1][0]) / mag;
 323         q[1] = (R[0][2] + R[2][0]) / mag;
 324         q[2] = (R[1][2] + R[2][1]) / mag;
 325     }
 326
 327     // q0 positive, i.e. angle between pi and -pi
 328     if (q[0] < 0) {
 329         q[0] = -q[0];
 330         q[1] = -q[1];
 331         q[2] = -q[2];
 332         q[3] = -q[3];
 333     }
 334 }
 335
 336 // ****** Rotation Matrix from Two Vector Directions ********
 337 // ****** given two vector directions (v1 and v2) known in two frames (b and e) find Rbe ***
 338 // ****** solution is approximate if can't be exact ***
 339 uint8_t RotFrom2Vectors(const float v1b[3], const float v1e[3], const float v2b[3], const float v2e[3], float Rbe[3][3])
 340 {
 341     float Rib[3][3], Rie[3][3];
 342     float mag;
 343     uint8_t i, j, k;
 344
 345     // identity rotation in case of error
 346     for (i = 0; i < 3; i++) {
 347         for (j = 0; j < 3; j++) {
 348             Rbe[i][j] = 0;
 349         }
 350         Rbe[i][i] = 1;
 351     }
 352
 353     // The first rows of rot matrices chosen in direction of v1
 354     mag = VectorMagnitude(v1b);
 355     if (fabsf(mag) < MIN_ALLOWABLE_MAGNITUDE) {
 356         return -1;
 357     }
 358     for (i = 0; i < 3; i++) {
 359         Rib[0][i] = v1b[i] / mag;
 360     }
 361
 362     mag = VectorMagnitude(v1e);
 363     if (fabsf(mag) < MIN_ALLOWABLE_MAGNITUDE) {
 364         return -1;
 365     }
 366     for (i = 0; i < 3; i++) {
 367         Rie[0][i] = v1e[i] / mag;
 368     }
 369
 370     // The second rows of rot matrices chosen in direction of v1xv2
 371     CrossProduct(v1b, v2b, &Rib[1][0]);
 372     mag = VectorMagnitude(&Rib[1][0]);
 373     if (fabsf(mag) < MIN_ALLOWABLE_MAGNITUDE) {
 374         return -1;
 375     }
 376     for (i = 0; i < 3; i++) {
 377         Rib[1][i] = Rib[1][i] / mag;
 378     }
 379
 380     CrossProduct(v1e, v2e, &Rie[1][0]);
 381     mag = VectorMagnitude(&Rie[1][0]);
 382     if (fabsf(mag) < MIN_ALLOWABLE_MAGNITUDE) {
 383         return -1;
 384     }
 385     for (i = 0; i < 3; i++) {
 386         Rie[1][i] = Rie[1][i] / mag;
 387     }
 388
 389     // The third rows of rot matrices are XxY (Row1xRow2)
 390     CrossProduct(&Rib[0][0], &Rib[1][0], &Rib[2][0]);
 391     CrossProduct(&Rie[0][0], &Rie[1][0], &Rie[2][0]);
 392
 393     // Rbe = Rbi*Rie = Rib'*Rie
 394     for (i = 0; i < 3; i++) {
 395         for (j = 0; j < 3; j++) {
 396             Rbe[i][j] = 0;
 397             for (k = 0; k < 3; k++) {
 398                 Rbe[i][j] += Rib[k][i] * Rie[k][j];
 399             }
 400         }
 401     }
 402
 403     return 1;
 404 }
 405
 406 void Rv2Rot(float Rv[3], float R[3][3])
 407 {
 408     // Compute rotation matrix from a rotation vector
 409     // To save .text space, uses Quaternion2R()
 410     float q[4];
 411
 412     float angle = VectorMagnitude(Rv);
 413
 414     if (angle <= 0.00048828125f) {
 415         // angle < sqrt(2*machine_epsilon(float)), so flush cos(x) to 1.0f
 416         q[0] = 1.0f;
 417
 418         // and flush sin(x/2)/x to 0.5
 419         q[1] = 0.5f * Rv[0];
 420         q[2] = 0.5f * Rv[1];
 421         q[3] = 0.5f * Rv[2];
 422         // This prevents division by zero, while retaining full accuracy
 423     } else {
 424         q[0] = cosf(angle * 0.5f);
 425         float scale = sinf(angle * 0.5f) / angle;
 426         q[1] = scale * Rv[0];
 427         q[2] = scale * Rv[1];
 428         q[3] = scale * Rv[2];
 429     }
 430
 431     Quaternion2R(q, R);
 432 }
 433
 434 // ****** Vector Cross Product ********
 435 void CrossProduct(const float v1[3], const float v2[3], float result[3])
 436 {
 437     result[0] = v1[1] * v2[2] - v2[1] * v1[2];
 438     result[1] = v2[0] * v1[2] - v1[0] * v2[2];
 439     result[2] = v1[0] * v2[1] - v2[0] * v1[1];
 440 }
 441
 442 // ****** Vector Magnitude ********
 443 float VectorMagnitude(const float v[3])
 444 {
 445     return sqrtf(v[0] * v[0] + v[1] * v[1] + v[2] * v[2]);
 446 }
 447
 448 /**
 449  * @brief Compute the inverse of a quaternion
 450  * @param [in][out] q The matrix to invert
 451  */
 452 void quat_inverse(float q[4])
 453 {
 454     q[1] = -q[1];
 455     q[2] = -q[2];
 456     q[3] = -q[3];
 457 }
 458
 459 /**
 460  * @brief Duplicate a quaternion
 461  * @param[in] q quaternion in
 462  * @param[out] qnew quaternion to copy to
 463  */
 464 void quat_copy(const float q[4], float qnew[4])
 465 {
 466     qnew[0] = q[0];
 467     qnew[1] = q[1];
 468     qnew[2] = q[2];
 469     qnew[3] = q[3];
 470 }
 471
 472 /**
 473  * @brief Multiply two quaternions into a third
 474  * @param[in] q1 First quaternion
 475  * @param[in] q2 Second quaternion
 476  * @param[out] qout Output quaternion
 477  */
 478 void quat_mult(const float q1[4], const float q2[4], float qout[4])
 479 {
 480     qout[0] = q1[0] * q2[0] - q1[1] * q2[1] - q1[2] * q2[2] - q1[3] * q2[3];
 481     qout[1] = q1[0] * q2[1] + q1[1] * q2[0] + q1[2] * q2[3] - q1[3] * q2[2];
 482     qout[2] = q1[0] * q2[2] - q1[1] * q2[3] + q1[2] * q2[0] + q1[3] * q2[1];
 483     qout[3] = q1[0] * q2[3] + q1[1] * q2[2] - q1[2] * q2[1] + q1[3] * q2[0];
 484 }
 485
 486 /**
 487  * @brief Rotate a vector by a rotation matrix
 488  * @param[in] R a three by three rotation matrix (first index is row)
 489  * @param[in] vec the source vector
 490  * @param[out] vec_out the output vector
 491  */
 492 void rot_mult(float R[3][3], const float vec[3], float vec_out[3])
 493 {
 494     vec_out[0] = R[0][0] * vec[0] + R[0][1] * vec[1] + R[0][2] * vec[2];
 495     vec_out[1] = R[1][0] * vec[0] + R[1][1] * vec[1] + R[1][2] * vec[2];
 496     vec_out[2] = R[2][0] * vec[0] + R[2][1] * vec[1] + R[2][2] * vec[2];
 497 }