src/main/rx/frsky_crc.c

   1 /*
   2  * This file is part of INAV.
   3  *
   4  * INAV 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  * INAV 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 INAV. If not, see <http://www.gnu.org/licenses/>.
  16  */
  17
  18 #include <stdbool.h>
  19 #include <stdint.h>
  20
  21 #include "platform.h"
  22 #include "rx/frsky_crc.h"
  23
  24 void frskyCheckSumStep(uint16_t *checksum, uint8_t byte)
  25 {
  26     *checksum += byte;
  27 }
  28
  29 void frskyCheckSumFini(uint16_t *checksum)
  30 {
  31     while (*checksum > 0xFF) {
  32         *checksum = (*checksum & 0xFF) + (*checksum >> 8);
  33     }
  34
  35     *checksum = 0xFF - *checksum;
  36 }
  37
  38 uint8_t frskyCheckSum(uint8_t *data, uint8_t length)
  39 {
  40     uint16_t checksum = 0;
  41     for (unsigned i = 0; i < length; i++) {
  42         frskyCheckSumStep(&checksum, *data++);
  43     }
  44     frskyCheckSumFini(&checksum);
  45     return checksum;
  46 }
  47
  48 bool frskyCheckSumIsGood(uint8_t *data, uint8_t length)
  49 {
  50     return !frskyCheckSum(data, length);
  51 }