src/main/io/gps_ublox_utils.c

   1 /*
   2  * This file is part of INAV
   3  *
   4  * Cleanflight is free software: you can redistribute it and/or modify
   5  * it under the terms of the GNU General Public License as published by
   6  * the Free Software Foundation, either version 3 of the License, or
   7  * (at your option) any later version.
   8  *
   9  * Cleanflight is distributed in the hope that it will be useful,
  10  * but WITHOUT ANY WARRANTY; without even the implied warranty of
  11  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
  12  * GNU General Public License for more details.
  13  *
  14  * You should have received a copy of the GNU General Public License
  15  * along with Cleanflight.  If not, see <http://www.gnu.org/licenses/>.
  16  */
  17
  18
  19 #include <stdint.h>
  20 #include <string.h>
  21
  22 #include "gps_ublox_utils.h"
  23
  24 void ublox_update_checksum(uint8_t *data, uint8_t len, uint8_t *ck_a, uint8_t *ck_b)
  25 {
  26     *ck_a = *ck_b = 0;
  27     while (len--) {
  28         *ck_a += *data;
  29         *ck_b += *ck_a;
  30         data++;
  31     }
  32 }
  33
  34 int ubloxCfgFillBytes(ubx_config_data8_t *cfg, ubx_config_data8_payload_t *kvPairs, uint8_t count)
  35 {
  36     if (count > MAX_CONFIG_SET_VAL_VALUES)
  37         count = MAX_CONFIG_SET_VAL_VALUES;
  38
  39     cfg->header.preamble1 = 0xb5;
  40     cfg->header.preamble2 = 0x62;
  41     cfg->header.msg_class = 0x06;
  42     cfg->header.msg_id = 0x8A;
  43     cfg->header.length = sizeof(ubx_config_data_header_v1_t) + ((sizeof(ubx_config_data8_payload_t) * count));
  44     cfg->configHeader.layers = 0x1;
  45     cfg->configHeader.transaction = 0;
  46     cfg->configHeader.reserved = 0;
  47     cfg->configHeader.version = 1;
  48
  49
  50     for (int i = 0; i < count; ++i) {
  51         cfg->data.payload[i].key = kvPairs[i].key;
  52         cfg->data.payload[i].value = kvPairs[i].value;
  53     }
  54
  55     uint8_t *buf = (uint8_t *)cfg;
  56     uint8_t ck_a, ck_b;
  57     ublox_update_checksum(buf + 2, cfg->header.length + 4, &ck_a, &ck_b);
  58     buf[cfg->header.length + 6] = ck_a;
  59     buf[cfg->header.length + 7] = ck_b;
  60
  61     return count;
  62 }
  63
  64 int ubloxCfgFillU2(ubx_config_data16_t *cfg, ubx_config_data16_payload_t *kvPairs, uint8_t count)
  65 {
  66     if (count > MAX_CONFIG_SET_VAL_VALUES)
  67         count = MAX_CONFIG_SET_VAL_VALUES;
  68
  69     cfg->header.preamble1 = 0xb5;
  70     cfg->header.preamble2 = 0x62;
  71     cfg->header.msg_class = 0x06;
  72     cfg->header.msg_id = 0x8A;
  73     cfg->header.length = sizeof(ubx_config_data_header_v1_t) + ((sizeof(ubx_config_data16_payload_t) * count));
  74     cfg->configHeader.layers = 0x1;
  75     cfg->configHeader.transaction = 0;
  76     cfg->configHeader.reserved = 0;
  77     cfg->configHeader.version = 1;
  78
  79     for (int i = 0; i < count; ++i) {
  80         cfg->data.payload[i].key = kvPairs[i].key;
  81         cfg->data.payload[i].value = kvPairs[i].value;
  82     }
  83
  84     uint8_t *buf = (uint8_t *)cfg;
  85     uint8_t ck_a, ck_b;
  86     ublox_update_checksum(buf + 2, cfg->header.length + 4, &ck_a, &ck_b);
  87     buf[cfg->header.length + 6] = ck_a;
  88     buf[cfg->header.length + 7] = ck_b;
  89
  90     return count;
  91 }
  92
  93 void ubloxNavSat2NavSig(const ubx_nav_svinfo_channel *navSat, ubx_nav_sig_info *navSig)
  94 {
  95     memset(navSig, 0, sizeof(ubx_nav_sig_info));
  96     navSig->svId = navSat->svId;
  97     navSig->gnssId = navSat->gnssId;
  98     navSig->cno = navSat->cno;
  99     navSig->prRes = navSat->prRes;
 100     navSig->quality = navSat->flags & (BIT(0)|BIT(1)|BIT(2));
 101     navSig->sigFlags = (navSat->flags >> 4) & (BIT(0)|BIT(1));  // Healthy, not healthy
 102     // non-converted items:
 103     //uint8_t sigId;    // signal ID
 104     //uint8_t freqId;   // 0-13 slot +, 0-13, glonass only
 105     //uint8_t corrSource; // Correction source: 0 = no correction, 1 = SBAS, 2 = BeiDou, 3 = RTCM2, 4 = RTCM3 OSR, 5 = RTCM3 SSR, 6 = QZSS SLAS, 7 = SPARTN
 106     //uint8_t ionoModel;  // 0 = no mode, 1 = Klobuchar GPS, 2 = SBAS, 3 = Klobuchar BeiDou, 8 = Iono derived from dual frequency observations
 107     //uint16_t sigFlags;
 108                         // bit2: pseudorange smoothed,
 109                         // bit3: pseudorange used,
 110                         // bit4: carrioer range used;
 111                         // bit5: doppler used
 112                         // bit6: pseudorange corrections used
 113                         // bit7: carrier correction used
 114                         // bit8: doper corrections used
 115     //uint8_t reserved[4];
 116 }