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"
887
+ − 32
+ − 33 union u_Short uShort;
+ − 34 union i_Short iShort;
+ − 35 union u_Long uLong;
+ − 36 union i_Long iLong;
+ − 37
+ − 38 /*!
+ − 39 * Structure initialization.
+ − 40 * @param GNSS Pointer to main GNSS structure.
+ − 41 * @param huart Pointer to uart handle.
+ − 42 */
+ − 43 void GNSS_Init(GNSS_StateHandle *GNSS, UART_HandleTypeDef *huart) {
+ − 44 GNSS->huart = huart;
+ − 45 GNSS->year = 0;
+ − 46 GNSS->month = 0;
+ − 47 GNSS->day = 0;
+ − 48 GNSS->hour = 0;
+ − 49 GNSS->min = 0;
+ − 50 GNSS->sec = 0;
+ − 51 GNSS->fixType = 0;
+ − 52 GNSS->lon = 0;
+ − 53 GNSS->lat = 0;
+ − 54 GNSS->height = 0;
+ − 55 GNSS->hMSL = 0;
+ − 56 GNSS->hAcc = 0;
+ − 57 GNSS->vAcc = 0;
+ − 58 GNSS->gSpeed = 0;
+ − 59 GNSS->headMot = 0;
947
+ − 60 GNSS->alive = 0;
887
+ − 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) {
942
+ − 69 for (int var = 0; var < 5; 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
887
+ − 83 uShort.bytes[0] = GNSS_Handle.uartWorkingBuffer[10];
+ − 84 GNSS->yearBytes[0]=GNSS_Handle.uartWorkingBuffer[10];
+ − 85 uShort.bytes[1] = GNSS_Handle.uartWorkingBuffer[11];
+ − 86 GNSS->yearBytes[1]=GNSS_Handle.uartWorkingBuffer[11];
+ − 87 GNSS->year = uShort.uShort;
+ − 88 GNSS->month = GNSS_Handle.uartWorkingBuffer[12];
+ − 89 GNSS->day = GNSS_Handle.uartWorkingBuffer[13];
+ − 90 GNSS->hour = GNSS_Handle.uartWorkingBuffer[14];
+ − 91 GNSS->min = GNSS_Handle.uartWorkingBuffer[15];
+ − 92 GNSS->sec = GNSS_Handle.uartWorkingBuffer[16];
+ − 93 GNSS->fixType = GNSS_Handle.uartWorkingBuffer[26];
+ − 94
+ − 95 for (int var = 0; var < 4; ++var) {
+ − 96 iLong.bytes[var] = GNSS_Handle.uartWorkingBuffer[var + 30];
+ − 97 GNSS->lonBytes[var]= GNSS_Handle.uartWorkingBuffer[var + 30];
+ − 98 }
+ − 99 GNSS->lon = iLong.iLong;
+ − 100 GNSS->fLon=(float)iLong.iLong/10000000.0;
+ − 101 for (int var = 0; var < 4; ++var) {
+ − 102 iLong.bytes[var] = GNSS_Handle.uartWorkingBuffer[var + 34];
+ − 103 GNSS->latBytes[var]=GNSS_Handle.uartWorkingBuffer[var + 34];
+ − 104 }
+ − 105 GNSS->lat = iLong.iLong;
+ − 106 GNSS->fLat=(float)iLong.iLong/10000000.0;
+ − 107 for (int var = 0; var < 4; ++var) {
+ − 108 iLong.bytes[var] = GNSS_Handle.uartWorkingBuffer[var + 38];
+ − 109 }
+ − 110 GNSS->height = iLong.iLong;
+ − 111
+ − 112 for (int var = 0; var < 4; ++var) {
+ − 113 iLong.bytes[var] = GNSS_Handle.uartWorkingBuffer[var + 42];
+ − 114 GNSS->hMSLBytes[var] = GNSS_Handle.uartWorkingBuffer[var + 42];
+ − 115 }
+ − 116 GNSS->hMSL = iLong.iLong;
+ − 117
+ − 118 for (int var = 0; var < 4; ++var) {
+ − 119 uLong.bytes[var] = GNSS_Handle.uartWorkingBuffer[var + 46];
+ − 120 }
+ − 121 GNSS->hAcc = uLong.uLong;
+ − 122
+ − 123 for (int var = 0; var < 4; ++var) {
+ − 124 uLong.bytes[var] = GNSS_Handle.uartWorkingBuffer[var + 50];
+ − 125 }
+ − 126 GNSS->vAcc = uLong.uLong;
+ − 127
+ − 128 for (int var = 0; var < 4; ++var) {
+ − 129 iLong.bytes[var] = GNSS_Handle.uartWorkingBuffer[var + 66];
+ − 130 GNSS->gSpeedBytes[var] = GNSS_Handle.uartWorkingBuffer[var + 66];
+ − 131 }
+ − 132 GNSS->gSpeed = iLong.iLong;
+ − 133
+ − 134 for (int var = 0; var < 4; ++var) {
+ − 135 iLong.bytes[var] = GNSS_Handle.uartWorkingBuffer[var + 70];
+ − 136 }
+ − 137 GNSS->headMot = iLong.iLong * 1e-5; // todo I'm not sure this good options.
919
+ − 138
+ − 139 if((GNSS->fLat == 0.0) && (GNSS->fLon == 0.0))
+ − 140 {
+ − 141 GNSS->fLat = searchCnt++;
+ − 142 }
940
+ − 143
947
+ − 144 if(GNSS->alive & GNSS_ALIVE_STATE_ALIVE) /* alive */
+ − 145 {
+ − 146 GNSS->alive &= !GNSS_ALIVE_STATE_ALIVE;
+ − 147 }
+ − 148 else
+ − 149 {
+ − 150 GNSS->alive |= GNSS_ALIVE_STATE_ALIVE;
+ − 151 }
+ − 152 if((GNSS_Handle.uartWorkingBuffer[17] & 0x03) == 0x03) /* date/time valid */
+ − 153 {
+ − 154 GNSS->alive |= GNSS_ALIVE_STATE_TIME;
+ − 155 }
+ − 156 else
+ − 157 {
+ − 158 GNSS->alive &= !GNSS_ALIVE_STATE_TIME;
+ − 159 }
887
+ − 160 }
+ − 161
+ − 162 /*!
+ − 163 * Parse data to UTC time solution standard.
+ − 164 * Look at: 32.17.30.1 u-blox 8 Receiver description.
+ − 165 * @param GNSS Pointer to main GNSS structure.
+ − 166 */
931
+ − 167 void GNSS_ParseNavSatData(GNSS_StateHandle *GNSS) {
+ − 168
+ − 169 uint8_t loop = 0;
+ − 170 uint8_t searchIndex = 0;
+ − 171 uint8_t statIndex = 0; /* only 4 state information will be forwarded */
+ − 172 uint8_t signalQuality = 0;
+ − 173 GNSS->numSat = GNSS_Handle.uartWorkingBuffer[11];
+ − 174
+ − 175 memset(GNSS->statSat, 0, sizeof(GNSS->statSat));
+ − 176
+ − 177 if(GNSS->numSat > 0)
+ − 178 {
+ − 179 searchIndex = 0;
+ − 180 while((searchIndex < GNSS->numSat) && (statIndex < 4)) /* get good signal quality */
+ − 181 {
+ − 182 signalQuality = (GNSS_Handle.uartWorkingBuffer[22 + searchIndex * 12] & 0x7);
+ − 183 if(signalQuality > 4)
+ − 184 {
+ − 185 GNSS->statSat[statIndex++] = signalQuality;
+ − 186 }
+ − 187 if(statIndex == 4) break;
+ − 188 searchIndex++;
+ − 189 }
+ − 190 searchIndex = 0;
+ − 191 while((searchIndex < GNSS->numSat) && (statIndex < 4)) /* get medium signal quality */
+ − 192 {
+ − 193 signalQuality = (GNSS_Handle.uartWorkingBuffer[22 + searchIndex * 12] & 0x7);
+ − 194 if((signalQuality > 2) && (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 poor signal quality */
+ − 203 {
+ − 204 signalQuality = (GNSS_Handle.uartWorkingBuffer[22 + searchIndex * 12] & 0x7);
+ − 205 if(signalQuality <= 2)
+ − 206 {
+ − 207 GNSS->statSat[statIndex++] = signalQuality;
+ − 208 }
+ − 209 if(statIndex == 4) break;
+ − 210 searchIndex++;
+ − 211 }
+ − 212 loop++;
+ − 213 }
+ − 214 }
+ − 215
887
+ − 216 void GNSS_ParseNavigatorData(GNSS_StateHandle *GNSS) {
+ − 217 uShort.bytes[0] = GNSS_Handle.uartWorkingBuffer[18];
+ − 218 uShort.bytes[1] = GNSS_Handle.uartWorkingBuffer[19];
+ − 219 GNSS->year = uShort.uShort;
+ − 220 GNSS->month = GNSS_Handle.uartWorkingBuffer[20];
+ − 221 GNSS->day = GNSS_Handle.uartWorkingBuffer[21];
+ − 222 GNSS->hour = GNSS_Handle.uartWorkingBuffer[22];
+ − 223 GNSS->min = GNSS_Handle.uartWorkingBuffer[23];
+ − 224 GNSS->sec = GNSS_Handle.uartWorkingBuffer[24];
+ − 225 }
+ − 226
931
+ − 227
887
+ − 228 /*!
+ − 229 * Parse data to geodetic position solution standard.
+ − 230 * Look at: 32.17.14.1 u-blox 8 Receiver description.
+ − 231 * @param GNSS Pointer to main GNSS structure.
+ − 232 */
+ − 233 void GNSS_ParsePOSLLHData(GNSS_StateHandle *GNSS) {
+ − 234 for (int var = 0; var < 4; ++var) {
+ − 235 iLong.bytes[var] = GNSS_Handle.uartWorkingBuffer[var + 10];
+ − 236 }
+ − 237 GNSS->lon = iLong.iLong;
+ − 238 GNSS->fLon=(float)iLong.iLong/10000000.0;
+ − 239
+ − 240 for (int var = 0; var < 4; ++var) {
+ − 241 iLong.bytes[var] = GNSS_Handle.uartWorkingBuffer[var + 14];
+ − 242 }
+ − 243 GNSS->lat = iLong.iLong;
+ − 244 GNSS->fLat=(float)iLong.iLong/10000000.0;
+ − 245
+ − 246 for (int var = 0; var < 4; ++var) {
+ − 247 iLong.bytes[var] = GNSS_Handle.uartWorkingBuffer[var + 18];
+ − 248 }
+ − 249 GNSS->height = iLong.iLong;
+ − 250
+ − 251 for (int var = 0; var < 4; ++var) {
+ − 252 iLong.bytes[var] = GNSS_Handle.uartWorkingBuffer[var + 22];
+ − 253 }
+ − 254 GNSS->hMSL = iLong.iLong;
+ − 255
+ − 256 for (int var = 0; var < 4; ++var) {
+ − 257 uLong.bytes[var] = GNSS_Handle.uartWorkingBuffer[var + 26];
+ − 258 }
+ − 259 GNSS->hAcc = uLong.uLong;
+ − 260
+ − 261 for (int var = 0; var < 4; ++var) {
+ − 262 uLong.bytes[var] = GNSS_Handle.uartWorkingBuffer[var + 30];
+ − 263 }
+ − 264 GNSS->vAcc = uLong.uLong;
+ − 265 }