Mercurial > public > ostc4
annotate Small_CPU/Src/GNSS.c @ 957:3420e3ba698d Evo_2_23
External sensor commands: Add sensor ID to command:
In the previous version a command was send without information regarding the target sensor. To have the possibility in future to e.g. calibrate a specific sensor, the sensor ID is now transmitted together with the command. As example in the new implementation the O2 Sensor selected in the sensor menu will blink to enable sensor identification.
| author | Ideenmodellierer |
|---|---|
| date | Mon, 06 Jan 2025 20:06:35 +0100 |
| parents | 9b29995d6619 |
| 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 | |
| 931 | 29 #include <string.h> |
| 887 | 30 #include "GNSS.h" |
| 947 | 31 #include "data_exchange.h" |
| 955 | 32 #include "rtc.h" |
| 887 | 33 |
| 34 union u_Short uShort; | |
| 35 union i_Short iShort; | |
| 36 union u_Long uLong; | |
| 37 union i_Long iLong; | |
| 38 | |
| 39 /*! | |
| 40 * Structure initialization. | |
| 41 * @param GNSS Pointer to main GNSS structure. | |
| 42 * @param huart Pointer to uart handle. | |
| 43 */ | |
| 44 void GNSS_Init(GNSS_StateHandle *GNSS, UART_HandleTypeDef *huart) { | |
| 45 GNSS->huart = huart; | |
| 46 GNSS->year = 0; | |
| 47 GNSS->month = 0; | |
| 48 GNSS->day = 0; | |
| 49 GNSS->hour = 0; | |
| 50 GNSS->min = 0; | |
| 51 GNSS->sec = 0; | |
| 52 GNSS->fixType = 0; | |
| 53 GNSS->lon = 0; | |
| 54 GNSS->lat = 0; | |
| 55 GNSS->height = 0; | |
| 56 GNSS->hMSL = 0; | |
| 57 GNSS->hAcc = 0; | |
| 58 GNSS->vAcc = 0; | |
| 59 GNSS->gSpeed = 0; | |
| 60 GNSS->headMot = 0; | |
| 61 } | |
| 62 | |
| 63 /*! | |
| 64 * Parse data to unique chip ID standard. | |
| 65 * Look at: 32.19.1.1 u-blox 8 Receiver description | |
| 66 * @param GNSS Pointer to main GNSS structure. | |
| 67 */ | |
| 68 void GNSS_ParseUniqID(GNSS_StateHandle *GNSS) { | |
|
957
3420e3ba698d
External sensor commands: Add sensor ID to command:
Ideenmodellierer
parents:
955
diff
changeset
|
69 for (int var = 0; var < 4; var++) { |
| 887 | 70 GNSS->uniqueID[var] = GNSS_Handle.uartWorkingBuffer[10 + var]; |
| 71 } | |
| 72 } | |
| 73 | |
| 74 /*! | |
| 75 * Parse data to navigation position velocity time solution standard. | |
| 76 * Look at: 32.17.15.1 u-blox 8 Receiver description. | |
| 77 * @param GNSS Pointer to main GNSS structure. | |
| 78 */ | |
| 79 void GNSS_ParsePVTData(GNSS_StateHandle *GNSS) { | |
| 919 | 80 |
| 81 static float searchCnt = 1.0; | |
| 82 | |
| 955 | 83 RTC_TimeTypeDef sTimeNow; |
| 84 | |
| 887 | 85 uShort.bytes[0] = GNSS_Handle.uartWorkingBuffer[10]; |
| 86 GNSS->yearBytes[0]=GNSS_Handle.uartWorkingBuffer[10]; | |
| 87 uShort.bytes[1] = GNSS_Handle.uartWorkingBuffer[11]; | |
| 88 GNSS->yearBytes[1]=GNSS_Handle.uartWorkingBuffer[11]; | |
| 89 GNSS->year = uShort.uShort; | |
| 90 GNSS->month = GNSS_Handle.uartWorkingBuffer[12]; | |
| 91 GNSS->day = GNSS_Handle.uartWorkingBuffer[13]; | |
| 92 GNSS->hour = GNSS_Handle.uartWorkingBuffer[14]; | |
| 93 GNSS->min = GNSS_Handle.uartWorkingBuffer[15]; | |
| 94 GNSS->sec = GNSS_Handle.uartWorkingBuffer[16]; | |
| 95 GNSS->fixType = GNSS_Handle.uartWorkingBuffer[26]; | |
| 96 | |
| 97 for (int var = 0; var < 4; ++var) { | |
| 98 iLong.bytes[var] = GNSS_Handle.uartWorkingBuffer[var + 30]; | |
| 99 GNSS->lonBytes[var]= GNSS_Handle.uartWorkingBuffer[var + 30]; | |
| 100 } | |
| 101 GNSS->lon = iLong.iLong; | |
| 102 GNSS->fLon=(float)iLong.iLong/10000000.0; | |
| 103 for (int var = 0; var < 4; ++var) { | |
| 104 iLong.bytes[var] = GNSS_Handle.uartWorkingBuffer[var + 34]; | |
| 105 GNSS->latBytes[var]=GNSS_Handle.uartWorkingBuffer[var + 34]; | |
| 106 } | |
| 107 GNSS->lat = iLong.iLong; | |
| 108 GNSS->fLat=(float)iLong.iLong/10000000.0; | |
| 109 for (int var = 0; var < 4; ++var) { | |
| 110 iLong.bytes[var] = GNSS_Handle.uartWorkingBuffer[var + 38]; | |
| 111 } | |
| 112 GNSS->height = iLong.iLong; | |
| 113 | |
| 114 for (int var = 0; var < 4; ++var) { | |
| 115 iLong.bytes[var] = GNSS_Handle.uartWorkingBuffer[var + 42]; | |
| 116 GNSS->hMSLBytes[var] = GNSS_Handle.uartWorkingBuffer[var + 42]; | |
| 117 } | |
| 118 GNSS->hMSL = iLong.iLong; | |
| 119 | |
| 120 for (int var = 0; var < 4; ++var) { | |
| 121 uLong.bytes[var] = GNSS_Handle.uartWorkingBuffer[var + 46]; | |
| 122 } | |
| 123 GNSS->hAcc = uLong.uLong; | |
| 124 | |
| 125 for (int var = 0; var < 4; ++var) { | |
| 126 uLong.bytes[var] = GNSS_Handle.uartWorkingBuffer[var + 50]; | |
| 127 } | |
| 128 GNSS->vAcc = uLong.uLong; | |
| 129 | |
| 130 for (int var = 0; var < 4; ++var) { | |
| 131 iLong.bytes[var] = GNSS_Handle.uartWorkingBuffer[var + 66]; | |
| 132 GNSS->gSpeedBytes[var] = GNSS_Handle.uartWorkingBuffer[var + 66]; | |
| 133 } | |
| 134 GNSS->gSpeed = iLong.iLong; | |
| 135 | |
| 136 for (int var = 0; var < 4; ++var) { | |
| 137 iLong.bytes[var] = GNSS_Handle.uartWorkingBuffer[var + 70]; | |
| 138 } | |
| 139 GNSS->headMot = iLong.iLong * 1e-5; // todo I'm not sure this good options. | |
| 919 | 140 |
| 141 if((GNSS->fLat == 0.0) && (GNSS->fLon == 0.0)) | |
| 142 { | |
| 143 GNSS->fLat = searchCnt++; | |
| 144 } | |
| 940 | 145 |
| 947 | 146 if(GNSS->alive & GNSS_ALIVE_STATE_ALIVE) /* alive */ |
| 147 { | |
| 955 | 148 GNSS->alive &= ~GNSS_ALIVE_STATE_ALIVE; |
| 947 | 149 } |
| 150 else | |
| 151 { | |
| 152 GNSS->alive |= GNSS_ALIVE_STATE_ALIVE; | |
| 153 } | |
| 154 if((GNSS_Handle.uartWorkingBuffer[17] & 0x03) == 0x03) /* date/time valid */ | |
| 155 { | |
| 156 GNSS->alive |= GNSS_ALIVE_STATE_TIME; | |
| 157 } | |
| 158 else | |
| 159 { | |
| 955 | 160 GNSS->alive &= ~GNSS_ALIVE_STATE_TIME; |
| 161 } | |
| 162 | |
| 163 if(GNSS->fixType >= 2) | |
| 164 { | |
| 165 RTC_GetTime(&sTimeNow); | |
| 166 GNSS->alive |= GNSS_ALIVE_BACKUP_POS; | |
| 167 GNSS->last_fLat = GNSS->fLat; | |
| 168 GNSS->last_fLon = GNSS->fLon; | |
| 169 GNSS->last_hour = sTimeNow.Hours; | |
| 947 | 170 } |
| 887 | 171 } |
| 172 | |
| 173 /*! | |
| 174 * Parse data to UTC time solution standard. | |
| 175 * Look at: 32.17.30.1 u-blox 8 Receiver description. | |
| 176 * @param GNSS Pointer to main GNSS structure. | |
| 177 */ | |
| 931 | 178 void GNSS_ParseNavSatData(GNSS_StateHandle *GNSS) { |
| 179 | |
| 180 uint8_t loop = 0; | |
| 181 uint8_t searchIndex = 0; | |
| 182 uint8_t statIndex = 0; /* only 4 state information will be forwarded */ | |
| 183 uint8_t signalQuality = 0; | |
| 184 GNSS->numSat = GNSS_Handle.uartWorkingBuffer[11]; | |
| 185 | |
| 186 memset(GNSS->statSat, 0, sizeof(GNSS->statSat)); | |
| 187 | |
| 188 if(GNSS->numSat > 0) | |
| 189 { | |
| 190 searchIndex = 0; | |
| 191 while((searchIndex < GNSS->numSat) && (statIndex < 4)) /* get good signal quality */ | |
| 192 { | |
| 193 signalQuality = (GNSS_Handle.uartWorkingBuffer[22 + searchIndex * 12] & 0x7); | |
| 194 if(signalQuality > 4) | |
| 195 { | |
| 196 GNSS->statSat[statIndex++] = signalQuality; | |
| 197 } | |
| 198 if(statIndex == 4) break; | |
| 199 searchIndex++; | |
| 200 } | |
| 201 searchIndex = 0; | |
| 202 while((searchIndex < GNSS->numSat) && (statIndex < 4)) /* get medium signal quality */ | |
| 203 { | |
| 204 signalQuality = (GNSS_Handle.uartWorkingBuffer[22 + searchIndex * 12] & 0x7); | |
| 205 if((signalQuality > 2) && (signalQuality <= 4)) | |
| 206 { | |
| 207 GNSS->statSat[statIndex++] = signalQuality; | |
| 208 } | |
| 209 if(statIndex == 4) break; | |
| 210 searchIndex++; | |
| 211 } | |
| 212 searchIndex = 0; | |
| 213 while((searchIndex < GNSS->numSat) && (statIndex < 4)) /* get poor signal quality */ | |
| 214 { | |
| 215 signalQuality = (GNSS_Handle.uartWorkingBuffer[22 + searchIndex * 12] & 0x7); | |
| 216 if(signalQuality <= 2) | |
| 217 { | |
| 218 GNSS->statSat[statIndex++] = signalQuality; | |
| 219 } | |
| 220 if(statIndex == 4) break; | |
| 221 searchIndex++; | |
| 222 } | |
| 223 loop++; | |
| 224 } | |
| 225 } | |
| 226 | |
| 887 | 227 void GNSS_ParseNavigatorData(GNSS_StateHandle *GNSS) { |
| 228 uShort.bytes[0] = GNSS_Handle.uartWorkingBuffer[18]; | |
| 229 uShort.bytes[1] = GNSS_Handle.uartWorkingBuffer[19]; | |
| 230 GNSS->year = uShort.uShort; | |
| 231 GNSS->month = GNSS_Handle.uartWorkingBuffer[20]; | |
| 232 GNSS->day = GNSS_Handle.uartWorkingBuffer[21]; | |
| 233 GNSS->hour = GNSS_Handle.uartWorkingBuffer[22]; | |
| 234 GNSS->min = GNSS_Handle.uartWorkingBuffer[23]; | |
| 235 GNSS->sec = GNSS_Handle.uartWorkingBuffer[24]; | |
| 236 } | |
| 237 | |
| 931 | 238 |
| 887 | 239 /*! |
| 240 * Parse data to geodetic position solution standard. | |
| 241 * Look at: 32.17.14.1 u-blox 8 Receiver description. | |
| 242 * @param GNSS Pointer to main GNSS structure. | |
| 243 */ | |
| 244 void GNSS_ParsePOSLLHData(GNSS_StateHandle *GNSS) { | |
| 245 for (int var = 0; var < 4; ++var) { | |
| 246 iLong.bytes[var] = GNSS_Handle.uartWorkingBuffer[var + 10]; | |
| 247 } | |
| 248 GNSS->lon = iLong.iLong; | |
| 249 GNSS->fLon=(float)iLong.iLong/10000000.0; | |
| 250 | |
| 251 for (int var = 0; var < 4; ++var) { | |
| 252 iLong.bytes[var] = GNSS_Handle.uartWorkingBuffer[var + 14]; | |
| 253 } | |
| 254 GNSS->lat = iLong.iLong; | |
| 255 GNSS->fLat=(float)iLong.iLong/10000000.0; | |
| 256 | |
| 257 for (int var = 0; var < 4; ++var) { | |
| 258 iLong.bytes[var] = GNSS_Handle.uartWorkingBuffer[var + 18]; | |
| 259 } | |
| 260 GNSS->height = iLong.iLong; | |
| 261 | |
| 262 for (int var = 0; var < 4; ++var) { | |
| 263 iLong.bytes[var] = GNSS_Handle.uartWorkingBuffer[var + 22]; | |
| 264 } | |
| 265 GNSS->hMSL = iLong.iLong; | |
| 266 | |
| 267 for (int var = 0; var < 4; ++var) { | |
| 268 uLong.bytes[var] = GNSS_Handle.uartWorkingBuffer[var + 26]; | |
| 269 } | |
| 270 GNSS->hAcc = uLong.uLong; | |
| 271 | |
| 272 for (int var = 0; var < 4; ++var) { | |
| 273 uLong.bytes[var] = GNSS_Handle.uartWorkingBuffer[var + 30]; | |
| 274 } | |
| 275 GNSS->vAcc = uLong.uLong; | |
| 276 } |