Mercurial > public > ostc4
annotate Small_CPU/Src/GNSS.c @ 901:e4e9acfde839 Evo_2_23
Bugfix simulator/planer:
For deco calculation two structures are used. The calculation structure and the input structure. During simulation fast forward (+5min) the input structure is manipulated. Especially for vpm calculation it could happen that the input structure was manipulated and then overwritten by the calculation structure => deco and tts may have wrong values. To avoid this thedeco calculation status is now checked before doing the FF manupulation. Based an calculation state deco or input structures are manipulated.
Surface time stamp in planer view:
The planer used its own (buggy) implementation for calculation of tts. The timestamp for the surface arrival did not match the bottom time + TTS. The new implementation uses the tts calculated by the deco loop for generation of surface time stamp.
| author | Ideenmodellierer |
|---|---|
| date | Wed, 02 Oct 2024 22:07:13 +0200 |
| parents | 2225c467f1e9 |
| children | c0553dd70608 |
| rev | line source |
|---|---|
| 887 | 1 /* |
| 2 * GNSS.c | |
| 3 * | |
| 4 * Created on: 03.10.2020 | |
| 5 * Author: SimpleMethod | |
| 6 * | |
| 7 *Copyright 2020 SimpleMethod | |
| 8 * | |
| 9 *Permission is hereby granted, free of charge, to any person obtaining a copy of | |
| 10 *this software and associated documentation files (the "Software"), to deal in | |
| 11 *the Software without restriction, including without limitation the rights to | |
| 12 *use, copy, modify, merge, publish, distribute, sublicense, and/or sell copies | |
| 13 *of the Software, and to permit persons to whom the Software is furnished to do | |
| 14 *so, subject to the following conditions: | |
| 15 * | |
| 16 *The above copyright notice and this permission notice shall be included in all | |
| 17 *copies or substantial portions of the Software. | |
| 18 * | |
| 19 *THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR | |
| 20 *IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, | |
| 21 *FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE | |
| 22 *AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER | |
| 23 *LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, | |
| 24 *OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN | |
| 25 *THE SOFTWARE. | |
| 26 ****************************************************************************** | |
| 27 */ | |
| 28 | |
| 29 #include "GNSS.h" | |
| 30 | |
| 31 union u_Short uShort; | |
| 32 union i_Short iShort; | |
| 33 union u_Long uLong; | |
| 34 union i_Long iLong; | |
| 35 | |
| 36 /*! | |
| 37 * Structure initialization. | |
| 38 * @param GNSS Pointer to main GNSS structure. | |
| 39 * @param huart Pointer to uart handle. | |
| 40 */ | |
| 41 void GNSS_Init(GNSS_StateHandle *GNSS, UART_HandleTypeDef *huart) { | |
| 42 GNSS->huart = huart; | |
| 43 GNSS->year = 0; | |
| 44 GNSS->month = 0; | |
| 45 GNSS->day = 0; | |
| 46 GNSS->hour = 0; | |
| 47 GNSS->min = 0; | |
| 48 GNSS->sec = 0; | |
| 49 GNSS->fixType = 0; | |
| 50 GNSS->lon = 0; | |
| 51 GNSS->lat = 0; | |
| 52 GNSS->height = 0; | |
| 53 GNSS->hMSL = 0; | |
| 54 GNSS->hAcc = 0; | |
| 55 GNSS->vAcc = 0; | |
| 56 GNSS->gSpeed = 0; | |
| 57 GNSS->headMot = 0; | |
| 58 } | |
| 59 | |
| 60 /*! | |
| 61 * Searching for a header in data buffer and matching class and message ID to buffer data. | |
| 62 * @param GNSS Pointer to main GNSS structure. | |
| 63 */ | |
|
899
2225c467f1e9
Added data path and visualization for position data:
Ideenmodellierer
parents:
887
diff
changeset
|
64 uint8_t GNSS_ParseBuffer(GNSS_StateHandle *GNSS) { |
|
2225c467f1e9
Added data path and visualization for position data:
Ideenmodellierer
parents:
887
diff
changeset
|
65 |
|
2225c467f1e9
Added data path and visualization for position data:
Ideenmodellierer
parents:
887
diff
changeset
|
66 uint8_t DataReceived = 0; |
| 887 | 67 |
| 68 for (int var = 0; var <= 100; ++var) { | |
| 69 if (GNSS->uartWorkingBuffer[var] == 0xB5 | |
| 70 && GNSS->uartWorkingBuffer[var + 1] == 0x62) { | |
|
899
2225c467f1e9
Added data path and visualization for position data:
Ideenmodellierer
parents:
887
diff
changeset
|
71 DataReceived = 1; |
| 887 | 72 if (GNSS->uartWorkingBuffer[var + 2] == 0x27 |
| 73 && GNSS->uartWorkingBuffer[var + 3] == 0x03) { //Look at: 32.19.1.1 u-blox 8 Receiver description | |
| 74 GNSS_ParseUniqID(GNSS); | |
| 75 } else if (GNSS->uartWorkingBuffer[var + 2] == 0x01 | |
| 76 && GNSS->uartWorkingBuffer[var + 3] == 0x21) { //Look at: 32.17.14.1 u-blox 8 Receiver description | |
| 77 GNSS_ParseNavigatorData(GNSS); | |
| 78 } else if (GNSS->uartWorkingBuffer[var + 2] == 0x01 | |
| 79 && GNSS->uartWorkingBuffer[var + 3] == 0x07) { //ook at: 32.17.30.1 u-blox 8 Receiver description | |
| 80 GNSS_ParsePVTData(GNSS); | |
| 81 } else if (GNSS->uartWorkingBuffer[var + 2] == 0x01 | |
| 82 && GNSS->uartWorkingBuffer[var + 3] == 0x02) { // Look at: 32.17.15.1 u-blox 8 Receiver description | |
| 83 GNSS_ParsePOSLLHData(GNSS); | |
| 84 } | |
| 85 } | |
| 86 } | |
|
899
2225c467f1e9
Added data path and visualization for position data:
Ideenmodellierer
parents:
887
diff
changeset
|
87 return DataReceived; |
| 887 | 88 } |
| 89 | |
| 90 /*! | |
| 91 * Make request for unique chip ID data. | |
| 92 * @param GNSS Pointer to main GNSS structure. | |
| 93 */ | |
| 94 void GNSS_GetUniqID(GNSS_StateHandle *GNSS) { | |
| 95 HAL_UART_Transmit_DMA(GNSS->huart, getDeviceID, | |
| 96 sizeof(getDeviceID) / sizeof(uint8_t)); | |
| 97 HAL_UART_Receive_IT(GNSS->huart, GNSS_Handle.uartWorkingBuffer, 17); | |
| 98 } | |
| 99 | |
| 100 /*! | |
| 101 * Make request for UTC time solution data. | |
| 102 * @param GNSS Pointer to main GNSS structure. | |
| 103 */ | |
| 104 void GNSS_GetNavigatorData(GNSS_StateHandle *GNSS) { | |
| 105 HAL_UART_Transmit_DMA(GNSS->huart, getNavigatorData, | |
| 106 sizeof(getNavigatorData) / sizeof(uint8_t)); | |
| 107 HAL_UART_Receive_IT(GNSS->huart, GNSS_Handle.uartWorkingBuffer, 28); | |
| 108 } | |
| 109 | |
| 110 /*! | |
| 111 * Make request for geodetic position solution data. | |
| 112 * @param GNSS Pointer to main GNSS structure. | |
| 113 */ | |
| 114 void GNSS_GetPOSLLHData(GNSS_StateHandle *GNSS) { | |
| 115 HAL_UART_Transmit_DMA(GNSS->huart, getPOSLLHData, | |
| 116 sizeof(getPOSLLHData) / sizeof(uint8_t)); | |
| 117 HAL_UART_Receive_IT(GNSS->huart, GNSS_Handle.uartWorkingBuffer, 36); | |
| 118 } | |
| 119 | |
| 120 /*! | |
| 121 * Make request for navigation position velocity time solution data. | |
| 122 * @param GNSS Pointer to main GNSS structure. | |
| 123 */ | |
| 124 void GNSS_GetPVTData(GNSS_StateHandle *GNSS) { | |
| 125 HAL_UART_Transmit_DMA(GNSS->huart, getPVTData, | |
| 126 sizeof(getPVTData) / sizeof(uint8_t)); | |
| 127 HAL_UART_Receive_IT(GNSS->huart, GNSS_Handle.uartWorkingBuffer, 100); | |
| 128 } | |
| 129 | |
| 130 /*! | |
| 131 * Parse data to unique chip ID standard. | |
| 132 * Look at: 32.19.1.1 u-blox 8 Receiver description | |
| 133 * @param GNSS Pointer to main GNSS structure. | |
| 134 */ | |
| 135 void GNSS_ParseUniqID(GNSS_StateHandle *GNSS) { | |
| 136 for (int var = 0; var < 5; ++var) { | |
| 137 GNSS->uniqueID[var] = GNSS_Handle.uartWorkingBuffer[10 + var]; | |
| 138 } | |
| 139 } | |
| 140 | |
| 141 /*! | |
| 142 * Changing the GNSS mode. | |
| 143 * Look at: 32.10.19 u-blox 8 Receiver description | |
| 144 */ | |
| 145 void GNSS_SetMode(GNSS_StateHandle *GNSS, short gnssMode) { | |
| 146 if (gnssMode == 0) { | |
| 147 HAL_UART_Transmit_DMA(GNSS->huart, setPortableMode,sizeof(setPortableMode) / sizeof(uint8_t)); | |
| 148 } else if (gnssMode == 1) { | |
| 149 HAL_UART_Transmit_DMA(GNSS->huart, setStationaryMode,sizeof(setStationaryMode) / sizeof(uint8_t)); | |
| 150 } else if (gnssMode == 2) { | |
| 151 HAL_UART_Transmit_DMA(GNSS->huart, setPedestrianMode,sizeof(setPedestrianMode) / sizeof(uint8_t)); | |
| 152 } else if (gnssMode == 3) { | |
| 153 HAL_UART_Transmit_DMA(GNSS->huart, setAutomotiveMode,sizeof(setAutomotiveMode) / sizeof(uint8_t)); | |
| 154 } else if (gnssMode == 4) { | |
| 155 HAL_UART_Transmit_DMA(GNSS->huart, setAutomotiveMode,sizeof(setAutomotiveMode) / sizeof(uint8_t)); | |
| 156 } else if (gnssMode == 5) { | |
| 157 HAL_UART_Transmit_DMA(GNSS->huart, setAirbone1GMode,sizeof(setAirbone1GMode) / sizeof(uint8_t)); | |
| 158 } else if (gnssMode == 6) { | |
| 159 HAL_UART_Transmit_DMA(GNSS->huart, setAirbone2GMode,sizeof(setAirbone2GMode) / sizeof(uint8_t)); | |
| 160 } else if (gnssMode == 7) { | |
| 161 HAL_UART_Transmit_DMA(GNSS->huart, setAirbone4GMode,sizeof(setAirbone4GMode) / sizeof(uint8_t)); | |
| 162 } else if (gnssMode == 8) { | |
| 163 HAL_UART_Transmit_DMA(GNSS->huart, setWirstMode,sizeof(setWirstMode) / sizeof(uint8_t)); | |
| 164 } else if (gnssMode == 9) { | |
| 165 HAL_UART_Transmit_DMA(GNSS->huart, setBikeMode,sizeof(setBikeMode) / sizeof(uint8_t)); | |
| 166 } | |
| 167 } | |
| 168 /*! | |
| 169 * Parse data to navigation position velocity time solution standard. | |
| 170 * Look at: 32.17.15.1 u-blox 8 Receiver description. | |
| 171 * @param GNSS Pointer to main GNSS structure. | |
| 172 */ | |
| 173 void GNSS_ParsePVTData(GNSS_StateHandle *GNSS) { | |
| 174 uShort.bytes[0] = GNSS_Handle.uartWorkingBuffer[10]; | |
| 175 GNSS->yearBytes[0]=GNSS_Handle.uartWorkingBuffer[10]; | |
| 176 uShort.bytes[1] = GNSS_Handle.uartWorkingBuffer[11]; | |
| 177 GNSS->yearBytes[1]=GNSS_Handle.uartWorkingBuffer[11]; | |
| 178 GNSS->year = uShort.uShort; | |
| 179 GNSS->month = GNSS_Handle.uartWorkingBuffer[12]; | |
| 180 GNSS->day = GNSS_Handle.uartWorkingBuffer[13]; | |
| 181 GNSS->hour = GNSS_Handle.uartWorkingBuffer[14]; | |
| 182 GNSS->min = GNSS_Handle.uartWorkingBuffer[15]; | |
| 183 GNSS->sec = GNSS_Handle.uartWorkingBuffer[16]; | |
| 184 GNSS->fixType = GNSS_Handle.uartWorkingBuffer[26]; | |
| 185 | |
| 186 for (int var = 0; var < 4; ++var) { | |
| 187 iLong.bytes[var] = GNSS_Handle.uartWorkingBuffer[var + 30]; | |
| 188 GNSS->lonBytes[var]= GNSS_Handle.uartWorkingBuffer[var + 30]; | |
| 189 } | |
| 190 GNSS->lon = iLong.iLong; | |
| 191 GNSS->fLon=(float)iLong.iLong/10000000.0; | |
| 192 for (int var = 0; var < 4; ++var) { | |
| 193 iLong.bytes[var] = GNSS_Handle.uartWorkingBuffer[var + 34]; | |
| 194 GNSS->latBytes[var]=GNSS_Handle.uartWorkingBuffer[var + 34]; | |
| 195 } | |
| 196 GNSS->lat = iLong.iLong; | |
| 197 GNSS->fLat=(float)iLong.iLong/10000000.0; | |
| 198 for (int var = 0; var < 4; ++var) { | |
| 199 iLong.bytes[var] = GNSS_Handle.uartWorkingBuffer[var + 38]; | |
| 200 } | |
| 201 GNSS->height = iLong.iLong; | |
| 202 | |
| 203 for (int var = 0; var < 4; ++var) { | |
| 204 iLong.bytes[var] = GNSS_Handle.uartWorkingBuffer[var + 42]; | |
| 205 GNSS->hMSLBytes[var] = GNSS_Handle.uartWorkingBuffer[var + 42]; | |
| 206 } | |
| 207 GNSS->hMSL = iLong.iLong; | |
| 208 | |
| 209 for (int var = 0; var < 4; ++var) { | |
| 210 uLong.bytes[var] = GNSS_Handle.uartWorkingBuffer[var + 46]; | |
| 211 } | |
| 212 GNSS->hAcc = uLong.uLong; | |
| 213 | |
| 214 for (int var = 0; var < 4; ++var) { | |
| 215 uLong.bytes[var] = GNSS_Handle.uartWorkingBuffer[var + 50]; | |
| 216 } | |
| 217 GNSS->vAcc = uLong.uLong; | |
| 218 | |
| 219 for (int var = 0; var < 4; ++var) { | |
| 220 iLong.bytes[var] = GNSS_Handle.uartWorkingBuffer[var + 66]; | |
| 221 GNSS->gSpeedBytes[var] = GNSS_Handle.uartWorkingBuffer[var + 66]; | |
| 222 } | |
| 223 GNSS->gSpeed = iLong.iLong; | |
| 224 | |
| 225 for (int var = 0; var < 4; ++var) { | |
| 226 iLong.bytes[var] = GNSS_Handle.uartWorkingBuffer[var + 70]; | |
| 227 } | |
| 228 GNSS->headMot = iLong.iLong * 1e-5; // todo I'm not sure this good options. | |
| 229 } | |
| 230 | |
| 231 /*! | |
| 232 * Parse data to UTC time solution standard. | |
| 233 * Look at: 32.17.30.1 u-blox 8 Receiver description. | |
| 234 * @param GNSS Pointer to main GNSS structure. | |
| 235 */ | |
| 236 void GNSS_ParseNavigatorData(GNSS_StateHandle *GNSS) { | |
| 237 uShort.bytes[0] = GNSS_Handle.uartWorkingBuffer[18]; | |
| 238 uShort.bytes[1] = GNSS_Handle.uartWorkingBuffer[19]; | |
| 239 GNSS->year = uShort.uShort; | |
| 240 GNSS->month = GNSS_Handle.uartWorkingBuffer[20]; | |
| 241 GNSS->day = GNSS_Handle.uartWorkingBuffer[21]; | |
| 242 GNSS->hour = GNSS_Handle.uartWorkingBuffer[22]; | |
| 243 GNSS->min = GNSS_Handle.uartWorkingBuffer[23]; | |
| 244 GNSS->sec = GNSS_Handle.uartWorkingBuffer[24]; | |
| 245 } | |
| 246 | |
| 247 /*! | |
| 248 * Parse data to geodetic position solution standard. | |
| 249 * Look at: 32.17.14.1 u-blox 8 Receiver description. | |
| 250 * @param GNSS Pointer to main GNSS structure. | |
| 251 */ | |
| 252 void GNSS_ParsePOSLLHData(GNSS_StateHandle *GNSS) { | |
| 253 for (int var = 0; var < 4; ++var) { | |
| 254 iLong.bytes[var] = GNSS_Handle.uartWorkingBuffer[var + 10]; | |
| 255 } | |
| 256 GNSS->lon = iLong.iLong; | |
| 257 GNSS->fLon=(float)iLong.iLong/10000000.0; | |
| 258 | |
| 259 for (int var = 0; var < 4; ++var) { | |
| 260 iLong.bytes[var] = GNSS_Handle.uartWorkingBuffer[var + 14]; | |
| 261 } | |
| 262 GNSS->lat = iLong.iLong; | |
| 263 GNSS->fLat=(float)iLong.iLong/10000000.0; | |
| 264 | |
| 265 for (int var = 0; var < 4; ++var) { | |
| 266 iLong.bytes[var] = GNSS_Handle.uartWorkingBuffer[var + 18]; | |
| 267 } | |
| 268 GNSS->height = iLong.iLong; | |
| 269 | |
| 270 for (int var = 0; var < 4; ++var) { | |
| 271 iLong.bytes[var] = GNSS_Handle.uartWorkingBuffer[var + 22]; | |
| 272 } | |
| 273 GNSS->hMSL = iLong.iLong; | |
| 274 | |
| 275 for (int var = 0; var < 4; ++var) { | |
| 276 uLong.bytes[var] = GNSS_Handle.uartWorkingBuffer[var + 26]; | |
| 277 } | |
| 278 GNSS->hAcc = uLong.uLong; | |
| 279 | |
| 280 for (int var = 0; var < 4; ++var) { | |
| 281 uLong.bytes[var] = GNSS_Handle.uartWorkingBuffer[var + 30]; | |
| 282 } | |
| 283 GNSS->vAcc = uLong.uLong; | |
| 284 } | |
| 285 | |
| 286 /*! | |
| 287 * Sends the basic configuration: Activation of the UBX standard, change of NMEA version to 4.10 and turn on of the Galileo system. | |
| 288 * @param GNSS Pointer to main GNSS structure. | |
| 289 */ | |
| 290 void GNSS_LoadConfig(GNSS_StateHandle *GNSS) { | |
| 291 HAL_UART_Transmit_DMA(GNSS->huart, configUBX, | |
| 292 sizeof(configUBX) / sizeof(uint8_t)); | |
| 293 HAL_Delay(250); | |
| 294 HAL_UART_Transmit_DMA(GNSS->huart, setNMEA410, | |
| 295 sizeof(setNMEA410) / sizeof(uint8_t)); | |
| 296 HAL_Delay(250); | |
| 297 HAL_UART_Transmit_DMA(GNSS->huart, setGNSS, | |
| 298 sizeof(setGNSS) / sizeof(uint8_t)); | |
| 299 HAL_Delay(250); | |
| 300 } | |
| 301 | |
| 302 | |
| 303 | |
| 304 /*! | |
| 305 * Creates a checksum based on UBX standard. | |
| 306 * @param class Class value from UBX doc. | |
| 307 * @param messageID MessageID value from UBX doc. | |
| 308 * @param dataLength Data length value from UBX doc. | |
| 309 * @param payload Just payload. | |
| 310 * @return Returns checksum. | |
| 311 */ | |
| 312 uint8_t GNSS_Checksum(uint8_t class, uint8_t messageID, uint8_t dataLength,uint8_t *payload) { | |
| 313 //todo: Look at 32.4 UBX Checksum | |
| 314 return 0; | |
| 315 } |