mirror of
https://github.com/betaflight/betaflight.git
synced 2025-07-16 04:45:24 +03:00
Fixed indentation in ibus_shared.c.
This commit is contained in:
mikeller
parent
db0f1c4f10
commit
d4f8cc7dfa
1 changed files with 481 additions and 481 deletions
|
|
@ -23,53 +23,53 @@
|
|||
*/
|
||||
|
||||
|
||||
#include <stdbool.h>
|
||||
#include <stdint.h>
|
||||
#include <stdlib.h>
|
||||
// #include <string.h>
|
||||
#include <stdbool.h>
|
||||
#include <stdint.h>
|
||||
#include <stdlib.h>
|
||||
// #include <string.h>
|
||||
|
||||
#include "platform.h"
|
||||
//#include "common/utils.h"
|
||||
#include "telemetry/telemetry.h"
|
||||
#include "telemetry/ibus_shared.h"
|
||||
#include "platform.h"
|
||||
//#include "common/utils.h"
|
||||
#include "telemetry/telemetry.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)
|
||||
#include "config/feature.h"
|
||||
#include "pg/pg.h"
|
||||
#include "pg/pg_ids.h"
|
||||
#include "sensors/battery.h"
|
||||
#include "fc/rc_controls.h"
|
||||
#include "fc/config.h"
|
||||
#include "sensors/gyro.h"
|
||||
#include "drivers/accgyro/accgyro.h"
|
||||
#include "fc/runtime_config.h"
|
||||
#include "sensors/acceleration.h"
|
||||
#include "sensors/sensors.h"
|
||||
#include "sensors/barometer.h"
|
||||
#include "flight/imu.h"
|
||||
#include "flight/altitude.h"
|
||||
#include "flight/navigation.h"
|
||||
#include "io/gps.h"
|
||||
#if defined(USE_TELEMETRY) && defined(USE_TELEMETRY_IBUS)
|
||||
#include "config/feature.h"
|
||||
#include "pg/pg.h"
|
||||
#include "pg/pg_ids.h"
|
||||
#include "sensors/battery.h"
|
||||
#include "fc/rc_controls.h"
|
||||
#include "fc/config.h"
|
||||
#include "sensors/gyro.h"
|
||||
#include "drivers/accgyro/accgyro.h"
|
||||
#include "fc/runtime_config.h"
|
||||
#include "sensors/acceleration.h"
|
||||
#include "sensors/sensors.h"
|
||||
#include "sensors/barometer.h"
|
||||
#include "flight/imu.h"
|
||||
#include "flight/altitude.h"
|
||||
#include "flight/navigation.h"
|
||||
#include "io/gps.h"
|
||||
|
||||
|
||||
#define IBUS_TEMPERATURE_OFFSET 400
|
||||
#define INVALID_IBUS_ADDRESS 0
|
||||
#define IBUS_BUFFSIZE 33 // biggest iBus message seen so far + 1
|
||||
#define IBUS_HEADER_FOOTER_SIZE 4
|
||||
#define IBUS_2BYTE_SESNSOR 2
|
||||
#define IBUS_4BYTE_SESNSOR 4
|
||||
#define IBUS_TEMPERATURE_OFFSET 400
|
||||
#define INVALID_IBUS_ADDRESS 0
|
||||
#define IBUS_BUFFSIZE 33 // biggest iBus message seen so far + 1
|
||||
#define IBUS_HEADER_FOOTER_SIZE 4
|
||||
#define IBUS_2BYTE_SESNSOR 2
|
||||
#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_SENSOR_TYPE = 0x90,
|
||||
IBUS_COMMAND_MEASUREMENT = 0xA0
|
||||
} ibusCommand_e;
|
||||
} ibusCommand_e;
|
||||
|
||||
typedef enum {
|
||||
typedef enum {
|
||||
IBUS_SENSOR_TYPE_NONE = 0x00,
|
||||
IBUS_SENSOR_TYPE_TEMPERATURE = 0x01,
|
||||
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_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_VOLT_FULL = 0xf0,
|
||||
IBUS_SENSOR_TYPE_ACC_FULL = 0xef,
|
||||
#endif //defined(TELEMETRY_IBUS_EXTENDED)
|
||||
#endif //defined(TELEMETRY_IBUS_EXTENDED)
|
||||
IBUS_SENSOR_TYPE_UNKNOWN = 0xff
|
||||
} ibusSensorType_e;
|
||||
} ibusSensorType_e;
|
||||
|
||||
typedef union ibusTelemetry {
|
||||
typedef union ibusTelemetry {
|
||||
uint16_t uint16;
|
||||
uint32_t uint32;
|
||||
int16_t int16;
|
||||
int32_t int32;
|
||||
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_SPE,
|
||||
IBUS_SENSOR_TYPE_GPS_LAT,
|
||||
|
|
@ -134,57 +134,57 @@
|
|||
IBUS_SENSOR_TYPE_ODO2,
|
||||
IBUS_SENSOR_TYPE_GPS_DIST,
|
||||
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_LAT,
|
||||
IBUS_SENSOR_TYPE_GPS_LON,
|
||||
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_CELL,
|
||||
IBUS_SENSOR_TYPE_BAT_CURR,
|
||||
IBUS_SENSOR_TYPE_FUEL,
|
||||
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_Y,
|
||||
IBUS_SENSOR_TYPE_ACC_Z,
|
||||
IBUS_SENSOR_TYPE_ROLL,
|
||||
IBUS_SENSOR_TYPE_PITCH,
|
||||
IBUS_SENSOR_TYPE_YAW,
|
||||
};
|
||||
};
|
||||
|
||||
#endif //defined(USE_TELEMETRY_IBUS_EXTENDED)
|
||||
#endif //defined(USE_TELEMETRY_IBUS_EXTENDED)
|
||||
|
||||
static serialPort_t *ibusSerialPort = NULL;
|
||||
static ibusAddress_t ibusBaseAddress = INVALID_IBUS_ADDRESS;
|
||||
static uint8_t sendBuffer[IBUS_BUFFSIZE];
|
||||
static serialPort_t *ibusSerialPort = NULL;
|
||||
static ibusAddress_t ibusBaseAddress = INVALID_IBUS_ADDRESS;
|
||||
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
|
||||
uint32_t index = address - ibusBaseAddress;
|
||||
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)) {
|
||||
return IBUS_4BYTE_SESNSOR;
|
||||
}
|
||||
#if defined(USE_TELEMETRY_IBUS_EXTENDED)
|
||||
#if defined(USE_TELEMETRY_IBUS_EXTENDED)
|
||||
if (sensorID == IBUS_SENSOR_TYPE_GPS_FULL) {
|
||||
return 14;
|
||||
}
|
||||
|
|
@ -194,12 +194,12 @@
|
|||
if (sensorID == IBUS_SENSOR_TYPE_VOLT_FULL) {
|
||||
return 12;
|
||||
}
|
||||
#endif
|
||||
#endif
|
||||
return IBUS_2BYTE_SESNSOR;
|
||||
}
|
||||
}
|
||||
|
||||
static uint8_t transmitIbusPacket()
|
||||
{
|
||||
static uint8_t transmitIbusPacket()
|
||||
{
|
||||
unsigned frameLength = sendBuffer[0];
|
||||
if (frameLength == INVALID_IBUS_ADDRESS) {
|
||||
return 0;
|
||||
|
|
@ -212,47 +212,47 @@
|
|||
serialWrite(ibusSerialPort, checksum & 0xFF);
|
||||
serialWrite(ibusSerialPort, checksum >> 8);
|
||||
return frameLength;
|
||||
}
|
||||
}
|
||||
|
||||
static void setIbusDiscoverSensorReply(ibusAddress_t address)
|
||||
{
|
||||
static void setIbusDiscoverSensorReply(ibusAddress_t address)
|
||||
{
|
||||
sendBuffer[0] = IBUS_HEADER_FOOTER_SIZE;
|
||||
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 sensorLength = getSensorLength(sensorID);
|
||||
sendBuffer[0] = IBUS_HEADER_FOOTER_SIZE + 2;
|
||||
sendBuffer[1] = IBUS_COMMAND_SENSOR_TYPE | address;
|
||||
sendBuffer[2] = sensorID;
|
||||
sendBuffer[3] = sensorLength;
|
||||
}
|
||||
}
|
||||
|
||||
static uint16_t getVoltage()
|
||||
{
|
||||
static uint16_t getVoltage()
|
||||
{
|
||||
uint16_t voltage = getBatteryVoltage() *10;
|
||||
if (telemetryConfig()->report_cell_voltage) {
|
||||
voltage /= getBatteryCellCount();
|
||||
}
|
||||
return voltage;
|
||||
}
|
||||
}
|
||||
|
||||
static uint16_t getTemperature()
|
||||
{
|
||||
static uint16_t getTemperature()
|
||||
{
|
||||
uint16_t temperature = gyroGetTemperature() * 10;
|
||||
#if defined(USE_BARO)
|
||||
#if defined(USE_BARO)
|
||||
if (sensors(SENSOR_BARO)) {
|
||||
temperature = (uint16_t) ((baro.baroTemperature + 50) / 10);
|
||||
}
|
||||
#endif
|
||||
#endif
|
||||
return temperature + IBUS_TEMPERATURE_OFFSET;
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
static uint16_t getFuel()
|
||||
{
|
||||
static uint16_t getFuel()
|
||||
{
|
||||
uint16_t fuel = 0;
|
||||
if (batteryConfig()->batteryCapacity > 0) {
|
||||
fuel = (uint16_t)calculateBatteryPercentageRemaining();
|
||||
|
|
@ -260,10 +260,10 @@
|
|||
fuel = (uint16_t)constrain(getMAhDrawn(), 0, 0xFFFF);
|
||||
}
|
||||
return fuel;
|
||||
}
|
||||
}
|
||||
|
||||
static uint16_t getRPM()
|
||||
{
|
||||
static uint16_t getRPM()
|
||||
{
|
||||
uint16_t rpm = 0;
|
||||
if (ARMING_FLAG(ARMED)) {
|
||||
const throttleStatus_e throttleStatus = calculateThrottleStatus();
|
||||
|
|
@ -273,10 +273,10 @@
|
|||
rpm = (uint16_t)(batteryConfig()->batteryCapacity); // / BLADE_NUMBER_DIVIDER
|
||||
}
|
||||
return rpm;
|
||||
}
|
||||
}
|
||||
|
||||
static uint16_t getMode()
|
||||
{
|
||||
static uint16_t getMode()
|
||||
{
|
||||
uint16_t flightMode = 1; //Acro
|
||||
if (FLIGHT_MODE(ANGLE_MODE)) {
|
||||
flightMode = 0; //Stab
|
||||
|
|
@ -306,16 +306,16 @@
|
|||
flightMode = 9; //Land
|
||||
}
|
||||
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);
|
||||
}
|
||||
}
|
||||
|
||||
#if defined(USE_TELEMETRY_IBUS_EXTENDED)
|
||||
static void setCombinedFrame(uint8_t* bufferPtr, const uint8_t* structure, uint8_t itemCount)
|
||||
{
|
||||
#if defined(USE_TELEMETRY_IBUS_EXTENDED)
|
||||
static void setCombinedFrame(uint8_t* bufferPtr, const uint8_t* structure, uint8_t itemCount)
|
||||
{
|
||||
uint8_t offset = 0;
|
||||
uint8_t size = 0;
|
||||
for (unsigned i = 0; i < itemCount; i++) {
|
||||
|
|
@ -323,14 +323,14 @@
|
|||
setValue(bufferPtr + offset, structure[i], size);
|
||||
offset += size;
|
||||
}
|
||||
}
|
||||
#endif
|
||||
}
|
||||
#endif
|
||||
|
||||
|
||||
|
||||
#if defined(USE_GPS)
|
||||
static bool setGPS(uint8_t sensorType, ibusTelemetry_s* value)
|
||||
{
|
||||
#if defined(USE_GPS)
|
||||
static bool setGPS(uint8_t sensorType, ibusTelemetry_s* value)
|
||||
{
|
||||
bool result = false;
|
||||
for (unsigned i = 0; i < sizeof(GPS_IDS); i++) {
|
||||
if (sensorType == GPS_IDS[i]) {
|
||||
|
|
@ -379,14 +379,14 @@
|
|||
}
|
||||
}
|
||||
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;
|
||||
|
||||
#if defined(USE_TELEMETRY_IBUS_EXTENDED)
|
||||
#if defined(USE_TELEMETRY_IBUS_EXTENDED)
|
||||
const uint8_t* structure = 0;
|
||||
uint8_t itemCount;
|
||||
if (sensorType == IBUS_SENSOR_TYPE_GPS_FULL) {
|
||||
|
|
@ -405,13 +405,13 @@
|
|||
setCombinedFrame(bufferPtr, structure, sizeof(itemCount));
|
||||
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)) {
|
||||
return;
|
||||
}
|
||||
#endif //defined(USE_TELEMETRY_IBUS_EXTENDED)
|
||||
#endif //defined(USE_TELEMETRY_IBUS_EXTENDED)
|
||||
|
||||
for (unsigned i = 0; i < length; i++) {
|
||||
bufferPtr[i] = value.byte[i] = 0;
|
||||
|
|
@ -454,7 +454,7 @@
|
|||
case IBUS_SENSOR_TYPE_ARMED:
|
||||
value.uint16 = ARMING_FLAG(ARMED) ? 0 : 1;
|
||||
break;
|
||||
#if defined(USE_TELEMETRY_IBUS_EXTENDED)
|
||||
#if defined(USE_TELEMETRY_IBUS_EXTENDED)
|
||||
case IBUS_SENSOR_TYPE_CMP_HEAD:
|
||||
value.uint16 = DECIDEGREES_TO_DEGREES(attitude.values.yaw);
|
||||
break;
|
||||
|
|
@ -471,48 +471,48 @@
|
|||
case IBUS_SENSOR_TYPE_PRES:
|
||||
value.uint32 = baro.baroPressure | (((uint32_t)getTemperature()) << 19);
|
||||
break;
|
||||
#endif //defined(TELEMETRY_IBUS_EXTENDED)
|
||||
#endif //defined(TELEMETRY_IBUS_EXTENDED)
|
||||
}
|
||||
for (unsigned i = 0; i < length; 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 sensorLength = getSensorLength(sensorID);
|
||||
sendBuffer[0] = IBUS_HEADER_FOOTER_SIZE + sensorLength;
|
||||
sendBuffer[1] = IBUS_COMMAND_MEASUREMENT | address;
|
||||
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;
|
||||
}
|
||||
}
|
||||
|
||||
static ibusAddress_t getAddress(const uint8_t *ibusPacket)
|
||||
{
|
||||
static ibusAddress_t getAddress(const uint8_t *ibusPacket)
|
||||
{
|
||||
return (ibusPacket[1] & 0x0F);
|
||||
}
|
||||
}
|
||||
|
||||
static void autodetectFirstReceivedAddressAsBaseAddress(ibusAddress_t returnAddress)
|
||||
{
|
||||
static void autodetectFirstReceivedAddressAsBaseAddress(ibusAddress_t returnAddress)
|
||||
{
|
||||
if ((INVALID_IBUS_ADDRESS == ibusBaseAddress) &&
|
||||
(INVALID_IBUS_ADDRESS != returnAddress)) {
|
||||
ibusBaseAddress = returnAddress;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
static bool theAddressIsWithinOurRange(ibusAddress_t returnAddress)
|
||||
{
|
||||
static bool theAddressIsWithinOurRange(ibusAddress_t returnAddress)
|
||||
{
|
||||
return (returnAddress >= ibusBaseAddress) &&
|
||||
(ibusAddress_t)(returnAddress - ibusBaseAddress) < ARRAYLEN(telemetryConfig()->flysky_sensors) &&
|
||||
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);
|
||||
autodetectFirstReceivedAddressAsBaseAddress(returnAddress);
|
||||
//set buffer to invalid
|
||||
|
|
@ -529,20 +529,20 @@
|
|||
}
|
||||
//transmit if content was set
|
||||
return transmitIbusPacket();
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
void initSharedIbusTelemetry(serialPort_t *port)
|
||||
{
|
||||
void initSharedIbusTelemetry(serialPort_t *port)
|
||||
{
|
||||
ibusSerialPort = port;
|
||||
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;
|
||||
uint8_t dataSize = ibusPacket[0] - IBUS_CHECKSUM_SIZE;
|
||||
for (unsigned i = 0; i < dataSize; i++) {
|
||||
|
|
@ -550,13 +550,13 @@
|
|||
}
|
||||
|
||||
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);
|
||||
|
||||
// Note that there's a byte order swap to little endian here
|
||||
return (calculatedChecksum >> 8) == ibusPacket[length - 1]
|
||||
&& (calculatedChecksum & 0xFF) == ibusPacket[length - 2];
|
||||
}
|
||||
}
|
||||
|
|
|
|||
Loading…
Add table
Add a link
Reference in a new issue