diff options
| author | Wei (GPS SD) Chen <weic@codeaurora.org> | 2020-10-23 10:38:24 -0700 |
|---|---|---|
| committer | Wei (GPS SD) Chen <weic@codeaurora.org> | 2020-10-23 10:43:55 -0700 |
| commit | 393236f4220fab1b89f5f926dbd138b6241f27e5 (patch) | |
| tree | b9765cefa0d1c5641d2d1772cd65d1e3990f6c2d | |
| parent | 76274627d126d46413f4068e9616b3c87db49edc (diff) | |
VRP based conversion: fix mix-up with up and down parameter
The input parameter is up offset, while the formula is using
it as down offset
Change-Id: Ic37a506ccb772b72564336307c8a928b0ec9d682
CRs-fixed: 2804313
| -rw-r--r-- | utils/loc_misc_utils.cpp | 10 |
1 files changed, 5 insertions, 5 deletions
diff --git a/utils/loc_misc_utils.cpp b/utils/loc_misc_utils.cpp index d0a69e0..c9cb71d 100644 --- a/utils/loc_misc_utils.cpp +++ b/utils/loc_misc_utils.cpp @@ -248,15 +248,15 @@ void loc_convert_lla_gnss_to_vrp(double lla[3], float rollPitchYaw[3], float rn = A6DOF_WGS_B * sf * sfr + lla[2]; float re = A6DOF_WGS_A * sfr + lla[2]; - float deltaNED[3]; + float deltaNEU[3]; // gps_pos_lla = imu_pos_lla + Cbn*la_b .* [1/geo.Rn; 1/(geo.Re*geo.cL); -1]; - Matrix_MxV(cnb, leverArm, deltaNED); + Matrix_MxV(cnb, leverArm, deltaNEU); // NED to lla conversion - lla[0] = lla[0] + deltaNED[0] / rn; - lla[1] = lla[1] + deltaNED[1] / (re * cl); - lla[2] = lla[2] - deltaNED[2]; + lla[0] = lla[0] + deltaNEU[0] / rn; + lla[1] = lla[1] + deltaNEU[1] / (re * cl); + lla[2] = lla[2] + deltaNEU[2]; } // Used for convert velocity from GSNS based to VRP based |