ground/gcs/src/libs/utils/coordinateconversions.cpp

   1 /**
   2  ******************************************************************************
   3  *
   4  * @file       coordinateconversions.cpp
   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 "coordinateconversions.h"
  31 #include <stdint.h>
  32 #include <QDebug>
  33 #include <math.h>
  34
  35 #define RAD2DEG (180.0 / M_PI)
  36 #define DEG2RAD (M_PI / 180.0)
  37
  38 namespace Utils {
  39 CoordinateConversions::CoordinateConversions()
  40 {}
  41
  42 /**
  43  * Get rotation matrix from ECEF to NED for that LLA
  44  * @param[in] LLA Longitude latitude altitude for this location
  45  * @param[out] Rne[3][3] Rotation matrix
  46  */
  47 void CoordinateConversions::RneFromLLA(double LLA[3], float Rne[3][3])
  48 {
  49     float sinLat, sinLon, cosLat, cosLon;
  50
  51     sinLat    = (float)sin(DEG2RAD * LLA[0]);
  52     sinLon    = (float)sin(DEG2RAD * LLA[1]);
  53     cosLat    = (float)cos(DEG2RAD * LLA[0]);
  54     cosLon    = (float)cos(DEG2RAD * LLA[1]);
  55
  56     Rne[0][0] = -sinLat * cosLon; Rne[0][1] = -sinLat * sinLon; Rne[0][2] = cosLat;
  57     Rne[1][0] = -sinLon; Rne[1][1] = cosLon; Rne[1][2] = 0;
  58     Rne[2][0] = -cosLat * cosLon; Rne[2][1] = -cosLat * sinLon; Rne[2][2] = -sinLat;
  59 }
  60
  61 /**
  62  * Convert from LLA coordinates to ECEF coordinates
  63  * @param[in] LLA[3] latitude longitude alititude coordinates in
  64  * @param[out] ECEF[3] location in ECEF coordinates
  65  */
  66 void CoordinateConversions::LLA2ECEF(double LLA[3], double ECEF[3])
  67 {
  68     const double a = 6378137.0; // Equatorial Radius
  69     const double e = 8.1819190842622e-2; // Eccentricity
  70     double sinLat, sinLon, cosLat, cosLon;
  71     double N;
  72
  73     sinLat = sin(DEG2RAD * LLA[0]);
  74     sinLon = sin(DEG2RAD * LLA[1]);
  75     cosLat = cos(DEG2RAD * LLA[0]);
  76     cosLon = cos(DEG2RAD * LLA[1]);
  77
  78     N = a / sqrt(1.0 - e * e * sinLat * sinLat); // prime vertical radius of curvature
  79
  80     ECEF[0] = (N + LLA[2]) * cosLat * cosLon;
  81     ECEF[1] = (N + LLA[2]) * cosLat * sinLon;
  82     ECEF[2] = ((1 - e * e) * N + LLA[2]) * sinLat;
  83 }
  84
  85 /**
  86  * Convert from ECEF coordinates to LLA coordinates
  87  * @param[in] ECEF[3] location in ECEF coordinates
  88  * @param[out] LLA[3] latitude longitude alititude coordinates
  89  */
  90 int CoordinateConversions::ECEF2LLA(double ECEF[3], double LLA[3])
  91 {
  92     const double a = 6378137.0; // Equatorial Radius
  93     const double e = 8.1819190842622e-2; // Eccentricity
  94     double x = ECEF[0], y = ECEF[1], z = ECEF[2];
  95     double Lat, N, NplusH, delta, esLat;
  96     uint16_t iter;
  97
  98     LLA[1] = RAD2DEG * atan2(y, x);
  99     N = a;
 100     NplusH = N;
 101     delta  = 1;
 102     Lat    = 1;
 103     iter   = 0;
 104
 105     while (((delta > 1.0e-14) || (delta < -1.0e-14)) && (iter < 100)) {
 106         delta  = Lat - atan(z / (sqrt(x * x + y * y) * (1 - (N * e * e / NplusH))));
 107         Lat    = Lat - delta;
 108         esLat  = e * sin(Lat);
 109         N      = a / sqrt(1 - esLat * esLat);
 110         NplusH = sqrt(x * x + y * y) / cos(Lat);
 111         iter  += 1;
 112     }
 113
 114     LLA[0] = RAD2DEG * Lat;
 115     LLA[2] = NplusH - N;
 116
 117     if (iter == 500) {
 118         return 0;
 119     } else { return 1; }
 120 }
 121
 122 /**
 123  * Get the current location in Longitude, Latitude Altitude (above WSG-84 ellipsoid)
 124  * @param[in] BaseECEF the ECEF of the home location (in m)
 125  * @param[in] NED the offset from the home location (in m)
 126  * @param[out] position three element double for position in decimal degrees and altitude in meters
 127  * @returns
 128  *  @arg 0 success
 129  *  @arg -1 for failure
 130  */
 131 int CoordinateConversions::NED2LLA_HomeECEF(double BaseECEFm[3], double NED[3], double position[3])
 132 {
 133     int i;
 134     // stored value is in cm, convert to m
 135     double BaseLLA[3];
 136     double ECEF[3];
 137     float Rne[3][3];
 138
 139     // Get LLA address to compute conversion matrix
 140     ECEF2LLA(BaseECEFm, BaseLLA);
 141     RneFromLLA(BaseLLA, Rne);
 142
 143     /* P = ECEF + Rne' * NED */
 144     for (i = 0; i < 3; i++) {
 145         ECEF[i] = BaseECEFm[i] + Rne[0][i] * NED[0] + Rne[1][i] * NED[1] + Rne[2][i] * NED[2];
 146     }
 147
 148     ECEF2LLA(ECEF, position);
 149
 150     return 0;
 151 }
 152
 153 /**
 154  * Get the current location in Longitude, Latitude, Altitude (above WSG-84 ellipsoid)
 155  * @param[in] homeLLA the latitude, longitude, and altitude of the home location (in [m])
 156  * @param[in] NED the offset from the home location (in [m])
 157  * @param[out] position three element double for position in decimal degrees and altitude in meters
 158  * @returns
 159  *  @arg 0 success
 160  *  @arg -1 for failure
 161  */
 162 int CoordinateConversions::NED2LLA_HomeLLA(double homeLLA[3], double NED[3], double position[3])
 163 {
 164     double T[3];
 165
 166     T[0] = homeLLA[2] + 6.378137E6f * M_PI / 180.0;
 167     T[1] = cosf(homeLLA[0] * M_PI / 180.0) * (homeLLA[2] + 6.378137E6f) * M_PI / 180.0;
 168     T[2] = -1.0f;
 169
 170     position[0] = homeLLA[0] + NED[0] / T[0];
 171     position[1] = homeLLA[1] + NED[1] / T[1];
 172     position[2] = homeLLA[2] + NED[2] / T[2];
 173
 174     return 0;
 175 }
 176
 177 void CoordinateConversions::LLA2Base(double LLA[3], double BaseECEF[3], float Rne[3][3], float NED[3])
 178 {
 179     double ECEF[3];
 180     float diff[3];
 181
 182     LLA2ECEF(LLA, ECEF);
 183
 184     diff[0] = (float)(ECEF[0] - BaseECEF[0]);
 185     diff[1] = (float)(ECEF[1] - BaseECEF[1]);
 186     diff[2] = (float)(ECEF[2] - BaseECEF[2]);
 187
 188     NED[0]  = Rne[0][0] * diff[0] + Rne[0][1] * diff[1] + Rne[0][2] * diff[2];
 189     NED[1]  = Rne[1][0] * diff[0] + Rne[1][1] * diff[1] + Rne[1][2] * diff[2];
 190     NED[2]  = Rne[2][0] * diff[0] + Rne[2][1] * diff[1] + Rne[2][2] * diff[2];
 191 }
 192
 193 // ****** find roll, pitch, yaw from quaternion ********
 194 void CoordinateConversions::Quaternion2RPY(const float q[4], float rpy[3])
 195 {
 196     float R13, R11, R12, R23, R33;
 197     float q0s = q[0] * q[0];
 198     float q1s = q[1] * q[1];
 199     float q2s = q[2] * q[2];
 200     float q3s = q[3] * q[3];
 201
 202     R13    = 2 * (q[1] * q[3] - q[0] * q[2]);
 203     R11    = q0s + q1s - q2s - q3s;
 204     R12    = 2 * (q[1] * q[2] + q[0] * q[3]);
 205     R23    = 2 * (q[2] * q[3] + q[0] * q[1]);
 206     R33    = q0s - q1s - q2s + q3s;
 207
 208     rpy[1] = RAD2DEG * asinf(-R13); // pitch always between -pi/2 to pi/2
 209     rpy[2] = RAD2DEG * atan2f(R12, R11);
 210     rpy[0] = RAD2DEG * atan2f(R23, R33);
 211
 212     // TODO: consider the cases where |R13| ~= 1, |pitch| ~= pi/2
 213 }
 214
 215 // ****** find quaternion from roll, pitch, yaw ********
 216 void CoordinateConversions::RPY2Quaternion(const float rpy[3], float q[4])
 217 {
 218     float phi, theta, psi;
 219     float cphi, sphi, ctheta, stheta, cpsi, spsi;
 220
 221     phi    = DEG2RAD * rpy[0] / 2;
 222     theta  = DEG2RAD * rpy[1] / 2;
 223     psi    = DEG2RAD * rpy[2] / 2;
 224     cphi   = cosf(phi);
 225     sphi   = sinf(phi);
 226     ctheta = cosf(theta);
 227     stheta = sinf(theta);
 228     cpsi   = cosf(psi);
 229     spsi   = sinf(psi);
 230
 231     q[0]   = cphi * ctheta * cpsi + sphi * stheta * spsi;
 232     q[1]   = sphi * ctheta * cpsi - cphi * stheta * spsi;
 233     q[2]   = cphi * stheta * cpsi + sphi * ctheta * spsi;
 234     q[3]   = cphi * ctheta * spsi - sphi * stheta * cpsi;
 235
 236     if (q[0] < 0) { // q0 always positive for uniqueness
 237         q[0] = -q[0];
 238         q[1] = -q[1];
 239         q[2] = -q[2];
 240         q[3] = -q[3];
 241     }
 242 }
 243
 244 // ** Find Rbe, that rotates a vector from earth fixed to body frame, from quaternion **
 245 void CoordinateConversions::Quaternion2R(const float q[4], float Rbe[3][3])
 246 {
 247     float q0s = q[0] * q[0], q1s = q[1] * q[1], q2s = q[2] * q[2], q3s = q[3] * q[3];
 248
 249     Rbe[0][0] = q0s + q1s - q2s - q3s;
 250     Rbe[0][1] = 2 * (q[1] * q[2] + q[0] * q[3]);
 251     Rbe[0][2] = 2 * (q[1] * q[3] - q[0] * q[2]);
 252     Rbe[1][0] = 2 * (q[1] * q[2] - q[0] * q[3]);
 253     Rbe[1][1] = q0s - q1s + q2s - q3s;
 254     Rbe[1][2] = 2 * (q[2] * q[3] + q[0] * q[1]);
 255     Rbe[2][0] = 2 * (q[1] * q[3] + q[0] * q[2]);
 256     Rbe[2][1] = 2 * (q[2] * q[3] - q[0] * q[1]);
 257     Rbe[2][2] = q0s - q1s - q2s + q3s;
 258 }
 259
 260 // ** Find quaternion vector from a rotation matrix, Rbe, a matrix which rotates a vector from earth frame to body frame **
 261 void CoordinateConversions::R2Quaternion(float const Rbe[3][3], float q[4])
 262 {
 263     qreal w, x, y, z;
 264
 265     // w always >= 0
 266     w    = sqrt(std::max(0.0, 1.0 + Rbe[0][0] + Rbe[1][1] + Rbe[2][2])) / 2.0;
 267     x    = sqrt(std::max(0.0, 1.0 + Rbe[0][0] - Rbe[1][1] - Rbe[2][2])) / 2.0;
 268     y    = sqrt(std::max(0.0, 1.0 - Rbe[0][0] + Rbe[1][1] - Rbe[2][2])) / 2.0;
 269     z    = sqrt(std::max(0.0, 1.0 - Rbe[0][0] - Rbe[1][1] + Rbe[2][2])) / 2.0;
 270
 271     x    = copysign(x, (Rbe[1][2] - Rbe[2][1]));
 272     y    = copysign(y, (Rbe[2][0] - Rbe[0][2]));
 273     z    = copysign(z, (Rbe[0][1] - Rbe[1][0]));
 274
 275     q[0] = w;
 276     q[1] = x;
 277     q[2] = y;
 278     q[3] = z;
 279 }
 280 }