Mercurial > public > ostc4
annotate Small_CPU/Src/GNSS.c @ 924:4d98fb2a178e Evo_2_23
Bugfix real scrubber time decreased in sim mode:
In the previous version the condition check for dive / simulation had been commented out causing the scrubber timer to be decreased during simulator usage. The problem has been fixed. In addition the scrubbertimer will now be maintained in simulator mode. In case the +5 minutes option is used the scrubber time is decreased by 5 minutes as well.
| author | Ideenmodellierer |
|---|---|
| date | Wed, 13 Nov 2024 17:55:05 +0100 |
| parents | c0553dd70608 |
| children |
| 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) { | |
| 919 | 174 |
| 175 static float searchCnt = 1.0; | |
| 176 | |
| 887 | 177 uShort.bytes[0] = GNSS_Handle.uartWorkingBuffer[10]; |
| 178 GNSS->yearBytes[0]=GNSS_Handle.uartWorkingBuffer[10]; | |
| 179 uShort.bytes[1] = GNSS_Handle.uartWorkingBuffer[11]; | |
| 180 GNSS->yearBytes[1]=GNSS_Handle.uartWorkingBuffer[11]; | |
| 181 GNSS->year = uShort.uShort; | |
| 182 GNSS->month = GNSS_Handle.uartWorkingBuffer[12]; | |
| 183 GNSS->day = GNSS_Handle.uartWorkingBuffer[13]; | |
| 184 GNSS->hour = GNSS_Handle.uartWorkingBuffer[14]; | |
| 185 GNSS->min = GNSS_Handle.uartWorkingBuffer[15]; | |
| 186 GNSS->sec = GNSS_Handle.uartWorkingBuffer[16]; | |
| 187 GNSS->fixType = GNSS_Handle.uartWorkingBuffer[26]; | |
| 188 | |
| 189 for (int var = 0; var < 4; ++var) { | |
| 190 iLong.bytes[var] = GNSS_Handle.uartWorkingBuffer[var + 30]; | |
| 191 GNSS->lonBytes[var]= GNSS_Handle.uartWorkingBuffer[var + 30]; | |
| 192 } | |
| 193 GNSS->lon = iLong.iLong; | |
| 194 GNSS->fLon=(float)iLong.iLong/10000000.0; | |
| 195 for (int var = 0; var < 4; ++var) { | |
| 196 iLong.bytes[var] = GNSS_Handle.uartWorkingBuffer[var + 34]; | |
| 197 GNSS->latBytes[var]=GNSS_Handle.uartWorkingBuffer[var + 34]; | |
| 198 } | |
| 199 GNSS->lat = iLong.iLong; | |
| 200 GNSS->fLat=(float)iLong.iLong/10000000.0; | |
| 201 for (int var = 0; var < 4; ++var) { | |
| 202 iLong.bytes[var] = GNSS_Handle.uartWorkingBuffer[var + 38]; | |
| 203 } | |
| 204 GNSS->height = iLong.iLong; | |
| 205 | |
| 206 for (int var = 0; var < 4; ++var) { | |
| 207 iLong.bytes[var] = GNSS_Handle.uartWorkingBuffer[var + 42]; | |
| 208 GNSS->hMSLBytes[var] = GNSS_Handle.uartWorkingBuffer[var + 42]; | |
| 209 } | |
| 210 GNSS->hMSL = iLong.iLong; | |
| 211 | |
| 212 for (int var = 0; var < 4; ++var) { | |
| 213 uLong.bytes[var] = GNSS_Handle.uartWorkingBuffer[var + 46]; | |
| 214 } | |
| 215 GNSS->hAcc = uLong.uLong; | |
| 216 | |
| 217 for (int var = 0; var < 4; ++var) { | |
| 218 uLong.bytes[var] = GNSS_Handle.uartWorkingBuffer[var + 50]; | |
| 219 } | |
| 220 GNSS->vAcc = uLong.uLong; | |
| 221 | |
| 222 for (int var = 0; var < 4; ++var) { | |
| 223 iLong.bytes[var] = GNSS_Handle.uartWorkingBuffer[var + 66]; | |
| 224 GNSS->gSpeedBytes[var] = GNSS_Handle.uartWorkingBuffer[var + 66]; | |
| 225 } | |
| 226 GNSS->gSpeed = iLong.iLong; | |
| 227 | |
| 228 for (int var = 0; var < 4; ++var) { | |
| 229 iLong.bytes[var] = GNSS_Handle.uartWorkingBuffer[var + 70]; | |
| 230 } | |
| 231 GNSS->headMot = iLong.iLong * 1e-5; // todo I'm not sure this good options. | |
| 919 | 232 |
| 233 if((GNSS->fLat == 0.0) && (GNSS->fLon == 0.0)) | |
| 234 { | |
| 235 GNSS->fLat = searchCnt++; | |
| 236 } | |
| 887 | 237 } |
| 238 | |
| 239 /*! | |
| 240 * Parse data to UTC time solution standard. | |
| 241 * Look at: 32.17.30.1 u-blox 8 Receiver description. | |
| 242 * @param GNSS Pointer to main GNSS structure. | |
| 243 */ | |
| 244 void GNSS_ParseNavigatorData(GNSS_StateHandle *GNSS) { | |
| 245 uShort.bytes[0] = GNSS_Handle.uartWorkingBuffer[18]; | |
| 246 uShort.bytes[1] = GNSS_Handle.uartWorkingBuffer[19]; | |
| 247 GNSS->year = uShort.uShort; | |
| 248 GNSS->month = GNSS_Handle.uartWorkingBuffer[20]; | |
| 249 GNSS->day = GNSS_Handle.uartWorkingBuffer[21]; | |
| 250 GNSS->hour = GNSS_Handle.uartWorkingBuffer[22]; | |
| 251 GNSS->min = GNSS_Handle.uartWorkingBuffer[23]; | |
| 252 GNSS->sec = GNSS_Handle.uartWorkingBuffer[24]; | |
| 253 } | |
| 254 | |
| 255 /*! | |
| 256 * Parse data to geodetic position solution standard. | |
| 257 * Look at: 32.17.14.1 u-blox 8 Receiver description. | |
| 258 * @param GNSS Pointer to main GNSS structure. | |
| 259 */ | |
| 260 void GNSS_ParsePOSLLHData(GNSS_StateHandle *GNSS) { | |
| 261 for (int var = 0; var < 4; ++var) { | |
| 262 iLong.bytes[var] = GNSS_Handle.uartWorkingBuffer[var + 10]; | |
| 263 } | |
| 264 GNSS->lon = iLong.iLong; | |
| 265 GNSS->fLon=(float)iLong.iLong/10000000.0; | |
| 266 | |
| 267 for (int var = 0; var < 4; ++var) { | |
| 268 iLong.bytes[var] = GNSS_Handle.uartWorkingBuffer[var + 14]; | |
| 269 } | |
| 270 GNSS->lat = iLong.iLong; | |
| 271 GNSS->fLat=(float)iLong.iLong/10000000.0; | |
| 272 | |
| 273 for (int var = 0; var < 4; ++var) { | |
| 274 iLong.bytes[var] = GNSS_Handle.uartWorkingBuffer[var + 18]; | |
| 275 } | |
| 276 GNSS->height = iLong.iLong; | |
| 277 | |
| 278 for (int var = 0; var < 4; ++var) { | |
| 279 iLong.bytes[var] = GNSS_Handle.uartWorkingBuffer[var + 22]; | |
| 280 } | |
| 281 GNSS->hMSL = iLong.iLong; | |
| 282 | |
| 283 for (int var = 0; var < 4; ++var) { | |
| 284 uLong.bytes[var] = GNSS_Handle.uartWorkingBuffer[var + 26]; | |
| 285 } | |
| 286 GNSS->hAcc = uLong.uLong; | |
| 287 | |
| 288 for (int var = 0; var < 4; ++var) { | |
| 289 uLong.bytes[var] = GNSS_Handle.uartWorkingBuffer[var + 30]; | |
| 290 } | |
| 291 GNSS->vAcc = uLong.uLong; | |
| 292 } | |
| 293 | |
| 294 /*! | |
| 295 * Sends the basic configuration: Activation of the UBX standard, change of NMEA version to 4.10 and turn on of the Galileo system. | |
| 296 * @param GNSS Pointer to main GNSS structure. | |
| 297 */ | |
| 298 void GNSS_LoadConfig(GNSS_StateHandle *GNSS) { | |
| 299 HAL_UART_Transmit_DMA(GNSS->huart, configUBX, | |
| 300 sizeof(configUBX) / sizeof(uint8_t)); | |
| 301 HAL_Delay(250); | |
| 302 HAL_UART_Transmit_DMA(GNSS->huart, setNMEA410, | |
| 303 sizeof(setNMEA410) / sizeof(uint8_t)); | |
| 304 HAL_Delay(250); | |
| 305 HAL_UART_Transmit_DMA(GNSS->huart, setGNSS, | |
| 306 sizeof(setGNSS) / sizeof(uint8_t)); | |
| 307 HAL_Delay(250); | |
| 308 } | |
| 309 | |
| 310 | |
| 311 | |
| 312 /*! | |
| 313 * Creates a checksum based on UBX standard. | |
| 314 * @param class Class value from UBX doc. | |
| 315 * @param messageID MessageID value from UBX doc. | |
| 316 * @param dataLength Data length value from UBX doc. | |
| 317 * @param payload Just payload. | |
| 318 * @return Returns checksum. | |
| 319 */ | |
| 320 uint8_t GNSS_Checksum(uint8_t class, uint8_t messageID, uint8_t dataLength,uint8_t *payload) { | |
| 321 //todo: Look at 32.4 UBX Checksum | |
| 322 return 0; | |
| 323 } |