Racelogic / Mbed 2 deprecated VIPS_LTC_RAW_IMU

Important changes to repositories hosted on mbed.com

Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.

To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.

It is also possible to export all your personal repositories from the account settings page.

Dependencies:   BufferedSerial FatFileSystemCpp mbed

PNTSerial.cpp@83:f0d1d948c306, 2022-11-15 (annotated)

Committer:
AndyA
Date:
Tue Nov 15 10:28:47 2022 +0000
Revision:
83:f0d1d948c306
Parent:
82:ee6eed2a51bd 
improve message parsing. Efficiency improvement in position conversion.

Who changed what in which revision?

UserRevisionLine numberNew contents of line
AndyA 83:f0d1d948c306 1 #include "PNTSerial.h"
AndyA 82:ee6eed2a51bd 2 #include "LTCApp.h"
AndyA 82:ee6eed2a51bd 3
AndyA 82:ee6eed2a51bd 4
AndyA 83:f0d1d948c306 5 PNTSerial::PNTSerial(const PinName Tx, const PinName Rx) : PosSource(Tx, Rx) {
AndyA 83:f0d1d948c306 6 originSet = false;
AndyA 82:ee6eed2a51bd 7 }
AndyA 82:ee6eed2a51bd 8
AndyA 82:ee6eed2a51bd 9 void PNTSerial::setOrigin(double Latitude, double Longitude, float altitude) {
AndyA 83:f0d1d948c306 10 origin.SetDecimalDegrees(Latitude, Longitude, altitude);
AndyA 83:f0d1d948c306 11 dataPoint.SetReferancePosition(&origin);
AndyA 83:f0d1d948c306 12 originSet = true;
AndyA 82:ee6eed2a51bd 13 }
AndyA 82:ee6eed2a51bd 14
AndyA 83:f0d1d948c306 15 void PNTSerial::run(void) {
AndyA 83:f0d1d948c306 16 _port.attach(callback(this, &PNTSerial::onSerialRx));
AndyA 82:ee6eed2a51bd 17 }
AndyA 82:ee6eed2a51bd 18
AndyA 83:f0d1d948c306 19 void PNTSerial::onSerialRx(void) {
AndyA 83:f0d1d948c306 20 while (_port.readable()) {
AndyA 83:f0d1d948c306 21 if (BypassMode) {
AndyA 83:f0d1d948c306 22 vipsBypassRx(_port.getc());
AndyA 83:f0d1d948c306 23 } else {
AndyA 83:f0d1d948c306 24 uint8_t charIn = _port.getc();
AndyA 83:f0d1d948c306 25 if (messagePrt == 0) { // wait for start character
AndyA 83:f0d1d948c306 26 if (charIn == '$') {
AndyA 83:f0d1d948c306 27 messageInBuffer[messagePrt++] = charIn;
AndyA 82:ee6eed2a51bd 28 }
AndyA 83:f0d1d948c306 29 } else {
AndyA 83:f0d1d948c306 30 messageInBuffer[messagePrt++] = charIn;
AndyA 83:f0d1d948c306 31 // basic sanity check that second byte is expected value.
AndyA 83:f0d1d948c306 32 if ((messagePrt==2) && (messageInBuffer[1] != 'R'))
AndyA 83:f0d1d948c306 33 messagePrt = 0;
AndyA 83:f0d1d948c306 34 // got the full message
AndyA 83:f0d1d948c306 35 else if (messagePrt == PNTSerialConstants::MessageLength) {
AndyA 83:f0d1d948c306 36 parsePostion();
AndyA 83:f0d1d948c306 37 messagePrt = 0;
AndyA 83:f0d1d948c306 38 }
AndyA 83:f0d1d948c306 39 }
AndyA 82:ee6eed2a51bd 40 }
AndyA 83:f0d1d948c306 41 }
AndyA 82:ee6eed2a51bd 42 }
AndyA 82:ee6eed2a51bd 43
AndyA 82:ee6eed2a51bd 44 bool PNTSerial::CheckValidMessage() {
AndyA 83:f0d1d948c306 45 // check length
AndyA 83:f0d1d948c306 46 if (messagePrt != PNTSerialConstants::MessageLength)
AndyA 83:f0d1d948c306 47 return false;
AndyA 82:ee6eed2a51bd 48
AndyA 83:f0d1d948c306 49 // check header
AndyA 83:f0d1d948c306 50 if (messageInBuffer[0] != '$')
AndyA 83:f0d1d948c306 51 return false;
AndyA 83:f0d1d948c306 52 if (messageInBuffer[1] != 'R')
AndyA 83:f0d1d948c306 53 return false;
AndyA 83:f0d1d948c306 54 if (messageInBuffer[2] != 'L')
AndyA 83:f0d1d948c306 55 return false;
AndyA 83:f0d1d948c306 56 if (messageInBuffer[3] != 'P')
AndyA 83:f0d1d948c306 57 return false;
AndyA 83:f0d1d948c306 58 if (messageInBuffer[4] != 'N')
AndyA 83:f0d1d948c306 59 return false;
AndyA 83:f0d1d948c306 60 if (messageInBuffer[5] != 'T')
AndyA 83:f0d1d948c306 61 return false;
AndyA 83:f0d1d948c306 62 if (messageInBuffer[6] != '$')
AndyA 83:f0d1d948c306 63 return false;
AndyA 83:f0d1d948c306 64 if (messageInBuffer[7] != ',')
AndyA 83:f0d1d948c306 65 return false;
AndyA 82:ee6eed2a51bd 66
AndyA 83:f0d1d948c306 67 // check CRC
AndyA 83:f0d1d948c306 68 uint16_t Polynomial = 4129;
AndyA 83:f0d1d948c306 69 uint16_t CRC = 0;
AndyA 83:f0d1d948c306 70 for (int i = 0; i < PNTSerialConstants::MessageLength - 2; i++) {
AndyA 83:f0d1d948c306 71 int tmp = messageInBuffer[i];
AndyA 83:f0d1d948c306 72 CRC ^= tmp << 8;
AndyA 83:f0d1d948c306 73 for (int bit = 8; bit > 0; bit--) {
AndyA 83:f0d1d948c306 74 if (CRC & 0x8000) {
AndyA 83:f0d1d948c306 75 CRC <<= 1;
AndyA 83:f0d1d948c306 76 CRC ^= Polynomial;
AndyA 83:f0d1d948c306 77 } else {
AndyA 83:f0d1d948c306 78 CRC <<= 1;
AndyA 83:f0d1d948c306 79 }
AndyA 82:ee6eed2a51bd 80 }
AndyA 83:f0d1d948c306 81 CRC &= 0xffff;
AndyA 83:f0d1d948c306 82 }
AndyA 83:f0d1d948c306 83 uint16_t messageCRC = ((uint16_t)messageInBuffer[PNTSerialConstants::MessageLength - 2]) << 8 |
AndyA 83:f0d1d948c306 84 messageInBuffer[PNTSerialConstants::MessageLength - 1];
AndyA 83:f0d1d948c306 85 return messageCRC == CRC;
AndyA 82:ee6eed2a51bd 86 }
AndyA 82:ee6eed2a51bd 87
AndyA 82:ee6eed2a51bd 88 uint32_t PNTSerial::readUInt32(int &start, int bytes) {
AndyA 83:f0d1d948c306 89 uint32_t value = 0;
AndyA 83:f0d1d948c306 90 do {
AndyA 83:f0d1d948c306 91 value <<= 8;
AndyA 83:f0d1d948c306 92 value |= messageInBuffer[start++];
AndyA 83:f0d1d948c306 93 } while (--bytes);
AndyA 83:f0d1d948c306 94 return value;
AndyA 82:ee6eed2a51bd 95 }
AndyA 82:ee6eed2a51bd 96
AndyA 82:ee6eed2a51bd 97 int32_t PNTSerial::readInt32(int &start, int bytes) {
AndyA 83:f0d1d948c306 98 int32_t value = 0;
AndyA 83:f0d1d948c306 99 if (messageInBuffer[start] & 0x80) // negative value
AndyA 83:f0d1d948c306 100 value = -1; // set to all 1's
AndyA 82:ee6eed2a51bd 101
AndyA 83:f0d1d948c306 102 do {
AndyA 83:f0d1d948c306 103 value <<= 8;
AndyA 83:f0d1d948c306 104 value |= messageInBuffer[start++];
AndyA 83:f0d1d948c306 105 } while (--bytes);
AndyA 83:f0d1d948c306 106 return value;
AndyA 82:ee6eed2a51bd 107 }
AndyA 82:ee6eed2a51bd 108
AndyA 82:ee6eed2a51bd 109 int64_t PNTSerial::readInt64(int &start, int bytes) {
AndyA 83:f0d1d948c306 110 int64_t value = 0;
AndyA 83:f0d1d948c306 111 if (messageInBuffer[start] & 0x80) // negative value
AndyA 83:f0d1d948c306 112 value = -1; // set to all 1's
AndyA 83:f0d1d948c306 113 do {
AndyA 83:f0d1d948c306 114 value <<= 8;
AndyA 83:f0d1d948c306 115 value |= messageInBuffer[start++];
AndyA 83:f0d1d948c306 116 } while (--bytes);
AndyA 82:ee6eed2a51bd 117
AndyA 83:f0d1d948c306 118 return value;
AndyA 82:ee6eed2a51bd 119 }
AndyA 82:ee6eed2a51bd 120
AndyA 82:ee6eed2a51bd 121 void PNTSerial::parsePostion() {
AndyA 83:f0d1d948c306 122 int messageIndex = 0;
AndyA 83:f0d1d948c306 123 lastPositions[nextPosition].time = TimeSinceLastFrame.read_us();
AndyA 83:f0d1d948c306 124 if (!CheckValidMessage())
AndyA 83:f0d1d948c306 125 return;
AndyA 83:f0d1d948c306 126 // 8 byte header ("$RLPNT$,")
AndyA 83:f0d1d948c306 127 messageIndex = 8;
AndyA 83:f0d1d948c306 128 // index 8 = 1 byte sat count
AndyA 83:f0d1d948c306 129 lastPositions[nextPosition].pos.beacons = messageInBuffer[messageIndex++];
AndyA 82:ee6eed2a51bd 130
AndyA 83:f0d1d948c306 131 // 3 bytes time since midnight (10ms per tick)
AndyA 83:f0d1d948c306 132 lastPositions[nextPosition].pos.time = readUInt32(messageIndex, 3) * 10;
AndyA 82:ee6eed2a51bd 133
AndyA 83:f0d1d948c306 134 // 5 byte lattitude in 0.000000001 degrees steps (9 decimal places)
AndyA 83:f0d1d948c306 135 int64_t latInt = readInt64(messageIndex, 5);
AndyA 83:f0d1d948c306 136 // 5 byte longitude in 0.000000001 degrees steps (9 decimal places)
AndyA 83:f0d1d948c306 137 int64_t lonInt = readInt64(messageIndex, 5);
AndyA 82:ee6eed2a51bd 138
AndyA 83:f0d1d948c306 139 // 3 byte velocity
AndyA 83:f0d1d948c306 140 messageIndex += 3;
AndyA 83:f0d1d948c306 141 // 2 byte heading
AndyA 83:f0d1d948c306 142 messageIndex += 2;
AndyA 83:f0d1d948c306 143 // 3 byte altitude
AndyA 83:f0d1d948c306 144 int32_t altInt = readInt32(messageIndex, 3);
AndyA 83:f0d1d948c306 145 if (originSet) {
AndyA 83:f0d1d948c306 146 dataPoint.SetDecimalDegrees(latInt * 0.000000001, lonInt * 0.000000001,
AndyA 83:f0d1d948c306 147 altInt * 0.01);
AndyA 83:f0d1d948c306 148 double tmpDouble;
AndyA 83:f0d1d948c306 149 dataPoint.GetENU(&(lastPositions[nextPosition].pos.X),
AndyA 83:f0d1d948c306 150 &(lastPositions[nextPosition].pos.Y), &tmpDouble);
AndyA 83:f0d1d948c306 151 lastPositions[nextPosition].pos.Height = (float)tmpDouble;
AndyA 83:f0d1d948c306 152 lastPositions[nextPosition].pos.LLAPosition = false;
AndyA 83:f0d1d948c306 153 } else {
AndyA 83:f0d1d948c306 154 lastPositions[nextPosition].pos.X = latInt * 0.000000001;
AndyA 83:f0d1d948c306 155 lastPositions[nextPosition].pos.Y = lonInt * 0.000000001;
AndyA 83:f0d1d948c306 156 lastPositions[nextPosition].pos.Height = altInt * 0.01f;
AndyA 83:f0d1d948c306 157 lastPositions[nextPosition].pos.LLAPosition = true;
AndyA 83:f0d1d948c306 158 }
AndyA 82:ee6eed2a51bd 159
AndyA 83:f0d1d948c306 160 // 3 byte vert velocity
AndyA 83:f0d1d948c306 161 messageIndex += 3;
AndyA 83:f0d1d948c306 162
AndyA 83:f0d1d948c306 163 // 1 byte solution type
AndyA 83:f0d1d948c306 164 lastPositions[nextPosition].pos.solutionType =
AndyA 83:f0d1d948c306 165 messageInBuffer[messageIndex++];
AndyA 82:ee6eed2a51bd 166
AndyA 83:f0d1d948c306 167 // 2 byte pitch
AndyA 83:f0d1d948c306 168 lastPositions[nextPosition].pos.pitch = readInt32(messageIndex, 2) * 0.01;
AndyA 83:f0d1d948c306 169 // 2 byte roll
AndyA 83:f0d1d948c306 170 lastPositions[nextPosition].pos.roll = readInt32(messageIndex, 2) * 0.01;
AndyA 83:f0d1d948c306 171 // 2 byte yaw
AndyA 83:f0d1d948c306 172 lastPositions[nextPosition].pos.yaw = readInt32(messageIndex, 2) * 0.01;
AndyA 82:ee6eed2a51bd 173
AndyA 83:f0d1d948c306 174 // 2 byte pitch, roll, yaw rates
AndyA 83:f0d1d948c306 175 messageIndex += 2*3;
AndyA 82:ee6eed2a51bd 176
AndyA 83:f0d1d948c306 177 // 2 byte X,Y,Z accelerations
AndyA 83:f0d1d948c306 178 messageIndex += 2*3;
AndyA 83:f0d1d948c306 179
AndyA 83:f0d1d948c306 180 // 2 byte KF status
AndyA 83:f0d1d948c306 181 lastPositions[nextPosition].pos.KFStatus = (uint16_t)readUInt32(messageIndex, 2);
AndyA 82:ee6eed2a51bd 182
AndyA 83:f0d1d948c306 183 // 3 byte wheel speed
AndyA 83:f0d1d948c306 184 messageIndex += 3;
AndyA 83:f0d1d948c306 185 // 1 byte spoof/jam mask
AndyA 83:f0d1d948c306 186 messageIndex++;
AndyA 83:f0d1d948c306 187 // 1 byte spoof/jam indicator
AndyA 83:f0d1d948c306 188 messageIndex++;
AndyA 83:f0d1d948c306 189 // 2 byte date
AndyA 83:f0d1d948c306 190 messageIndex += 2;
AndyA 82:ee6eed2a51bd 191
AndyA 83:f0d1d948c306 192 if (UserSettings.AutoHyperSmooth) {
AndyA 83:f0d1d948c306 193 int testValue = (lastPositions[nextPosition].pos.KFStatus & 0xE634);
AndyA 82:ee6eed2a51bd 194
AndyA 83:f0d1d948c306 195 if (((testValue & 0x400) == 0x400) && (!hyperSmoothEnabled)) {
AndyA 83:f0d1d948c306 196 EnableSmoothing(true);
AndyA 83:f0d1d948c306 197 // pc.write("Auto HS On\r\n", 12);
AndyA 83:f0d1d948c306 198 } else if (((testValue & 0x400) != 0x400) && (hyperSmoothEnabled) &&
AndyA 83:f0d1d948c306 199 (!forcedHyperSmooth)) {
AndyA 83:f0d1d948c306 200 EnableSmoothing(false);
AndyA 83:f0d1d948c306 201 // pc.write("Auto HS Off\r\n", 13);
AndyA 83:f0d1d948c306 202 } // Auto Hypersmooth
AndyA 83:f0d1d948c306 203 }
AndyA 82:ee6eed2a51bd 204
AndyA 83:f0d1d948c306 205 smoothOutputPacket(&(lastPositions[nextPosition].pos));
AndyA 83:f0d1d948c306 206
AndyA 83:f0d1d948c306 207 if (enableAllUpdates) {
AndyA 83:f0d1d948c306 208 // printf("Add pos\r\n");
AndyA 83:f0d1d948c306 209 outputPtr = &outputPosition;
AndyA 83:f0d1d948c306 210 memcpy(outputPtr, &(lastPositions[nextPosition].pos), sizeof(position));
AndyA 83:f0d1d948c306 211 }
AndyA 82:ee6eed2a51bd 212
AndyA 83:f0d1d948c306 213 nextPosition++;
AndyA 83:f0d1d948c306 214 if (nextPosition == posHistoryLen) {
AndyA 83:f0d1d948c306 215 nextPosition = 0;
AndyA 83:f0d1d948c306 216 }
AndyA 83:f0d1d948c306 217 pointCount++;
AndyA 83:f0d1d948c306 218 }
AndyA 82:ee6eed2a51bd 219