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
  21 #include "gps_ublox_utils.h"
  22
  23 void ublox_update_checksum(uint8_t *data, uint8_t len, uint8_t *ck_a, uint8_t *ck_b)
  24 {
  25     *ck_a = *ck_b = 0;
  26     while (len--) {
  27         *ck_a += *data;
  28         *ck_b += *ck_a;
  29         data++;
  30     }
  31 }
  32
  33 int ubloxCfgFillBytes(ubx_config_data8_t *cfg, ubx_config_data8_payload_t *kvPairs, uint8_t count)
  34 {
  35     if (count > MAX_CONFIG_SET_VAL_VALUES)
  36         count = MAX_CONFIG_SET_VAL_VALUES;
  37
  38     cfg->header.preamble1 = 0xb5;
  39     cfg->header.preamble2 = 0x62;
  40     cfg->header.msg_class = 0x06;
  41     cfg->header.msg_id = 0x8A;
  42     cfg->header.length = sizeof(ubx_config_data_header_v1_t) + ((sizeof(ubx_config_data8_payload_t) * count));
  43     cfg->configHeader.layers = 0x1;
  44     cfg->configHeader.transaction = 0;
  45     cfg->configHeader.reserved = 0;
  46     cfg->configHeader.version = 1;
  47
  48
  49     for (int i = 0; i < count; ++i) {
  50         cfg->data.payload[i].key = kvPairs[i].key; //htonl(kvPairs[i].key);
  51         cfg->data.payload[i].value = kvPairs[i].value;
  52     }
  53
  54     uint8_t *buf = (uint8_t *)cfg;
  55     uint8_t ck_a, ck_b;
  56     ublox_update_checksum(buf + 2, cfg->header.length + 4, &ck_a, &ck_b);
  57     buf[cfg->header.length + 6] = ck_a;
  58     buf[cfg->header.length + 7] = ck_b;
  59
  60     return count;
  61 }
  62