Mirror/betaflight
1
0
Fork 0
mirror of https://github.com/betaflight/betaflight.git synced 2025-07-17 21:35:44 +03:00
Code Issues Wiki Activity

Fixed indentation in ibus_shared.c.

Browse source
This commit is contained in:
mikeller 2018-01-21 09:27:19 +13:00
parent db0f1c4f10
commit d4f8cc7dfa
1 changed files with 481 additions and 481 deletions
Download patch file Download diff file Expand all files Collapse all files

282
src/main/telemetry/ibus_shared.c
View file

@ -23,53 +23,53 @@
*/ */
#include <stdbool.h> #include <stdbool.h>
#include <stdint.h> #include <stdint.h>
#include <stdlib.h> #include <stdlib.h>
// #include <string.h> // #include <string.h>
#include "platform.h" #include "platform.h"
//#include "common/utils.h" //#include "common/utils.h"
#include "telemetry/telemetry.h" #include "telemetry/telemetry.h"
#include "telemetry/ibus_shared.h" #include "telemetry/ibus_shared.h"
static uint16_t calculateChecksum(const uint8_t *ibusPacket); static uint16_t calculateChecksum(const uint8_t *ibusPacket);
#if defined(USE_TELEMETRY) && defined(USE_TELEMETRY_IBUS) #if defined(USE_TELEMETRY) && defined(USE_TELEMETRY_IBUS)
#include "config/feature.h" #include "config/feature.h"
#include "pg/pg.h" #include "pg/pg.h"
#include "pg/pg_ids.h" #include "pg/pg_ids.h"
#include "sensors/battery.h" #include "sensors/battery.h"
#include "fc/rc_controls.h" #include "fc/rc_controls.h"
#include "fc/config.h" #include "fc/config.h"
#include "sensors/gyro.h" #include "sensors/gyro.h"
#include "drivers/accgyro/accgyro.h" #include "drivers/accgyro/accgyro.h"
#include "fc/runtime_config.h" #include "fc/runtime_config.h"
#include "sensors/acceleration.h" #include "sensors/acceleration.h"
#include "sensors/sensors.h" #include "sensors/sensors.h"
#include "sensors/barometer.h" #include "sensors/barometer.h"
#include "flight/imu.h" #include "flight/imu.h"
#include "flight/altitude.h" #include "flight/altitude.h"
#include "flight/navigation.h" #include "flight/navigation.h"
#include "io/gps.h" #include "io/gps.h"
#define IBUS_TEMPERATURE_OFFSET 400 #define IBUS_TEMPERATURE_OFFSET 400
#define INVALID_IBUS_ADDRESS 0 #define INVALID_IBUS_ADDRESS 0
#define IBUS_BUFFSIZE 33 // biggest iBus message seen so far + 1 #define IBUS_BUFFSIZE 33 // biggest iBus message seen so far + 1
#define IBUS_HEADER_FOOTER_SIZE 4 #define IBUS_HEADER_FOOTER_SIZE 4
#define IBUS_2BYTE_SESNSOR 2 #define IBUS_2BYTE_SESNSOR 2
#define IBUS_4BYTE_SESNSOR 4 #define IBUS_4BYTE_SESNSOR 4
typedef uint8_t ibusAddress_t; typedef uint8_t ibusAddress_t;
typedef enum { typedef enum {
IBUS_COMMAND_DISCOVER_SENSOR = 0x80, IBUS_COMMAND_DISCOVER_SENSOR = 0x80,
IBUS_COMMAND_SENSOR_TYPE = 0x90, IBUS_COMMAND_SENSOR_TYPE = 0x90,
IBUS_COMMAND_MEASUREMENT = 0xA0 IBUS_COMMAND_MEASUREMENT = 0xA0
} ibusCommand_e; } ibusCommand_e;
typedef enum { typedef enum {
IBUS_SENSOR_TYPE_NONE = 0x00, IBUS_SENSOR_TYPE_NONE = 0x00,
IBUS_SENSOR_TYPE_TEMPERATURE = 0x01, IBUS_SENSOR_TYPE_TEMPERATURE = 0x01,
IBUS_SENSOR_TYPE_RPM_FLYSKY = 0x02, IBUS_SENSOR_TYPE_RPM_FLYSKY = 0x02,
@ -105,25 +105,25 @@
IBUS_SENSOR_TYPE_ALT_MAX = 0x84, //4bytes signed MaxAlt m*100 IBUS_SENSOR_TYPE_ALT_MAX = 0x84, //4bytes signed MaxAlt m*100
IBUS_SENSOR_TYPE_ALT_FLYSKY = 0xf9, // Altitude 2 bytes signed in m IBUS_SENSOR_TYPE_ALT_FLYSKY = 0xf9, // Altitude 2 bytes signed in m
#if defined(USE_TELEMETRY_IBUS_EXTENDED) #if defined(USE_TELEMETRY_IBUS_EXTENDED)
IBUS_SENSOR_TYPE_GPS_FULL = 0xfd, IBUS_SENSOR_TYPE_GPS_FULL = 0xfd,
IBUS_SENSOR_TYPE_VOLT_FULL = 0xf0, IBUS_SENSOR_TYPE_VOLT_FULL = 0xf0,
IBUS_SENSOR_TYPE_ACC_FULL = 0xef, IBUS_SENSOR_TYPE_ACC_FULL = 0xef,
#endif //defined(TELEMETRY_IBUS_EXTENDED) #endif //defined(TELEMETRY_IBUS_EXTENDED)
IBUS_SENSOR_TYPE_UNKNOWN = 0xff IBUS_SENSOR_TYPE_UNKNOWN = 0xff
} ibusSensorType_e; } ibusSensorType_e;
typedef union ibusTelemetry { typedef union ibusTelemetry {
uint16_t uint16; uint16_t uint16;
uint32_t uint32; uint32_t uint32;
int16_t int16; int16_t int16;
int32_t int32; int32_t int32;
uint8_t byte[4]; uint8_t byte[4];
} ibusTelemetry_s; } ibusTelemetry_s;
#if defined(USE_GPS) #if defined(USE_GPS)
const uint8_t GPS_IDS[] = { const uint8_t GPS_IDS[] = {
IBUS_SENSOR_TYPE_GPS_STATUS, IBUS_SENSOR_TYPE_GPS_STATUS,
IBUS_SENSOR_TYPE_SPE, IBUS_SENSOR_TYPE_SPE,
IBUS_SENSOR_TYPE_GPS_LAT, IBUS_SENSOR_TYPE_GPS_LAT,
@ -134,57 +134,57 @@
IBUS_SENSOR_TYPE_ODO2, IBUS_SENSOR_TYPE_ODO2,
IBUS_SENSOR_TYPE_GPS_DIST, IBUS_SENSOR_TYPE_GPS_DIST,
IBUS_SENSOR_TYPE_COG, IBUS_SENSOR_TYPE_COG,
}; };
#endif #endif
#if defined(USE_TELEMETRY_IBUS_EXTENDED) #if defined(USE_TELEMETRY_IBUS_EXTENDED)
const uint8_t FULL_GPS_IDS[] = { const uint8_t FULL_GPS_IDS[] = {
IBUS_SENSOR_TYPE_GPS_STATUS, IBUS_SENSOR_TYPE_GPS_STATUS,
IBUS_SENSOR_TYPE_GPS_LAT, IBUS_SENSOR_TYPE_GPS_LAT,
IBUS_SENSOR_TYPE_GPS_LON, IBUS_SENSOR_TYPE_GPS_LON,
IBUS_SENSOR_TYPE_GPS_ALT, IBUS_SENSOR_TYPE_GPS_ALT,
}; };
const uint8_t FULL_VOLT_IDS[] = { const uint8_t FULL_VOLT_IDS[] = {
IBUS_SENSOR_TYPE_EXTERNAL_VOLTAGE, IBUS_SENSOR_TYPE_EXTERNAL_VOLTAGE,
IBUS_SENSOR_TYPE_CELL, IBUS_SENSOR_TYPE_CELL,
IBUS_SENSOR_TYPE_BAT_CURR, IBUS_SENSOR_TYPE_BAT_CURR,
IBUS_SENSOR_TYPE_FUEL, IBUS_SENSOR_TYPE_FUEL,
IBUS_SENSOR_TYPE_RPM, IBUS_SENSOR_TYPE_RPM,
}; };
const uint8_t FULL_ACC_IDS[] = { const uint8_t FULL_ACC_IDS[] = {
IBUS_SENSOR_TYPE_ACC_X, IBUS_SENSOR_TYPE_ACC_X,
IBUS_SENSOR_TYPE_ACC_Y, IBUS_SENSOR_TYPE_ACC_Y,
IBUS_SENSOR_TYPE_ACC_Z, IBUS_SENSOR_TYPE_ACC_Z,
IBUS_SENSOR_TYPE_ROLL, IBUS_SENSOR_TYPE_ROLL,
IBUS_SENSOR_TYPE_PITCH, IBUS_SENSOR_TYPE_PITCH,
IBUS_SENSOR_TYPE_YAW, IBUS_SENSOR_TYPE_YAW,
}; };
#endif //defined(USE_TELEMETRY_IBUS_EXTENDED) #endif //defined(USE_TELEMETRY_IBUS_EXTENDED)
static serialPort_t *ibusSerialPort = NULL; static serialPort_t *ibusSerialPort = NULL;
static ibusAddress_t ibusBaseAddress = INVALID_IBUS_ADDRESS; static ibusAddress_t ibusBaseAddress = INVALID_IBUS_ADDRESS;
static uint8_t sendBuffer[IBUS_BUFFSIZE]; static uint8_t sendBuffer[IBUS_BUFFSIZE];
static void setValue(uint8_t* bufferPtr, uint8_t sensorType, uint8_t length); static void setValue(uint8_t* bufferPtr, uint8_t sensorType, uint8_t length);
static uint8_t getSensorID(ibusAddress_t address) static uint8_t getSensorID(ibusAddress_t address)
{ {
//all checks are done in theAddressIsWithinOurRange //all checks are done in theAddressIsWithinOurRange
uint32_t index = address - ibusBaseAddress; uint32_t index = address - ibusBaseAddress;
return telemetryConfig()->flysky_sensors[index]; return telemetryConfig()->flysky_sensors[index];
} }
static uint8_t getSensorLength(uint8_t sensorID) static uint8_t getSensorLength(uint8_t sensorID)
{ {
if (sensorID == IBUS_SENSOR_TYPE_PRES || (sensorID >= IBUS_SENSOR_TYPE_GPS_LAT && sensorID <= IBUS_SENSOR_TYPE_ALT_MAX)) { if (sensorID == IBUS_SENSOR_TYPE_PRES || (sensorID >= IBUS_SENSOR_TYPE_GPS_LAT && sensorID <= IBUS_SENSOR_TYPE_ALT_MAX)) {
return IBUS_4BYTE_SESNSOR; return IBUS_4BYTE_SESNSOR;
} }
#if defined(USE_TELEMETRY_IBUS_EXTENDED) #if defined(USE_TELEMETRY_IBUS_EXTENDED)
if (sensorID == IBUS_SENSOR_TYPE_GPS_FULL) { if (sensorID == IBUS_SENSOR_TYPE_GPS_FULL) {
return 14; return 14;
} }
@ -194,12 +194,12 @@
if (sensorID == IBUS_SENSOR_TYPE_VOLT_FULL) { if (sensorID == IBUS_SENSOR_TYPE_VOLT_FULL) {
return 12; return 12;
} }
#endif #endif
return IBUS_2BYTE_SESNSOR; return IBUS_2BYTE_SESNSOR;
} }
static uint8_t transmitIbusPacket() static uint8_t transmitIbusPacket()
{ {
unsigned frameLength = sendBuffer[0]; unsigned frameLength = sendBuffer[0];
if (frameLength == INVALID_IBUS_ADDRESS) { if (frameLength == INVALID_IBUS_ADDRESS) {
return 0; return 0;
@ -212,47 +212,47 @@
serialWrite(ibusSerialPort, checksum & 0xFF); serialWrite(ibusSerialPort, checksum & 0xFF);
serialWrite(ibusSerialPort, checksum >> 8); serialWrite(ibusSerialPort, checksum >> 8);
return frameLength; return frameLength;
} }
static void setIbusDiscoverSensorReply(ibusAddress_t address) static void setIbusDiscoverSensorReply(ibusAddress_t address)
{ {
sendBuffer[0] = IBUS_HEADER_FOOTER_SIZE; sendBuffer[0] = IBUS_HEADER_FOOTER_SIZE;
sendBuffer[1] = IBUS_COMMAND_DISCOVER_SENSOR | address; sendBuffer[1] = IBUS_COMMAND_DISCOVER_SENSOR | address;
} }
static void setIbusSensorType(ibusAddress_t address) static void setIbusSensorType(ibusAddress_t address)
{ {
uint8_t sensorID = getSensorID(address); uint8_t sensorID = getSensorID(address);
uint8_t sensorLength = getSensorLength(sensorID); uint8_t sensorLength = getSensorLength(sensorID);
sendBuffer[0] = IBUS_HEADER_FOOTER_SIZE + 2; sendBuffer[0] = IBUS_HEADER_FOOTER_SIZE + 2;
sendBuffer[1] = IBUS_COMMAND_SENSOR_TYPE | address; sendBuffer[1] = IBUS_COMMAND_SENSOR_TYPE | address;
sendBuffer[2] = sensorID; sendBuffer[2] = sensorID;
sendBuffer[3] = sensorLength; sendBuffer[3] = sensorLength;
} }
static uint16_t getVoltage() static uint16_t getVoltage()
{ {
uint16_t voltage = getBatteryVoltage() *10; uint16_t voltage = getBatteryVoltage() *10;
if (telemetryConfig()->report_cell_voltage) { if (telemetryConfig()->report_cell_voltage) {
voltage /= getBatteryCellCount(); voltage /= getBatteryCellCount();
} }
return voltage; return voltage;
} }
static uint16_t getTemperature() static uint16_t getTemperature()
{ {
uint16_t temperature = gyroGetTemperature() * 10; uint16_t temperature = gyroGetTemperature() * 10;
#if defined(USE_BARO) #if defined(USE_BARO)
if (sensors(SENSOR_BARO)) { if (sensors(SENSOR_BARO)) {
temperature = (uint16_t) ((baro.baroTemperature + 50) / 10); temperature = (uint16_t) ((baro.baroTemperature + 50) / 10);
} }
#endif #endif
return temperature + IBUS_TEMPERATURE_OFFSET; return temperature + IBUS_TEMPERATURE_OFFSET;
} }
static uint16_t getFuel() static uint16_t getFuel()
{ {
uint16_t fuel = 0; uint16_t fuel = 0;
if (batteryConfig()->batteryCapacity > 0) { if (batteryConfig()->batteryCapacity > 0) {
fuel = (uint16_t)calculateBatteryPercentageRemaining(); fuel = (uint16_t)calculateBatteryPercentageRemaining();
@ -260,10 +260,10 @@
fuel = (uint16_t)constrain(getMAhDrawn(), 0, 0xFFFF); fuel = (uint16_t)constrain(getMAhDrawn(), 0, 0xFFFF);
} }
return fuel; return fuel;
} }
static uint16_t getRPM() static uint16_t getRPM()
{ {
uint16_t rpm = 0; uint16_t rpm = 0;
if (ARMING_FLAG(ARMED)) { if (ARMING_FLAG(ARMED)) {
const throttleStatus_e throttleStatus = calculateThrottleStatus(); const throttleStatus_e throttleStatus = calculateThrottleStatus();
@ -273,10 +273,10 @@
rpm = (uint16_t)(batteryConfig()->batteryCapacity); // / BLADE_NUMBER_DIVIDER rpm = (uint16_t)(batteryConfig()->batteryCapacity); // / BLADE_NUMBER_DIVIDER
} }
return rpm; return rpm;
} }
static uint16_t getMode() static uint16_t getMode()
{ {
uint16_t flightMode = 1; //Acro uint16_t flightMode = 1; //Acro
if (FLIGHT_MODE(ANGLE_MODE)) { if (FLIGHT_MODE(ANGLE_MODE)) {
flightMode = 0; //Stab flightMode = 0; //Stab
@ -306,16 +306,16 @@
flightMode = 9; //Land flightMode = 9; //Land
} }
return flightMode; return flightMode;
} }
static int16_t getACC(uint8_t index) static int16_t getACC(uint8_t index)
{ {
return (int16_t)(((float)acc.accADC[index] / acc.dev.acc_1G) * 1000); return (int16_t)(((float)acc.accADC[index] / acc.dev.acc_1G) * 1000);
} }
#if defined(USE_TELEMETRY_IBUS_EXTENDED) #if defined(USE_TELEMETRY_IBUS_EXTENDED)
static void setCombinedFrame(uint8_t* bufferPtr, const uint8_t* structure, uint8_t itemCount) static void setCombinedFrame(uint8_t* bufferPtr, const uint8_t* structure, uint8_t itemCount)
{ {
uint8_t offset = 0; uint8_t offset = 0;
uint8_t size = 0; uint8_t size = 0;
for (unsigned i = 0; i < itemCount; i++) { for (unsigned i = 0; i < itemCount; i++) {
@ -323,14 +323,14 @@
setValue(bufferPtr + offset, structure[i], size); setValue(bufferPtr + offset, structure[i], size);
offset += size; offset += size;
} }
} }
#endif #endif
#if defined(USE_GPS) #if defined(USE_GPS)
static bool setGPS(uint8_t sensorType, ibusTelemetry_s* value) static bool setGPS(uint8_t sensorType, ibusTelemetry_s* value)
{ {
bool result = false; bool result = false;
for (unsigned i = 0; i < sizeof(GPS_IDS); i++) { for (unsigned i = 0; i < sizeof(GPS_IDS); i++) {
if (sensorType == GPS_IDS[i]) { if (sensorType == GPS_IDS[i]) {
@ -379,14 +379,14 @@
} }
} }
return result; return result;
} }
#endif //defined(USE_GPS) #endif //defined(USE_GPS)
static void setValue(uint8_t* bufferPtr, uint8_t sensorType, uint8_t length) static void setValue(uint8_t* bufferPtr, uint8_t sensorType, uint8_t length)
{ {
ibusTelemetry_s value; ibusTelemetry_s value;
#if defined(USE_TELEMETRY_IBUS_EXTENDED) #if defined(USE_TELEMETRY_IBUS_EXTENDED)
const uint8_t* structure = 0; const uint8_t* structure = 0;
uint8_t itemCount; uint8_t itemCount;
if (sensorType == IBUS_SENSOR_TYPE_GPS_FULL) { if (sensorType == IBUS_SENSOR_TYPE_GPS_FULL) {
@ -405,13 +405,13 @@
setCombinedFrame(bufferPtr, structure, sizeof(itemCount)); setCombinedFrame(bufferPtr, structure, sizeof(itemCount));
return; return;
} }
#endif //defined(USE_TELEMETRY_IBUS_EXTENDED) #endif //defined(USE_TELEMETRY_IBUS_EXTENDED)
#if defined(USE_GPS) #if defined(USE_GPS)
if (setGPS(sensorType, &value)) { if (setGPS(sensorType, &value)) {
return; return;
} }
#endif //defined(USE_TELEMETRY_IBUS_EXTENDED) #endif //defined(USE_TELEMETRY_IBUS_EXTENDED)
for (unsigned i = 0; i < length; i++) { for (unsigned i = 0; i < length; i++) {
bufferPtr[i] = value.byte[i] = 0; bufferPtr[i] = value.byte[i] = 0;
@ -454,7 +454,7 @@
case IBUS_SENSOR_TYPE_ARMED: case IBUS_SENSOR_TYPE_ARMED:
value.uint16 = ARMING_FLAG(ARMED) ? 0 : 1; value.uint16 = ARMING_FLAG(ARMED) ? 0 : 1;
break; break;
#if defined(USE_TELEMETRY_IBUS_EXTENDED) #if defined(USE_TELEMETRY_IBUS_EXTENDED)
case IBUS_SENSOR_TYPE_CMP_HEAD: case IBUS_SENSOR_TYPE_CMP_HEAD:
value.uint16 = DECIDEGREES_TO_DEGREES(attitude.values.yaw); value.uint16 = DECIDEGREES_TO_DEGREES(attitude.values.yaw);
break; break;
@ -471,48 +471,48 @@
case IBUS_SENSOR_TYPE_PRES: case IBUS_SENSOR_TYPE_PRES:
value.uint32 = baro.baroPressure | (((uint32_t)getTemperature()) << 19); value.uint32 = baro.baroPressure | (((uint32_t)getTemperature()) << 19);
break; break;
#endif //defined(TELEMETRY_IBUS_EXTENDED) #endif //defined(TELEMETRY_IBUS_EXTENDED)
} }
for (unsigned i = 0; i < length; i++) { for (unsigned i = 0; i < length; i++) {
bufferPtr[i] = value.byte[i]; bufferPtr[i] = value.byte[i];
} }
} }
static void setIbusMeasurement(ibusAddress_t address) static void setIbusMeasurement(ibusAddress_t address)
{ {
uint8_t sensorID = getSensorID(address); uint8_t sensorID = getSensorID(address);
uint8_t sensorLength = getSensorLength(sensorID); uint8_t sensorLength = getSensorLength(sensorID);
sendBuffer[0] = IBUS_HEADER_FOOTER_SIZE + sensorLength; sendBuffer[0] = IBUS_HEADER_FOOTER_SIZE + sensorLength;
sendBuffer[1] = IBUS_COMMAND_MEASUREMENT | address; sendBuffer[1] = IBUS_COMMAND_MEASUREMENT | address;
setValue(sendBuffer + 2, sensorID, sensorLength); setValue(sendBuffer + 2, sensorID, sensorLength);
} }
static bool isCommand(ibusCommand_e expected, const uint8_t *ibusPacket) static bool isCommand(ibusCommand_e expected, const uint8_t *ibusPacket)
{ {
return (ibusPacket[1] & 0xF0) == expected; return (ibusPacket[1] & 0xF0) == expected;
} }
static ibusAddress_t getAddress(const uint8_t *ibusPacket) static ibusAddress_t getAddress(const uint8_t *ibusPacket)
{ {
return (ibusPacket[1] & 0x0F); return (ibusPacket[1] & 0x0F);
} }
static void autodetectFirstReceivedAddressAsBaseAddress(ibusAddress_t returnAddress) static void autodetectFirstReceivedAddressAsBaseAddress(ibusAddress_t returnAddress)
{ {
if ((INVALID_IBUS_ADDRESS == ibusBaseAddress) && if ((INVALID_IBUS_ADDRESS == ibusBaseAddress) &&
(INVALID_IBUS_ADDRESS != returnAddress)) { (INVALID_IBUS_ADDRESS != returnAddress)) {
ibusBaseAddress = returnAddress; ibusBaseAddress = returnAddress;
} }
} }
static bool theAddressIsWithinOurRange(ibusAddress_t returnAddress) static bool theAddressIsWithinOurRange(ibusAddress_t returnAddress)
{ {
return (returnAddress >= ibusBaseAddress) && return (returnAddress >= ibusBaseAddress) &&
(ibusAddress_t)(returnAddress - ibusBaseAddress) < ARRAYLEN(telemetryConfig()->flysky_sensors) && (ibusAddress_t)(returnAddress - ibusBaseAddress) < ARRAYLEN(telemetryConfig()->flysky_sensors) &&
telemetryConfig()->flysky_sensors[(returnAddress - ibusBaseAddress)] != IBUS_SENSOR_TYPE_NONE; telemetryConfig()->flysky_sensors[(returnAddress - ibusBaseAddress)] != IBUS_SENSOR_TYPE_NONE;
} }
uint8_t respondToIbusRequest(uint8_t const * const ibusPacket) uint8_t respondToIbusRequest(uint8_t const * const ibusPacket)
{ {
ibusAddress_t returnAddress = getAddress(ibusPacket); ibusAddress_t returnAddress = getAddress(ibusPacket);
autodetectFirstReceivedAddressAsBaseAddress(returnAddress); autodetectFirstReceivedAddressAsBaseAddress(returnAddress);
//set buffer to invalid //set buffer to invalid
@ -529,20 +529,20 @@
} }
//transmit if content was set //transmit if content was set
return transmitIbusPacket(); return transmitIbusPacket();
} }
void initSharedIbusTelemetry(serialPort_t *port) void initSharedIbusTelemetry(serialPort_t *port)
{ {
ibusSerialPort = port; ibusSerialPort = port;
ibusBaseAddress = INVALID_IBUS_ADDRESS; ibusBaseAddress = INVALID_IBUS_ADDRESS;
} }
#endif //defined(USE_TELEMETRY) && defined(USE_TELEMETRY_IBUS) #endif //defined(USE_TELEMETRY) && defined(USE_TELEMETRY_IBUS)
static uint16_t calculateChecksum(const uint8_t *ibusPacket) static uint16_t calculateChecksum(const uint8_t *ibusPacket)
{ {
uint16_t checksum = 0xFFFF; uint16_t checksum = 0xFFFF;
uint8_t dataSize = ibusPacket[0] - IBUS_CHECKSUM_SIZE; uint8_t dataSize = ibusPacket[0] - IBUS_CHECKSUM_SIZE;
for (unsigned i = 0; i < dataSize; i++) { for (unsigned i = 0; i < dataSize; i++) {
@ -550,13 +550,13 @@
} }
return checksum; return checksum;
} }
bool isChecksumOkIa6b(const uint8_t *ibusPacket, const uint8_t length) bool isChecksumOkIa6b(const uint8_t *ibusPacket, const uint8_t length)
{ {
uint16_t calculatedChecksum = calculateChecksum(ibusPacket); uint16_t calculatedChecksum = calculateChecksum(ibusPacket);
// Note that there's a byte order swap to little endian here // Note that there's a byte order swap to little endian here
return (calculatedChecksum >> 8) == ibusPacket[length - 1] return (calculatedChecksum >> 8) == ibusPacket[length - 1]
&& (calculatedChecksum & 0xFF) == ibusPacket[length - 2]; && (calculatedChecksum & 0xFF) == ibusPacket[length - 2];
} }