diff --git a/Makefile b/Makefile index 74d2188e1d..01511d2ec1 100644 --- a/Makefile +++ b/Makefile @@ -540,6 +540,7 @@ COMMON_SRC = \ rx/pwm.c \ rx/rx.c \ rx/rx_spi.c \ + rx/crsf.c \ rx/sbus.c \ rx/spektrum.c \ rx/sumd.c \ @@ -585,6 +586,7 @@ HIGHEND_SRC = \ sensors/sonar.c \ sensors/barometer.c \ telemetry/telemetry.c \ + telemetry/crsf.c \ telemetry/frsky.c \ telemetry/hott.c \ telemetry/smartport.c \ diff --git a/src/main/build/debug.h b/src/main/build/debug.h index 69644af5d9..7f17cc34c5 100644 --- a/src/main/build/debug.h +++ b/src/main/build/debug.h @@ -60,5 +60,6 @@ typedef enum { DEBUG_DTERM_FILTER, DEBUG_ANGLERATE, DEBUG_ESC_TELEMETRY, + DEBUG_CRSF, DEBUG_COUNT } debugType_e; diff --git a/src/main/fc/mw.c b/src/main/fc/mw.c index 9a77edd6e9..ac3895ceb2 100644 --- a/src/main/fc/mw.c +++ b/src/main/fc/mw.c @@ -377,7 +377,7 @@ void mwDisarm(void) } } -#define TELEMETRY_FUNCTION_MASK (FUNCTION_TELEMETRY_FRSKY | FUNCTION_TELEMETRY_HOTT | FUNCTION_TELEMETRY_LTM | FUNCTION_TELEMETRY_SMARTPORT) +#define TELEMETRY_FUNCTION_MASK (FUNCTION_TELEMETRY_FRSKY | FUNCTION_TELEMETRY_HOTT | FUNCTION_TELEMETRY_LTM | FUNCTION_TELEMETRY_SMARTPORT | FUNCTION_TELEMETRY_CRSF) void releaseSharedTelemetryPorts(void) { serialPort_t *sharedPort = findSharedSerialPort(TELEMETRY_FUNCTION_MASK, FUNCTION_MSP); diff --git a/src/main/io/serial.c b/src/main/io/serial.c index ebc242cc21..39b19641b4 100644 --- a/src/main/io/serial.c +++ b/src/main/io/serial.c @@ -209,9 +209,9 @@ serialPort_t *findNextSharedSerialPort(uint16_t functionMask, serialPortFunction } #ifdef TELEMETRY -#define ALL_TELEMETRY_FUNCTIONS_MASK (TELEMETRY_SHAREABLE_PORT_FUNCTIONS_MASK | FUNCTION_TELEMETRY_HOTT | FUNCTION_TELEMETRY_SMARTPORT | FUNCTION_TELEMETRY_MAVLINK) +#define ALL_TELEMETRY_FUNCTIONS_MASK (TELEMETRY_SHAREABLE_PORT_FUNCTIONS_MASK | FUNCTION_TELEMETRY_HOTT | FUNCTION_TELEMETRY_SMARTPORT | FUNCTION_TELEMETRY_MAVLINK | FUNCTION_TELEMETRY_CRSF) #else -#define ALL_TELEMETRY_FUNCTIONS_MASK (FUNCTION_TELEMETRY_FRSKY | FUNCTION_TELEMETRY_HOTT | FUNCTION_TELEMETRY_SMARTPORT | FUNCTION_TELEMETRY_LTM | FUNCTION_TELEMETRY_MAVLINK) +#define ALL_TELEMETRY_FUNCTIONS_MASK (FUNCTION_TELEMETRY_FRSKY | FUNCTION_TELEMETRY_HOTT | FUNCTION_TELEMETRY_SMARTPORT | FUNCTION_TELEMETRY_LTM | FUNCTION_TELEMETRY_MAVLINK | FUNCTION_TELEMETRY_CRSF) #endif #define ALL_FUNCTIONS_SHARABLE_WITH_MSP (FUNCTION_BLACKBOX | ALL_TELEMETRY_FUNCTIONS_MASK) diff --git a/src/main/io/serial.h b/src/main/io/serial.h index 99027830d8..458c45638c 100644 --- a/src/main/io/serial.h +++ b/src/main/io/serial.h @@ -38,6 +38,7 @@ typedef enum { FUNCTION_PASSTHROUGH = (1 << 8), // 256 FUNCTION_TELEMETRY_MAVLINK = (1 << 9), // 512 FUNCTION_TELEMETRY_ESC = (1 << 10), // 1024 + FUNCTION_TELEMETRY_CRSF = (1 << 11), // 2048 } serialPortFunction_e; typedef enum { diff --git a/src/main/rx/crsf.c b/src/main/rx/crsf.c new file mode 100644 index 0000000000..eac9d82b6b --- /dev/null +++ b/src/main/rx/crsf.c @@ -0,0 +1,220 @@ +/* + * This file is part of Cleanflight. + * + * Cleanflight is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * Cleanflight is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with Cleanflight. If not, see . + */ + +#include +#include +#include + +#include "platform.h" + +#ifdef SERIAL_RX + +#include "build/version.h" + +#if (FC_VERSION_MAJOR == 3) // not a very good way of finding out if this is betaflight or Cleanflight +#define BETAFLIGHT +#else +#define CLEANFLIGHT +#endif + +#ifdef CLEANFLIGHT +#include "config/parameter_group.h" +#include "config/parameter_group_ids.h" +#include "fc/fc_debug.h" +#endif + +#include "build/debug.h" + +#include "common/utils.h" + +#include "drivers/system.h" +#include "drivers/serial.h" +#include "drivers/serial_uart.h" + +#include "io/serial.h" + +#include "rx/rx.h" +#include "rx/crsf.h" + +#include "telemetry/telemetry.h" + +#define CRSF_TIME_NEEDED_PER_FRAME_US 1500 //!! this needs checking + +#define CRSF_MAX_CHANNEL 16 + +#define CRSF_DIGITAL_CHANNEL_MIN 172 +#define CRSF_DIGITAL_CHANNEL_MAX 1811 + +static bool crsfFrameDone = false; + +static uint32_t crsfChannelData[CRSF_MAX_CHANNEL]; + +/* +Structure +400kbaud +Inverted None +8 Bit +1 Stop bit None +Big endian + +Every frame has the structure: + < Type> < CRC> + +Device address: (uint8_t) +Frame length: length in bytes including Type (uint8_t) +Type: (uint8_t) +CRC: (uint8_t) +*/ +struct crsfPayloadRcChannelsPacked_s { + // 176 bits of data (11 bits per channel * 16 channels) = 22 bytes. + unsigned int chan0 : 11; + unsigned int chan1 : 11; + unsigned int chan2 : 11; + unsigned int chan3 : 11; + unsigned int chan4 : 11; + unsigned int chan5 : 11; + unsigned int chan6 : 11; + unsigned int chan7 : 11; + unsigned int chan8 : 11; + unsigned int chan9 : 11; + unsigned int chan10 : 11; + unsigned int chan11 : 11; + unsigned int chan12 : 11; + unsigned int chan13 : 11; + unsigned int chan14 : 11; + unsigned int chan15 : 11; +} __attribute__ ((__packed__)); + +typedef struct crsfPayloadRcChannelsPacked_s crsfPayloadRcChannelsPacked_t; + +typedef union crsfFrameDef_s { + uint8_t deviceAddress; + uint8_t frameLength; + uint8_t type; + uint8_t payload[CRSF_PAYLOAD_SIZE_MAX + 1]; // +1 for CRC at end of payload +} crsfFrameDef_t; + +typedef union crsfFrame_u { + uint8_t bytes[CRSF_FRAME_SIZE_MAX]; + crsfFrameDef_t frame; +} crsfFrame_t; + +static crsfFrame_t crsfFrame; + +// Receive ISR callback, called back from serial port +static void crsfDataReceive(uint16_t c) +{ + static uint8_t crsfFramePosition = 0; + static uint32_t crsfFrameStartAt = 0; + const uint32_t now = micros(); + + const int32_t crsfFrameTime = now - crsfFrameStartAt; + DEBUG_SET(DEBUG_CRSF, 2, crsfFrameTime); + + if (crsfFrameTime > (long)(CRSF_TIME_NEEDED_PER_FRAME_US + 500)) { + crsfFramePosition = 0; + } + + if (crsfFramePosition == 0) { + crsfFrameStartAt = now; + } + // assume frame is 5 bytes long until we have received the frame length + const int frameLength = crsfFramePosition < 3 ? 5 : crsfFrame.frame.frameLength; + + if (crsfFramePosition < frameLength) { + crsfFrame.bytes[crsfFramePosition++] = (uint8_t)c; + crsfFrameDone = crsfFramePosition < frameLength ? false : true; + } +} + +uint8_t crsfFrameStatus(void) +{ + if (crsfFrameDone) { + crsfFrameDone = false; + if (crsfFrame.frame.type == CRSF_FRAMETYPE_RC_CHANNELS_PACKED) { + // unpack the RC channels + const crsfPayloadRcChannelsPacked_t* rcChannels = (crsfPayloadRcChannelsPacked_t*)&crsfFrame.frame.payload; + crsfChannelData[0] = rcChannels->chan0; + crsfChannelData[1] = rcChannels->chan1; + crsfChannelData[2] = rcChannels->chan2; + crsfChannelData[3] = rcChannels->chan3; + crsfChannelData[4] = rcChannels->chan4; + crsfChannelData[5] = rcChannels->chan5; + crsfChannelData[6] = rcChannels->chan6; + crsfChannelData[7] = rcChannels->chan7; + crsfChannelData[8] = rcChannels->chan8; + crsfChannelData[9] = rcChannels->chan9; + crsfChannelData[10] = rcChannels->chan10; + crsfChannelData[11] = rcChannels->chan11; + crsfChannelData[12] = rcChannels->chan12; + crsfChannelData[13] = rcChannels->chan13; + crsfChannelData[14] = rcChannels->chan14; + crsfChannelData[15] = rcChannels->chan15; + return RX_FRAME_COMPLETE; + } + } + return RX_FRAME_PENDING; +} + +static uint16_t crsfReadRawRC(const rxRuntimeConfig_t *rxRuntimeConfig, uint8_t chan) +{ + UNUSED(rxRuntimeConfig); + /* conversion from RC value to PWM + * RC PWM + * min 172 -> 988us + * mid 992 -> 1500us + * max 1811 -> 2012us + * scale factor = (2012-988) / (1811-172) = 0.62477120195241 + * offset = 988 - 172 * 0.62477120195241 = 880.53935326418548 + */ + return (0.62477120195241f * crsfChannelData[chan]) + 881; +} + +bool crsfInit(const rxConfig_t *rxConfig, rxRuntimeConfig_t *rxRuntimeConfig) +{ + for (int ii = 0; ii < CRSF_MAX_CHANNEL; ++ii) { + crsfChannelData[ii] = (16 * rxConfig->midrc) / 10 - 1408; + } + + rxRuntimeConfig->channelCount = CRSF_MAX_CHANNEL; + rxRuntimeConfig->rxRefreshRate = 11000; //!!TODO this needs checking + + rxRuntimeConfig->rcReadRawFunc = crsfReadRawRC; + rxRuntimeConfig->rcFrameStatusFunc = crsfFrameStatus; + + const serialPortConfig_t *portConfig = findSerialPortConfig(FUNCTION_RX_SERIAL); + if (!portConfig) { + return false; + } + +#if defined(TELEMETRY) && !defined(CLEANFLIGHT) + const bool portShared = telemetryCheckRxPortShared(portConfig); +#else + const bool portShared = false; +#endif + + serialPort_t *serialPort = openSerialPort(portConfig->identifier, FUNCTION_RX_SERIAL, crsfDataReceive, CRSF_BAUDRATE, portShared ? MODE_RXTX : MODE_RX, CRSF_PORT_OPTIONS); + +#if defined(TELEMETRY) && !defined(CLEANFLIGHT) + if (portShared) { + telemetrySharedPort = serialPort; + } +#endif + + return serialPort != NULL; +} +#endif diff --git a/src/main/rx/crsf.h b/src/main/rx/crsf.h new file mode 100644 index 0000000000..75a82e5849 --- /dev/null +++ b/src/main/rx/crsf.h @@ -0,0 +1,51 @@ +/* + * This file is part of Cleanflight. + * + * Cleanflight is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * Cleanflight is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with Cleanflight. If not, see . + */ + +#pragma once + +#define CRSF_BAUDRATE 400000 +#define CRSF_PORT_OPTIONS (SERIAL_INVERTED | SERIAL_STOPBITS_1 | SERIAL_PARITY_NO) + +typedef enum { + CRSF_FRAMETYPE_GPS = 0x02, + CRSF_FRAMETYPE_BATTERY_SENSOR = 0x08, + CRSF_FRAMETYPE_LINK_STATISTICS = 0x14, + CRSF_FRAMETYPE_RC_CHANNELS_PACKED = 0x16, + CRSF_FRAMETYPE_ATTITUDE = 0x1E, + CRSF_FRAMETYPE_FLIGHT_MODE = 0x21 +} crsfFrameTypes_e; + +enum { + CRSF_FRAME_GPS_PAYLOAD_SIZE = 15, + CRSF_FRAME_BATTERY_SENSOR_PAYLOAD_SIZE = 8, + CRSF_FRAME_LINK_STATISTICS_PAYLOAD_SIZE = 10, + CRSF_FRAME_RC_CHANNELS_PAYLOAD_SIZE = 22, // 11 bits per channel * 16 channels = 22 bytes. + CRSF_FRAME_ATTITUDE_PAYLOAD_SIZE = 6, + CRSF_FRAME_LENGTH_TYPE = 1, // length of TYPE field + CRSF_FRAME_LENGTH_CRC = 1, // length of CRC field + CRSF_FRAME_LENGTH_TYPE_CRC = 2 // length of TYPE and CRC fields combined +}; + +#define CRSF_RECEIVER_ADDRESS 0xEC + +#define CRSF_PAYLOAD_SIZE_MAX 32 // !!TODO needs checking +#define CRSF_FRAME_SIZE_MAX (CRSF_PAYLOAD_SIZE_MAX + 4) + +uint8_t crsfFrameStatus(void); +struct rxConfig_s; +struct rxRuntimeConfig_s; +bool crsfInit(const struct rxConfig_s *initialRxConfig, struct rxRuntimeConfig_s *rxRuntimeConfig); diff --git a/src/main/rx/rx.c b/src/main/rx/rx.c index e176608e9b..9a1611fc1c 100644 --- a/src/main/rx/rx.c +++ b/src/main/rx/rx.c @@ -52,6 +52,7 @@ #include "rx/xbus.h" #include "rx/ibus.h" #include "rx/jetiexbus.h" +#include "rx/crsf.h" #include "rx/rx_spi.h" @@ -187,10 +188,14 @@ bool serialRxInit(const rxConfig_t *rxConfig, rxRuntimeConfig_t *rxRuntimeConfig case SERIALRX_JETIEXBUS: enabled = jetiExBusInit(rxConfig, rxRuntimeConfig); break; +#endif +#ifdef USE_SERIALRX_CRSF + case SERIALRX_CRSF: + enabled = crsfInit(rxConfig, rxRuntimeConfig); + break; #endif default: enabled = false; - break; } return enabled; } diff --git a/src/main/rx/rx.h b/src/main/rx/rx.h index 98a17e7c60..3a47af66d2 100644 --- a/src/main/rx/rx.h +++ b/src/main/rx/rx.h @@ -53,6 +53,7 @@ typedef enum { SERIALRX_XBUS_MODE_B_RJ01 = 6, SERIALRX_IBUS = 7, SERIALRX_JETIEXBUS = 8, + SERIALRX_CRSF = 9, SERIALRX_PROVIDER_COUNT, SERIALRX_PROVIDER_MAX = SERIALRX_PROVIDER_COUNT - 1 } SerialRXType; diff --git a/src/main/target/CC3D/target.h b/src/main/target/CC3D/target.h index e57b5cbfa8..c1149951e8 100644 --- a/src/main/target/CC3D/target.h +++ b/src/main/target/CC3D/target.h @@ -111,6 +111,7 @@ #undef MAG #ifdef CC3D_OPBL +#undef USE_SERIAL_4WAY_BLHELI_INTERFACE #define SKIP_CLI_COMMAND_HELP #undef BARO #undef SONAR diff --git a/src/main/target/common.h b/src/main/target/common.h index f2682ee217..a38b075a78 100644 --- a/src/main/target/common.h +++ b/src/main/target/common.h @@ -44,6 +44,7 @@ #endif #define SERIAL_RX +#define USE_SERIALRX_CRSF // Team Black Sheep Crossfire protocol #define USE_SERIALRX_SPEKTRUM // DSM2 and DSMX protocol #define USE_SERIALRX_SBUS // Frsky and Futaba receivers #define USE_SERIALRX_IBUS // FlySky and Turnigy receivers @@ -80,6 +81,7 @@ #define CMS #define USE_DASHBOARD #define USE_MSP_DISPLAYPORT +#define TELEMETRY_CRSF #define TELEMETRY_JETIEXBUS #define TELEMETRY_MAVLINK #define USE_RX_MSP diff --git a/src/main/telemetry/crsf.c b/src/main/telemetry/crsf.c new file mode 100644 index 0000000000..d9ee54ca0a --- /dev/null +++ b/src/main/telemetry/crsf.c @@ -0,0 +1,437 @@ +/* + * This file is part of Cleanflight. + * + * Cleanflight is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * Cleanflight is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with Cleanflight. If not, see . + */ + +#include +#include +#include + +#include "platform.h" + +#ifdef TELEMETRY + +#include "build/version.h" + +#if (FC_VERSION_MAJOR == 3) // not a very good way of finding out if this is betaflight or Cleanflight +#define BETAFLIGHT +#else +#define CLEANFLIGHT +#endif + +#ifdef CLEANFLIGHT +#include "config/parameter_group.h" +#include "config/parameter_group_ids.h" +#endif + +#include "build/build_config.h" + +#include "common/maths.h" +#include "common/axis.h" +#include "common/color.h" +#include "common/maths.h" +#include "common/streambuf.h" +#include "common/utils.h" + +#include "drivers/system.h" +#include "drivers/sensor.h" +#include "drivers/accgyro.h" +#include "drivers/timer.h" +#include "drivers/serial.h" +#include "drivers/pwm_rx.h" + +#include "sensors/sensors.h" +#include "sensors/acceleration.h" +#include "sensors/gyro.h" +#include "sensors/barometer.h" +#include "sensors/boardalignment.h" +#include "sensors/battery.h" + +#include "io/gps.h" +#include "io/serial.h" + +#include "fc/rc_controls.h" +#include "fc/runtime_config.h" + +#include "io/gps.h" + +#include "flight/mixer.h" +#include "flight/pid.h" +#include "flight/imu.h" +#include "flight/failsafe.h" + +#include "rx/rx.h" +#include "rx/crsf.h" + +#include "telemetry/telemetry.h" +#include "telemetry/crsf.h" + +#ifdef CLEANFLIGHT +#include "fc/fc_serial.h" +#include "sensors/amperage.h" +#else +#include "fc/config.h" +#endif + +#define TELEMETRY_CRSF_INITIAL_PORT_MODE MODE_RXTX +#define CRSF_CYCLETIME_US 100000 + +static serialPort_t *serialPort; +static serialPortConfig_t *serialPortConfig; +static portSharing_e portSharing; +static bool crsfTelemetryEnabled; +static uint8_t crsfCrc; +static uint8_t crsfFrame[CRSF_FRAME_SIZE_MAX]; + +static void crsfInitializeFrame(sbuf_t *dst) +{ + crsfCrc = 0; + dst->ptr = crsfFrame; + dst->end = ARRAYEND(crsfFrame); + + sbufWriteU8(dst, CRSF_RECEIVER_ADDRESS); +} + +static void crsfSerialize8(sbuf_t *dst, uint8_t v) +{ + sbufWriteU8(dst, v); + crsfCrc = crc8_dvb_s2(crsfCrc, v); +} + +static void crsfSerialize16(sbuf_t *dst, uint16_t v) +{ + // Use BigEndian format + crsfSerialize8(dst, (v >> 8)); + crsfSerialize8(dst, (uint8_t)v); +} + +static void crsfSerialize32(sbuf_t *dst, uint32_t v) +{ + // Use BigEndian format + crsfSerialize8(dst, (v >> 24)); + crsfSerialize8(dst, (v >> 16)); + crsfSerialize8(dst, (v >> 8)); + crsfSerialize8(dst, (uint8_t)v); +} + +static void crsfFinalize(sbuf_t *dst) +{ + sbufWriteU8(dst, crsfCrc); + sbufSwitchToReader(dst, crsfFrame); + serialWriteBuf(serialPort, sbufPtr(dst), sbufBytesRemaining(dst)); +} + +static int crsfFinalizeBuf(sbuf_t *dst, uint8_t *frame) +{ + sbufWriteU8(dst, crsfCrc); + sbufSwitchToReader(dst, crsfFrame); + const int frameSize = sbufBytesRemaining(dst); + for (int ii = 0; sbufBytesRemaining(dst); ++ii) { + frame[ii] = sbufReadU8(dst); + } + return frameSize; +} +/* +CRSF frame has the structure: + +Device address: (uint8_t) +Frame length: length in bytes including Type (uint8_t) +Type: (uint8_t) +CRC: (uint8_t) +*/ + +/* +0x02 GPS +Payload: +int32_t Latitude ( degree / 10`000`000 ) +int32_t Longitude (degree / 10`000`000 ) +uint16_t Groundspeed ( km/h / 100 ) +uint16_t GPS heading ( degree / 100 ) +uint16 Altitude ( meter ­ 1000m offset ) +uint8_t Satellites in use ( counter ) +*/ +void crsfFrameGps(sbuf_t *dst) +{ + // use sbufWrite since CRC does not include frame length + sbufWriteU8(dst, CRSF_FRAME_GPS_PAYLOAD_SIZE + CRSF_FRAME_LENGTH_TYPE_CRC); + crsfSerialize8(dst, CRSF_FRAMETYPE_GPS); + crsfSerialize32(dst, GPS_coord[LAT]); // CRSF and betaflight use same units for degrees + crsfSerialize32(dst, GPS_coord[LON]); + crsfSerialize16(dst, GPS_speed * 36); // GPS_speed is in 0.1m/s + crsfSerialize16(dst, GPS_ground_course * 10); // GPS_ground_course is degrees * 10 + //Send real GPS altitude only if it's reliable (there's a GPS fix) + const uint16_t altitude = (STATE(GPS_FIX) ? GPS_altitude : 0) + 1000; + crsfSerialize16(dst, altitude); + crsfSerialize8(dst, GPS_numSat); +} + +/* +0x08 Battery sensor +Payload: +uint16_t Voltage ( mV * 100 ) +uint16_t Current ( mA * 100 ) +uint24_t Capacity ( mAh ) +uint8_t Battery remaining ( percent ) +*/ +void crsfFrameBatterySensor(sbuf_t *dst) +{ + // use sbufWrite since CRC does not include frame length + sbufWriteU8(dst, CRSF_FRAME_BATTERY_SENSOR_PAYLOAD_SIZE + CRSF_FRAME_LENGTH_TYPE_CRC); + crsfSerialize8(dst, CRSF_FRAMETYPE_BATTERY_SENSOR); + crsfSerialize16(dst, vbat); // vbat is in units of 0.1V +#ifdef CLEANFLIGHT + const amperageMeter_t *amperageMeter = getAmperageMeter(batteryConfig()->amperageMeterSource); + const int16_t amperage = constrain(amperageMeter->amperage, -0x8000, 0x7FFF) / 10; // send amperage in 0.01 A steps, range is -320A to 320A + crsfSerialize16(dst, amperage); // amperage is in units of 0.1A + const uint32_t batteryCapacity = batteryConfig()->batteryCapacity; + const uint8_t batteryRemainingPercentage = batteryCapacityRemainingPercentage(); +#else + crsfSerialize16(dst, amperage / 10); + const uint32_t batteryCapacity = batteryConfig->batteryCapacity; + const uint8_t batteryRemainingPercentage = calculateBatteryCapacityRemainingPercentage(); +#endif + crsfSerialize8(dst, (batteryCapacity >> 16)); + crsfSerialize8(dst, (batteryCapacity >> 8)); + crsfSerialize8(dst, (uint8_t)batteryCapacity); + + crsfSerialize8(dst, batteryRemainingPercentage); +} + +typedef enum { + CRSF_ACTIVE_ANTENNA1 = 0, + CRSF_ACTIVE_ANTENNA2 = 1, +} crsfActiveAntenna_e; + +typedef enum { + CRSF_RF_MODE_4_HZ = 0, + CRSF_RF_MODE_50_HZ = 1, + CRSF_RF_MODE_150_HZ = 2, +} crsrRfMode_e; + +typedef enum { + CRSF_RF_POWER_0_mW = 0, + CRSF_RF_POWER_10_mW = 1, + CRSF_RF_POWER_25_mW = 2, + CRSF_RF_POWER_100_mW = 3, + CRSF_RF_POWER_500_mW = 4, + CRSF_RF_POWER_1000_mW = 5, + CRSF_RF_POWER_2000_mW = 6, +} crsrRfPower_e; + +/* +0x14 Link statistics +Uplink is the connection from the ground to the UAV and downlink the opposite direction. +Payload: +uint8_t UplinkRSSI Ant.1(dBm*­1) +uint8_t UplinkRSSI Ant.2(dBm*­1) +uint8_t Uplink Package success rate / Link quality ( % ) +int8_t Uplink SNR ( db ) +uint8_t Diversity active antenna ( enum ant. 1 = 0, ant. 2 ) +uint8_t RF Mode ( enum 4fps = 0 , 50fps, 150hz) +uint8_t Uplink TX Power ( enum 0mW = 0, 10mW, 25 mW, 100 mW, 500 mW, 1000 mW, 2000mW ) +uint8_t Downlink RSSI ( dBm * ­-1 ) +uint8_t Downlink package success rate / Link quality ( % ) +int8_t Downlink SNR ( db ) +*/ + +void crsfFrameLinkStatistics(sbuf_t *dst) +{ + // use sbufWrite since CRC does not include frame length + sbufWriteU8(dst, CRSF_FRAME_LINK_STATISTICS_PAYLOAD_SIZE + CRSF_FRAME_LENGTH_TYPE_CRC); + crsfSerialize8(dst, CRSF_FRAMETYPE_LINK_STATISTICS); + crsfSerialize8(dst, 0); + crsfSerialize8(dst, 0); + crsfSerialize8(dst, 0); + crsfSerialize8(dst, 0); + crsfSerialize8(dst, 0); + crsfSerialize8(dst, 0); + crsfSerialize8(dst, 0); + crsfSerialize8(dst, 0); + crsfSerialize8(dst, 0); + crsfSerialize8(dst, 0); +} + +/* +0x1E Attitude +Payload: +int16_t Pitch angle ( rad / 10000 ) +int16_t Roll angle ( rad / 10000 ) +int16_t Yaw angle ( rad / 10000 ) +*/ + +#define DECIDEGREES_TO_RADIANS10000(angle) (1000.0f * (angle) * RAD) + +void crsfFrameAttitude(sbuf_t *dst) +{ + sbufWriteU8(dst, CRSF_FRAME_ATTITUDE_PAYLOAD_SIZE + CRSF_FRAME_LENGTH_TYPE_CRC); + crsfSerialize8(dst, CRSF_FRAMETYPE_ATTITUDE); + crsfSerialize16(dst, DECIDEGREES_TO_RADIANS10000(attitude.values.pitch)); + crsfSerialize16(dst, DECIDEGREES_TO_RADIANS10000(attitude.values.roll)); + crsfSerialize16(dst, DECIDEGREES_TO_RADIANS10000(attitude.values.yaw)); +} + +/* +0x21 Flight mode text based +Payload: +char[] Flight mode ( Null­terminated string ) +*/ +void crsfFrameFlightMode(sbuf_t *dst) +{ + // just do Angle for the moment as a placeholder + const char *flightMode = "Angle"; + // use sbufWrite since CRC does not include frame length + sbufWriteU8(dst, strlen(flightMode) + 1 + CRSF_FRAME_LENGTH_TYPE_CRC); + crsfSerialize8(dst, CRSF_FRAMETYPE_FLIGHT_MODE); + const int len = strlen(flightMode); + for (int ii = 0; ii< len; ++ii) { + crsfSerialize8(dst, flightMode[ii]); + } + crsfSerialize8(dst, 0); +} + +#define BV(x) (1 << (x)) // bit value + +// schedule array to decide how often each type of frame is sent +#define CRSF_SCHEDULE_COUNT 5 +static uint8_t crsfSchedule[CRSF_SCHEDULE_COUNT] = { + BV(CRSF_FRAME_ATTITUDE), + BV(CRSF_FRAME_BATTERY_SENSOR), + BV(CRSF_FRAME_LINK_STATISTICS), + BV(CRSF_FRAME_FLIGHT_MODE), + BV(CRSF_FRAME_GPS), +}; + +static void processCrsf(void) +{ + static uint8_t crsfScheduleIndex = 0; + const uint8_t currentSchedule = crsfSchedule[crsfScheduleIndex]; + + sbuf_t crsfPayloadBuf; + sbuf_t *dst = &crsfPayloadBuf; + + if (currentSchedule & BV(CRSF_FRAME_ATTITUDE)) { + crsfInitializeFrame(dst); + crsfFrameAttitude(dst); + crsfFinalize(dst); + } + if (currentSchedule & BV(CRSF_FRAME_BATTERY_SENSOR)) { + crsfInitializeFrame(dst); + crsfFrameBatterySensor(dst); + crsfFinalize(dst); + } + if (currentSchedule & BV(CRSF_FRAME_LINK_STATISTICS)) { + crsfInitializeFrame(dst); + crsfFrameLinkStatistics(dst); + crsfFinalize(dst); + } + if (currentSchedule & BV(CRSF_FRAME_FLIGHT_MODE)) { + crsfInitializeFrame(dst); + crsfFrameFlightMode(dst); + crsfFinalize(dst); + } +#ifdef GPS + if (currentSchedule & BV(CRSF_FRAME_GPS)) { + crsfInitializeFrame(dst); + crsfFrameGps(dst); + crsfFinalize(dst); + } +#endif + crsfScheduleIndex = (crsfScheduleIndex + 1) % CRSF_SCHEDULE_COUNT; +} + +void handleCrsfTelemetry(uint32_t currentTime) +{ + static uint32_t crsfLastCycleTime; + if (!crsfTelemetryEnabled) { + return; + } + if (!serialPort) { + return; + } + if ((currentTime - crsfLastCycleTime) >= CRSF_CYCLETIME_US) { + processCrsf(); + crsfLastCycleTime = currentTime; + } +} + +void freeCrsfTelemetryPort(void) +{ + closeSerialPort(serialPort); + serialPort = NULL; + crsfTelemetryEnabled = false; +} + +void initCrsfTelemetry(void) +{ + serialPortConfig = findSerialPortConfig(FUNCTION_TELEMETRY_CRSF); + portSharing = determinePortSharing(serialPortConfig, FUNCTION_TELEMETRY_CRSF); +} + +void configureCrsfTelemetryPort(void) +{ + if (!serialPortConfig) { + return; + } + serialPort = openSerialPort(serialPortConfig->identifier, FUNCTION_TELEMETRY_CRSF, NULL, CRSF_BAUDRATE, TELEMETRY_CRSF_INITIAL_PORT_MODE, CRSF_PORT_OPTIONS); + if (!serialPort) { + return; + } + crsfTelemetryEnabled = true; +} + +bool checkCrsfTelemetryState(void) +{ + const bool newTelemetryEnabled = telemetryDetermineEnabledState(portSharing); + if (newTelemetryEnabled == crsfTelemetryEnabled) { + return false; + } + if (newTelemetryEnabled) { + configureCrsfTelemetryPort(); + } else { + freeCrsfTelemetryPort(); + } + return true; +} + +int getCrsfFrame(uint8_t *frame, crsfFrameType_e frameType) +{ + sbuf_t crsfFrameBuf; + sbuf_t *sbuf = &crsfFrameBuf; + + crsfInitializeFrame(sbuf); + switch (frameType) { + default: + case CRSF_FRAME_ATTITUDE: + crsfFrameAttitude(sbuf); + break; + case CRSF_FRAME_BATTERY_SENSOR: + crsfFrameBatterySensor(sbuf); + break; + case CRSF_FRAME_LINK_STATISTICS: + crsfFrameLinkStatistics(sbuf); + break; + case CRSF_FRAME_FLIGHT_MODE: + crsfFrameFlightMode(sbuf); + break; +#if defined(GPS) + case CRSF_FRAME_GPS: + crsfFrameGps(sbuf); + break; +#endif + } + const int frameSize = crsfFinalizeBuf(sbuf, frame); + return frameSize; +} +#endif diff --git a/src/main/telemetry/crsf.h b/src/main/telemetry/crsf.h new file mode 100644 index 0000000000..fb7f4e9a9d --- /dev/null +++ b/src/main/telemetry/crsf.h @@ -0,0 +1,40 @@ +/* + * ltm.h + * + * This file is part of Cleanflight. + * + * Cleanflight is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * Cleanflight is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with Cleanflight. If not, see . + */ + +#pragma once + +typedef enum { + CRSF_FRAME_START = 0, + CRSF_FRAME_ATTITUDE = CRSF_FRAME_START, + CRSF_FRAME_BATTERY_SENSOR, + CRSF_FRAME_LINK_STATISTICS, + CRSF_FRAME_FLIGHT_MODE, + CRSF_FRAME_GPS, + CRSF_FRAME_COUNT +} crsfFrameType_e; + +void initCrsfTelemetry(void); +void handleCrsfTelemetry(uint32_t currentTime); +bool checkCrsfTelemetryState(void); + +void freeCrsfTelemetryPort(void); +void configureCrsfTelemetryPort(void); + +int getCrsfFrame(uint8_t *frame, crsfFrameType_e frameType); + diff --git a/src/main/telemetry/telemetry.c b/src/main/telemetry/telemetry.c index 305d4df229..6b59e72d1d 100644 --- a/src/main/telemetry/telemetry.c +++ b/src/main/telemetry/telemetry.c @@ -44,6 +44,7 @@ #include "telemetry/ltm.h" #include "telemetry/jetiexbus.h" #include "telemetry/mavlink.h" +#include "telemetry/crsf.h" static telemetryConfig_t *telemetryConfig; @@ -72,6 +73,9 @@ void telemetryInit(void) #ifdef TELEMETRY_MAVLINK initMAVLinkTelemetry(); #endif +#ifdef TELEMETRY_CRSF + initCrsfTelemetry(); +#endif telemetryCheckState(); } @@ -117,6 +121,9 @@ void telemetryCheckState(void) #ifdef TELEMETRY_MAVLINK checkMAVLinkTelemetryState(); #endif +#ifdef TELEMETRY_CRSF + checkCrsfTelemetryState(); +#endif } void telemetryProcess(uint32_t currentTime, rxConfig_t *rxConfig, uint16_t deadband3d_throttle) @@ -144,6 +151,9 @@ void telemetryProcess(uint32_t currentTime, rxConfig_t *rxConfig, uint16_t deadb #ifdef TELEMETRY_MAVLINK handleMAVLinkTelemetry(); #endif +#ifdef TELEMETRY_CRSF + handleCrsfTelemetry(currentTime); +#endif } #endif diff --git a/src/test/Makefile b/src/test/Makefile index d8eb183f34..c65f812780 100644 --- a/src/test/Makefile +++ b/src/test/Makefile @@ -141,6 +141,15 @@ $(OBJECT_DIR)/common_filter_unittest : \ $(CXX) $(CXX_FLAGS) $^ -o $(OBJECT_DIR)/$@ +$(OBJECT_DIR)/common/streambuf.o : \ + $(USER_DIR)/common/streambuf.c \ + $(USER_DIR)/common/streambuf.h \ + $(GTEST_HEADERS) + + @mkdir -p $(dir $@) + $(CC) $(C_FLAGS) $(TEST_CFLAGS) -D'__TARGET__="TEST"' -D'__REVISION__="revision"' -c $(USER_DIR)/common/streambuf.c -o $@ + + $(OBJECT_DIR)/drivers/io.o : \ $(USER_DIR)/drivers/io.c \ $(USER_DIR)/drivers/io.h \ @@ -149,6 +158,16 @@ $(OBJECT_DIR)/drivers/io.o : \ @mkdir -p $(dir $@) $(CC) $(C_FLAGS) $(TEST_CFLAGS) -c $(USER_DIR)/drivers/io.c -o $@ + +$(OBJECT_DIR)/fc/runtime_config.o : \ + $(USER_DIR)/fc/runtime_config.c \ + $(USER_DIR)/fc/runtime_config.h \ + $(GTEST_HEADERS) + + @mkdir -p $(dir $@) + $(CC) $(C_FLAGS) $(TEST_CFLAGS) -D'FLASH_SIZE = 128' -DSTM32F10X_MD -c $(USER_DIR)/fc/runtime_config.c -o $@ + + $(OBJECT_DIR)/sensors/battery.o : $(USER_DIR)/sensors/battery.c $(USER_DIR)/sensors/battery.h $(GTEST_HEADERS) @mkdir -p $(dir $@) $(CC) $(C_FLAGS) $(TEST_CFLAGS) -c $(USER_DIR)/sensors/battery.c -o $@ @@ -471,6 +490,70 @@ $(OBJECT_DIR)/rx_rx_unittest : \ $(CXX) $(CXX_FLAGS) $^ -o $(OBJECT_DIR)/$@ +$(OBJECT_DIR)/telemetry/crsf.o : \ + $(USER_DIR)/telemetry/crsf.c \ + $(USER_DIR)/telemetry/crsf.h \ + $(USER_DIR)/rx/crsf.h \ + $(GTEST_HEADERS) + + @mkdir -p $(dir $@) + $(CC) $(C_FLAGS) $(TEST_CFLAGS) -c $(USER_DIR)/telemetry/crsf.c -o $@ + +$(OBJECT_DIR)/telemetry_crsf_unittest.o : \ + $(TEST_DIR)/telemetry_crsf_unittest.cc \ + $(USER_DIR)/telemetry/crsf.h \ + $(USER_DIR)/telemetry/crsf.c \ + $(USER_DIR)/rx/crsf.c \ + $(USER_DIR)/rx/crsf.h \ + $(GTEST_HEADERS) + + @mkdir -p $(dir $@) + $(CXX) $(CXX_FLAGS) $(TEST_CFLAGS) -c $(TEST_DIR)/telemetry_crsf_unittest.cc -o $@ + +$(OBJECT_DIR)/telemetry_crsf_unittest : \ + $(OBJECT_DIR)/telemetry/crsf.o \ + $(OBJECT_DIR)/telemetry_crsf_unittest.o \ + $(OBJECT_DIR)/common/maths.o \ + $(OBJECT_DIR)/common/streambuf.o \ + $(OBJECT_DIR)/flight/gps_conversion.o \ + $(OBJECT_DIR)/fc/runtime_config.o \ + $(OBJECT_DIR)/gtest_main.a + + $(CXX) $(CXX_FLAGS) $^ -o $(OBJECT_DIR)/$@ + +$(OBJECT_DIR)/rx/crsf.o : \ + $(USER_DIR)/rx/crsf.c \ + $(USER_DIR)/rx/crsf.h \ + $(GTEST_HEADERS) + + @mkdir -p $(dir $@) + $(CC) $(C_FLAGS) $(TEST_CFLAGS) -c $(USER_DIR)/rx/crsf.c -o $@ + +$(OBJECT_DIR)/telemetry/crsf.o : \ + $(USER_DIR)/telemetry/crsf.c \ + $(USER_DIR)/telemetry/crsf.h \ + $(GTEST_HEADERS) + + @mkdir -p $(dir $@) + $(CC) $(C_FLAGS) $(TEST_CFLAGS) -c $(USER_DIR)/telemetry/crsf.c -o $@ + +$(OBJECT_DIR)/rx_crsf_unittest.o : \ + $(TEST_DIR)/rx_crsf_unittest.cc \ + $(USER_DIR)/rx/crsf.h \ + $(USER_DIR)/rx/crsf.c \ + $(GTEST_HEADERS) + + @mkdir -p $(dir $@) + $(CXX) $(CXX_FLAGS) $(TEST_CFLAGS) -c $(TEST_DIR)/rx_crsf_unittest.cc -o $@ + +$(OBJECT_DIR)/rx_crsf_unittest : \ + $(OBJECT_DIR)/rx/crsf.o \ + $(OBJECT_DIR)/common/maths.o \ + $(OBJECT_DIR)/rx_crsf_unittest.o \ + $(OBJECT_DIR)/gtest_main.a + + $(CXX) $(CXX_FLAGS) $^ -o $(OBJECT_DIR)/$@ + $(OBJECT_DIR)/rx_ranges_unittest.o : \ $(TEST_DIR)/rx_ranges_unittest.cc \ $(USER_DIR)/rx/rx.h \ diff --git a/src/test/unit/platform.h b/src/test/unit/platform.h index 5e983c9687..4b0e458c9f 100644 --- a/src/test/unit/platform.h +++ b/src/test/unit/platform.h @@ -52,6 +52,11 @@ typedef struct void *test; } GPIO_TypeDef; +typedef struct +{ + void* test; +} TIM_TypeDef; + typedef struct { void* test; } DMA_Channel_TypeDef; diff --git a/src/test/unit/rx_crsf_unittest.cc b/src/test/unit/rx_crsf_unittest.cc new file mode 100644 index 0000000000..2d70a6eef2 --- /dev/null +++ b/src/test/unit/rx_crsf_unittest.cc @@ -0,0 +1,99 @@ +/* + * This file is part of Cleanflight. + * + * Cleanflight is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * Cleanflight is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with Cleanflight. If not, see . + */ + +#include +#include + +#include +#include + +extern "C" { + #include + + #include "config/parameter_group.h" + #include "config/parameter_group_ids.h" + + #include "rx/rx.h" + #include "fc/rc_controls.h" + #include "common/maths.h" + #include "common/utils.h" + +} + +#include "unittest_macros.h" +#include "gtest/gtest.h" + +// CRC8 implementation with polynom = x^8+x^7+x^6+x^4+x^2+1 (0xD5) +unsigned char crc8tab[256] = { + 0x00, 0xD5, 0x7F, 0xAA, 0xFE, 0x2B, 0x81, 0x54, 0x29, 0xFC, 0x56, 0x83, 0xD7, 0x02, 0xA8, 0x7D, + 0x52, 0x87, 0x2D, 0xF8, 0xAC, 0x79, 0xD3, 0x06, 0x7B, 0xAE, 0x04, 0xD1, 0x85, 0x50, 0xFA, 0x2F, + 0xA4, 0x71, 0xDB, 0x0E, 0x5A, 0x8F, 0x25, 0xF0, 0x8D, 0x58, 0xF2, 0x27, 0x73, 0xA6, 0x0C, 0xD9, + 0xF6, 0x23, 0x89, 0x5C, 0x08, 0xDD, 0x77, 0xA2, 0xDF, 0x0A, 0xA0, 0x75, 0x21, 0xF4, 0x5E, 0x8B, + 0x9D, 0x48, 0xE2, 0x37, 0x63, 0xB6, 0x1C, 0xC9, 0xB4, 0x61, 0xCB, 0x1E, 0x4A, 0x9F, 0x35, 0xE0, + 0xCF, 0x1A, 0xB0, 0x65, 0x31, 0xE4, 0x4E, 0x9B, 0xE6, 0x33, 0x99, 0x4C, 0x18, 0xCD, 0x67, 0xB2, + 0x39, 0xEC, 0x46, 0x93, 0xC7, 0x12, 0xB8, 0x6D, 0x10, 0xC5, 0x6F, 0xBA, 0xEE, 0x3B, 0x91, 0x44, + 0x6B, 0xBE, 0x14, 0xC1, 0x95, 0x40, 0xEA, 0x3F, 0x42, 0x97, 0x3D, 0xE8, 0xBC, 0x69, 0xC3, 0x16, + 0xEF, 0x3A, 0x90, 0x45, 0x11, 0xC4, 0x6E, 0xBB, 0xC6, 0x13, 0xB9, 0x6C, 0x38, 0xED, 0x47, 0x92, + 0xBD, 0x68, 0xC2, 0x17, 0x43, 0x96, 0x3C, 0xE9, 0x94, 0x41, 0xEB, 0x3E, 0x6A, 0xBF, 0x15, 0xC0, + 0x4B, 0x9E, 0x34, 0xE1, 0xB5, 0x60, 0xCA, 0x1F, 0x62, 0xB7, 0x1D, 0xC8, 0x9C, 0x49, 0xE3, 0x36, + 0x19, 0xCC, 0x66, 0xB3, 0xE7, 0x32, 0x98, 0x4D, 0x30, 0xE5, 0x4F, 0x9A, 0xCE, 0x1B, 0xB1, 0x64, + 0x72, 0xA7, 0x0D, 0xD8, 0x8C, 0x59, 0xF3, 0x26, 0x5B, 0x8E, 0x24, 0xF1, 0xA5, 0x70, 0xDA, 0x0F, + 0x20, 0xF5, 0x5F, 0x8A, 0xDE, 0x0B, 0xA1, 0x74, 0x09, 0xDC, 0x76, 0xA3, 0xF7, 0x22, 0x88, 0x5D, + 0xD6, 0x03, 0xA9, 0x7C, 0x28, 0xFD, 0x57, 0x82, 0xFF, 0x2A, 0x80, 0x55, 0x01, 0xD4, 0x7E, 0xAB, + 0x84, 0x51, 0xFB, 0x2E, 0x7A, 0xAF, 0x05, 0xD0, 0xAD, 0x78, 0xD2, 0x07, 0x53, 0x86, 0x2C, 0xF9 +}; + +uint8_t crc8_buf(const uint8_t * ptr, uint8_t len) +{ + uint8_t crc = 0; + for (uint8_t i=0; i. + */ + +#include +#include +#include + +#include + +extern "C" { + #include "build/debug.h" + + #include + + #include "common/axis.h" + #include "common/filter.h" + #include "common/maths.h" + + #include "config/parameter_group.h" + #include "config/parameter_group_ids.h" + + #include "drivers/system.h" + #include "drivers/serial.h" + + #include "fc/runtime_config.h" + + #include "io/gps.h" + #include "io/serial.h" + + #include "rx/crsf.h" + + #include "sensors/sensors.h" + #include "sensors/battery.h" + + #include "telemetry/telemetry.h" + #include "telemetry/crsf.h" + + #include "flight/pid.h" + #include "flight/imu.h" + #include "flight/gps_conversion.h" + +} + +#include "unittest_macros.h" +#include "gtest/gtest.h" + +uint8_t crfsCrc(uint8_t *frame, int frameLen) +{ + uint8_t crc = 0; + for (int ii = 2; ii < frameLen - 1; ++ii) { + crc = crc8_dvb_s2(crc, frame[ii]); + } + return crc; +} +/* +int32_t Latitude ( degree / 10`000`000 ) +int32_t Longitude (degree / 10`000`000 ) +uint16_t Groundspeed ( km/h / 100 ) +uint16_t GPS heading ( degree / 100 ) +uint16 Altitude ( meter ­ 1000m offset ) +uint8_t Satellites in use ( counter ) +uint8_t GPS_numSat; +int32_t GPS_coord[2]; +uint16_t GPS_distanceToHome; // distance to home point in meters +uint16_t GPS_altitude; // altitude in m +uint16_t GPS_speed; // speed in 0.1m/s +uint16_t GPS_ground_course = 0; // degrees * 10 + + */ +TEST(TelemetryCrsfTest, TestGPS) +{ + uint8_t frame[CRSF_FRAME_SIZE_MAX]; + + int frameLen = getCrsfFrame(frame, CRSF_FRAME_GPS); + EXPECT_EQ(CRSF_FRAME_GPS_PAYLOAD_SIZE + 4, frameLen); + EXPECT_EQ(CRSF_RECEIVER_ADDRESS, frame[0]); // address + EXPECT_EQ(17, frame[1]); // length + EXPECT_EQ(0x02, frame[2]); // type + int32_t lattitude = frame[3] << 24 | frame[4] << 16 | frame[5] << 8 | frame[6]; + EXPECT_EQ(0, lattitude); + int32_t longitude = frame[7] << 24 | frame[8] << 16 | frame[9] << 8 | frame[10]; + EXPECT_EQ(0, longitude); + uint16_t groundSpeed = frame[11] << 8 | frame[12]; + EXPECT_EQ(0, groundSpeed); + uint16_t GPSheading = frame[13] << 8 | frame[14]; + EXPECT_EQ(0, GPSheading); + uint16_t altitude = frame[15] << 8 | frame[16]; + EXPECT_EQ(1000, altitude); + uint8_t satelliteCount = frame[17]; + EXPECT_EQ(0, satelliteCount); + EXPECT_EQ(crfsCrc(frame, frameLen), frame[18]); + + GPS_coord[LAT] = 56 * GPS_DEGREES_DIVIDER; + GPS_coord[LON] = 163 * GPS_DEGREES_DIVIDER; + ENABLE_STATE(GPS_FIX); + GPS_altitude = 2345; // altitude in m + GPS_speed = 163; // speed in 0.1m/s, 16.3 m/s = 58.68 km/h + GPS_numSat = 9; + GPS_ground_course = 1479; // degrees * 10 + frameLen = getCrsfFrame(frame, CRSF_FRAME_GPS); + lattitude = frame[3] << 24 | frame[4] << 16 | frame[5] << 8 | frame[6]; + EXPECT_EQ(560000000, lattitude); + longitude = frame[7] << 24 | frame[8] << 16 | frame[9] << 8 | frame[10]; + EXPECT_EQ(1630000000, longitude); + groundSpeed = frame[11] << 8 | frame[12]; + EXPECT_EQ(5868, groundSpeed); + GPSheading = frame[13] << 8 | frame[14]; + EXPECT_EQ(14790, GPSheading); + altitude = frame[15] << 8 | frame[16]; + EXPECT_EQ(3345, altitude); + satelliteCount = frame[17]; + EXPECT_EQ(9, satelliteCount); + EXPECT_EQ(crfsCrc(frame, frameLen), frame[18]); +} + +TEST(TelemetryCrsfTest, TestBattery) +{ + uint8_t frame[CRSF_FRAME_SIZE_MAX]; + batteryConfig_t workingBatteryConfig; + + batteryConfig = &workingBatteryConfig; + memset(batteryConfig, 0, sizeof(batteryConfig_t)); + vbat = 0; // 0.1V units + int frameLen = getCrsfFrame(frame, CRSF_FRAME_BATTERY_SENSOR); + EXPECT_EQ(CRSF_FRAME_BATTERY_SENSOR_PAYLOAD_SIZE + 4, frameLen); + EXPECT_EQ(CRSF_RECEIVER_ADDRESS, frame[0]); // address + EXPECT_EQ(10, frame[1]); // length + EXPECT_EQ(0x08, frame[2]); // type + uint16_t voltage = frame[3] << 8 | frame[4]; // mV * 100 + EXPECT_EQ(0, voltage); + uint16_t current = frame[5] << 8 | frame[6]; // mA * 100 + EXPECT_EQ(0, current); + uint32_t capacity = frame[7] << 16 | frame[8] << 8 | frame [9]; // mAh + EXPECT_EQ(0, capacity); + uint16_t remaining = frame[10]; // percent + EXPECT_EQ(67, remaining); + EXPECT_EQ(crfsCrc(frame, frameLen), frame[11]); + + vbat = 33; // 3.3V = 3300 mv + amperage = 2960; // = 29.60A = 29600mA - amperage is in 0.01A steps + batteryConfig->batteryCapacity = 1234; + frameLen = getCrsfFrame(frame, CRSF_FRAME_BATTERY_SENSOR); + voltage = frame[3] << 8 | frame[4]; // mV * 100 + EXPECT_EQ(33, voltage); + current = frame[5] << 8 | frame[6]; // mA * 100 + EXPECT_EQ(296, current); + capacity = frame[7] << 16 | frame[8] << 8 | frame [9]; // mAh + EXPECT_EQ(1234, capacity); + remaining = frame[10]; // percent + EXPECT_EQ(67, remaining); + EXPECT_EQ(crfsCrc(frame, frameLen), frame[11]); +} + +TEST(TelemetryCrsfTest, TestLinkStatistics) +{ + uint8_t frame[CRSF_FRAME_SIZE_MAX]; + + int frameLen = getCrsfFrame(frame, CRSF_FRAME_LINK_STATISTICS); + EXPECT_EQ(CRSF_FRAME_LINK_STATISTICS_PAYLOAD_SIZE + 4, frameLen); + EXPECT_EQ(CRSF_RECEIVER_ADDRESS, frame[0]); // address + EXPECT_EQ(12, frame[1]); // length + EXPECT_EQ(0x14, frame[2]); // type + EXPECT_EQ(crfsCrc(frame, frameLen), frame[13]); +} + +TEST(TelemetryCrsfTest, TestAttitude) +{ + uint8_t frame[CRSF_FRAME_SIZE_MAX]; + + attitude.values.pitch = 0; + attitude.values.roll = 0; + attitude.values.yaw = 0; + int frameLen = getCrsfFrame(frame, CRSF_FRAME_ATTITUDE); + EXPECT_EQ(CRSF_FRAME_ATTITUDE_PAYLOAD_SIZE + 4, frameLen); + EXPECT_EQ(CRSF_RECEIVER_ADDRESS, frame[0]); // address + EXPECT_EQ(8, frame[1]); // length + EXPECT_EQ(0x1e, frame[2]); // type + int16_t pitch = frame[3] << 8 | frame[4]; // rad / 10000 + EXPECT_EQ(0, pitch); + int16_t roll = frame[5] << 8 | frame[6]; + EXPECT_EQ(0, roll); + int16_t yaw = frame[7] << 8 | frame[8]; + EXPECT_EQ(0, yaw); + EXPECT_EQ(crfsCrc(frame, frameLen), frame[9]); + + attitude.values.pitch = 678; // decidegrees == 1.183333232852155 rad + attitude.values.roll = 1495; // 2.609267231731523 rad + attitude.values.yaw = -1799; //3.139847324337799 rad + frameLen = getCrsfFrame(frame, CRSF_FRAME_ATTITUDE); + pitch = frame[3] << 8 | frame[4]; // rad / 10000 + EXPECT_EQ(11833, pitch); + roll = frame[5] << 8 | frame[6]; + EXPECT_EQ(26092, roll); + yaw = frame[7] << 8 | frame[8]; + EXPECT_EQ(-31398, yaw); + EXPECT_EQ(crfsCrc(frame, frameLen), frame[9]); +} + +TEST(TelemetryCrsfTest, TestFlightMode) +{ + uint8_t frame[CRSF_FRAME_SIZE_MAX]; + + int frameLen = getCrsfFrame(frame, CRSF_FRAME_FLIGHT_MODE); + EXPECT_EQ(6 + 4, frameLen); + enableFlightMode(ANGLE_MODE); + EXPECT_EQ(1, FLIGHT_MODE(ANGLE_MODE)); + EXPECT_EQ(CRSF_RECEIVER_ADDRESS, frame[0]); // address + EXPECT_EQ(8, frame[1]); // length + EXPECT_EQ(0x21, frame[2]); // type + EXPECT_EQ('A', frame[3]); // type + EXPECT_EQ('n', frame[4]); // type + EXPECT_EQ('g', frame[5]); // type + EXPECT_EQ('l', frame[6]); // type + EXPECT_EQ('e', frame[7]); // type + EXPECT_EQ(0, frame[8]); // type + EXPECT_EQ(crfsCrc(frame, frameLen), frame[9]); +} + +// STUBS + +extern "C" { + +int16_t debug[DEBUG16_VALUE_COUNT]; + +const uint32_t baudRates[] = {0, 9600, 19200, 38400, 57600, 115200, 230400, 250000, 400000}; // see baudRate_e + +uint16_t batteryWarningVoltage; +uint8_t useHottAlarmSoundPeriod (void) { return 0; } + +attitudeEulerAngles_t attitude = { { 0, 0, 0 } }; // absolute angle inclination in multiple of 0.1 degree 180 deg = 1800 + +uint8_t GPS_numSat; +int32_t GPS_coord[2]; +uint16_t GPS_distanceToHome; // distance to home point in meters +uint16_t GPS_altitude; // altitude in m +uint16_t GPS_speed; // speed in 0.1m/s +uint16_t GPS_ground_course = 0; // degrees * 10 +uint16_t vbat; + +int32_t amperage; +int32_t mAhDrawn; + +void beeperConfirmationBeeps(uint8_t beepCount) {UNUSED(beepCount);} + +uint32_t serialRxBytesWaiting(const serialPort_t *instance) { + UNUSED(instance); + return 0; +} + +uint32_t serialTxBytesFree(const serialPort_t *instance) { + UNUSED(instance); + return 0; +} + +uint8_t serialRead(serialPort_t *instance) { + UNUSED(instance); + return 0; +} + +void serialWrite(serialPort_t *instance, uint8_t ch) { + UNUSED(instance); + UNUSED(ch); +} +void serialWriteBuf(serialPort_t *instance, const uint8_t *data, int count) { + UNUSED(instance); + UNUSED(data); + UNUSED(count); +} + +void serialSetMode(serialPort_t *instance, portMode_t mode) { + UNUSED(instance); + UNUSED(mode); +} + + +serialPort_t *openSerialPort(serialPortIdentifier_e identifier, serialPortFunction_e functionMask, serialReceiveCallbackPtr callback, uint32_t baudRate, portMode_t mode, portOptions_t options) { + UNUSED(identifier); + UNUSED(functionMask); + UNUSED(baudRate); + UNUSED(callback); + UNUSED(mode); + UNUSED(options); + + return NULL; +} + +void closeSerialPort(serialPort_t *serialPort) { + UNUSED(serialPort); +} + +serialPortConfig_t *findSerialPortConfig(serialPortFunction_e function) { + UNUSED(function); + + return NULL; +} + +bool telemetryDetermineEnabledState(portSharing_e) { + return true; +} + +portSharing_e determinePortSharing(serialPortConfig_t *, serialPortFunction_e) { + return PORTSHARING_NOT_SHARED; +} + +uint8_t batteryCapacityRemainingPercentage(void) {return 67;} + +uint8_t calculateBatteryCapacityRemainingPercentage(void) {return 67;} + +batteryState_e getBatteryState(void) { + return BATTERY_OK; +} + +} +