mirror of
https://github.com/betaflight/betaflight.git
synced 2025-07-25 17:25:20 +03:00
Merge pull request #1504 from betaflight/bf_rx_crsf
Team Blacksheep Crossfire RX and telemetry
This commit is contained in:
J Blackman
GitHub
commit
3a772c447c
20 changed files with 1553 additions and 28 deletions
2
Makefile
2
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 \
|
||||
|
|
|
|||
|
|
@ -72,6 +72,9 @@
|
|||
#include "config/config_profile.h"
|
||||
#include "config/config_master.h"
|
||||
|
||||
#define TASK_PERIOD_HZ(hz) (1000000 / (hz))
|
||||
#define TASK_PERIOD_MS(ms) ((ms) * 1000)
|
||||
#define TASK_PERIOD_US(us) (us)
|
||||
|
||||
/* VBAT monitoring interval (in microseconds) - 1s*/
|
||||
#define VBATINTERVAL (6 * 3500)
|
||||
|
|
@ -230,7 +233,7 @@ void fcTasksInit(void)
|
|||
setTaskEnabled(TASK_COMPASS, sensors(SENSOR_MAG));
|
||||
#if defined(USE_SPI) && defined(USE_MAG_AK8963)
|
||||
// fixme temporary solution for AK6983 via slave I2C on MPU9250
|
||||
rescheduleTask(TASK_COMPASS, 1000000 / 40);
|
||||
rescheduleTask(TASK_COMPASS, TASK_PERIOD_HZ(40));
|
||||
#endif
|
||||
#endif
|
||||
#ifdef BARO
|
||||
|
|
@ -247,9 +250,14 @@ void fcTasksInit(void)
|
|||
#endif
|
||||
#ifdef TELEMETRY
|
||||
setTaskEnabled(TASK_TELEMETRY, feature(FEATURE_TELEMETRY));
|
||||
if (feature(FEATURE_TELEMETRY)) {
|
||||
if (masterConfig.rxConfig.serialrx_provider == SERIALRX_JETIEXBUS) {
|
||||
// Reschedule telemetry to 500hz for Jeti Exbus
|
||||
if (feature(FEATURE_TELEMETRY) || masterConfig.rxConfig.serialrx_provider == SERIALRX_JETIEXBUS) {
|
||||
rescheduleTask(TASK_TELEMETRY, 2000);
|
||||
rescheduleTask(TASK_TELEMETRY, TASK_PERIOD_HZ(500));
|
||||
} else if (masterConfig.rxConfig.serialrx_provider == SERIALRX_CRSF) {
|
||||
// Reschedule telemetry to 500hz, 2ms for CRSF
|
||||
rescheduleTask(TASK_TELEMETRY, TASK_PERIOD_HZ(500));
|
||||
}
|
||||
}
|
||||
#endif
|
||||
#ifdef LED_STRIP
|
||||
|
|
@ -280,7 +288,7 @@ cfTask_t cfTasks[TASK_COUNT] = {
|
|||
[TASK_SYSTEM] = {
|
||||
.taskName = "SYSTEM",
|
||||
.taskFunc = taskSystem,
|
||||
.desiredPeriod = 1000000 / 10, // run every 100 ms
|
||||
.desiredPeriod = TASK_PERIOD_HZ(10), // 10Hz, every 100 ms
|
||||
.staticPriority = TASK_PRIORITY_HIGH,
|
||||
},
|
||||
|
||||
|
|
@ -295,14 +303,14 @@ cfTask_t cfTasks[TASK_COUNT] = {
|
|||
[TASK_ACCEL] = {
|
||||
.taskName = "ACCEL",
|
||||
.taskFunc = taskUpdateAccelerometer,
|
||||
.desiredPeriod = 1000000 / 1000, // every 1ms
|
||||
.desiredPeriod = TASK_PERIOD_HZ(1000), // 1000Hz, every 1ms
|
||||
.staticPriority = TASK_PRIORITY_MEDIUM,
|
||||
},
|
||||
|
||||
[TASK_ATTITUDE] = {
|
||||
.taskName = "ATTITUDE",
|
||||
.taskFunc = imuUpdateAttitude,
|
||||
.desiredPeriod = 1000000 / 100,
|
||||
.desiredPeriod = TASK_PERIOD_HZ(100),
|
||||
.staticPriority = TASK_PRIORITY_MEDIUM,
|
||||
},
|
||||
|
||||
|
|
@ -310,21 +318,21 @@ cfTask_t cfTasks[TASK_COUNT] = {
|
|||
.taskName = "RX",
|
||||
.checkFunc = rxUpdateCheck,
|
||||
.taskFunc = taskUpdateRxMain,
|
||||
.desiredPeriod = 1000000 / 50, // If event-based scheduling doesn't work, fallback to periodic scheduling
|
||||
.desiredPeriod = TASK_PERIOD_HZ(50), // If event-based scheduling doesn't work, fallback to periodic scheduling
|
||||
.staticPriority = TASK_PRIORITY_HIGH,
|
||||
},
|
||||
|
||||
[TASK_SERIAL] = {
|
||||
.taskName = "SERIAL",
|
||||
.taskFunc = taskHandleSerial,
|
||||
.desiredPeriod = 1000000 / 100, // 100 Hz should be enough to flush up to 115 bytes @ 115200 baud
|
||||
.desiredPeriod = TASK_PERIOD_HZ(100), // 100 Hz should be enough to flush up to 115 bytes @ 115200 baud
|
||||
.staticPriority = TASK_PRIORITY_LOW,
|
||||
},
|
||||
|
||||
[TASK_BATTERY] = {
|
||||
.taskName = "BATTERY",
|
||||
.taskFunc = taskUpdateBattery,
|
||||
.desiredPeriod = 1000000 / 50, // 50 Hz
|
||||
.desiredPeriod = TASK_PERIOD_HZ(50), // 50 Hz
|
||||
.staticPriority = TASK_PRIORITY_MEDIUM,
|
||||
},
|
||||
|
||||
|
|
@ -332,7 +340,7 @@ cfTask_t cfTasks[TASK_COUNT] = {
|
|||
[TASK_BEEPER] = {
|
||||
.taskName = "BEEPER",
|
||||
.taskFunc = beeperUpdate,
|
||||
.desiredPeriod = 1000000 / 100, // 100 Hz
|
||||
.desiredPeriod = TASK_PERIOD_HZ(100), // 100 Hz
|
||||
.staticPriority = TASK_PRIORITY_LOW,
|
||||
},
|
||||
#endif
|
||||
|
|
@ -341,7 +349,7 @@ cfTask_t cfTasks[TASK_COUNT] = {
|
|||
[TASK_GPS] = {
|
||||
.taskName = "GPS",
|
||||
.taskFunc = gpsUpdate,
|
||||
.desiredPeriod = 1000000 / 10, // GPS usually don't go raster than 10Hz
|
||||
.desiredPeriod = TASK_PERIOD_HZ(10), // GPS usually don't go raster than 10Hz
|
||||
.staticPriority = TASK_PRIORITY_MEDIUM,
|
||||
},
|
||||
#endif
|
||||
|
|
@ -350,7 +358,7 @@ cfTask_t cfTasks[TASK_COUNT] = {
|
|||
[TASK_COMPASS] = {
|
||||
.taskName = "COMPASS",
|
||||
.taskFunc = taskUpdateCompass,
|
||||
.desiredPeriod = 1000000 / 10, // Compass is updated at 10 Hz
|
||||
.desiredPeriod = TASK_PERIOD_HZ(10), // Compass is updated at 10 Hz
|
||||
.staticPriority = TASK_PRIORITY_LOW,
|
||||
},
|
||||
#endif
|
||||
|
|
@ -359,7 +367,7 @@ cfTask_t cfTasks[TASK_COUNT] = {
|
|||
[TASK_BARO] = {
|
||||
.taskName = "BARO",
|
||||
.taskFunc = taskUpdateBaro,
|
||||
.desiredPeriod = 1000000 / 20,
|
||||
.desiredPeriod = TASK_PERIOD_HZ(20),
|
||||
.staticPriority = TASK_PRIORITY_LOW,
|
||||
},
|
||||
#endif
|
||||
|
|
@ -368,7 +376,7 @@ cfTask_t cfTasks[TASK_COUNT] = {
|
|||
[TASK_SONAR] = {
|
||||
.taskName = "SONAR",
|
||||
.taskFunc = sonarUpdate,
|
||||
.desiredPeriod = 70000, // 70ms required so that SONAR pulses do not interfer with each other
|
||||
.desiredPeriod = TASK_PERIOD_MS(70), // 70ms required so that SONAR pulses do not interfer with each other
|
||||
.staticPriority = TASK_PRIORITY_LOW,
|
||||
},
|
||||
#endif
|
||||
|
|
@ -377,7 +385,7 @@ cfTask_t cfTasks[TASK_COUNT] = {
|
|||
[TASK_ALTITUDE] = {
|
||||
.taskName = "ALTITUDE",
|
||||
.taskFunc = taskCalculateAltitude,
|
||||
.desiredPeriod = 1000000 / 40,
|
||||
.desiredPeriod = TASK_PERIOD_HZ(40),
|
||||
.staticPriority = TASK_PRIORITY_LOW,
|
||||
},
|
||||
#endif
|
||||
|
|
@ -386,7 +394,7 @@ cfTask_t cfTasks[TASK_COUNT] = {
|
|||
[TASK_TRANSPONDER] = {
|
||||
.taskName = "TRANSPONDER",
|
||||
.taskFunc = transponderUpdate,
|
||||
.desiredPeriod = 1000000 / 250, // 250 Hz
|
||||
.desiredPeriod = TASK_PERIOD_HZ(250), // 250 Hz, 4ms
|
||||
.staticPriority = TASK_PRIORITY_LOW,
|
||||
},
|
||||
#endif
|
||||
|
|
@ -395,7 +403,7 @@ cfTask_t cfTasks[TASK_COUNT] = {
|
|||
[TASK_DASHBOARD] = {
|
||||
.taskName = "DASHBOARD",
|
||||
.taskFunc = dashboardUpdate,
|
||||
.desiredPeriod = 1000000 / 10,
|
||||
.desiredPeriod = TASK_PERIOD_HZ(10),
|
||||
.staticPriority = TASK_PRIORITY_LOW,
|
||||
},
|
||||
#endif
|
||||
|
|
@ -403,7 +411,7 @@ cfTask_t cfTasks[TASK_COUNT] = {
|
|||
[TASK_OSD] = {
|
||||
.taskName = "OSD",
|
||||
.taskFunc = osdUpdate,
|
||||
.desiredPeriod = 1000000 / 60, // 60 Hz
|
||||
.desiredPeriod = TASK_PERIOD_HZ(60), // 60 Hz
|
||||
.staticPriority = TASK_PRIORITY_LOW,
|
||||
},
|
||||
#endif
|
||||
|
|
@ -411,7 +419,7 @@ cfTask_t cfTasks[TASK_COUNT] = {
|
|||
[TASK_TELEMETRY] = {
|
||||
.taskName = "TELEMETRY",
|
||||
.taskFunc = taskTelemetry,
|
||||
.desiredPeriod = 1000000 / 250, // 250 Hz
|
||||
.desiredPeriod = TASK_PERIOD_HZ(250), // 250 Hz, 4ms
|
||||
.staticPriority = TASK_PRIORITY_LOW,
|
||||
},
|
||||
#endif
|
||||
|
|
@ -420,7 +428,7 @@ cfTask_t cfTasks[TASK_COUNT] = {
|
|||
[TASK_LEDSTRIP] = {
|
||||
.taskName = "LEDSTRIP",
|
||||
.taskFunc = ledStripUpdate,
|
||||
.desiredPeriod = 1000000 / 100, // 100 Hz
|
||||
.desiredPeriod = TASK_PERIOD_HZ(100), // 100 Hz, 10ms
|
||||
.staticPriority = TASK_PRIORITY_LOW,
|
||||
},
|
||||
#endif
|
||||
|
|
@ -429,7 +437,7 @@ cfTask_t cfTasks[TASK_COUNT] = {
|
|||
[TASK_BST_MASTER_PROCESS] = {
|
||||
.taskName = "BST_MASTER_PROCESS",
|
||||
.taskFunc = taskBstMasterProcess,
|
||||
.desiredPeriod = 1000000 / 50, // 50 Hz
|
||||
.desiredPeriod = TASK_PERIOD_HZ(50), // 50 Hz, 20ms
|
||||
.staticPriority = TASK_PRIORITY_IDLE,
|
||||
},
|
||||
#endif
|
||||
|
|
@ -447,7 +455,7 @@ cfTask_t cfTasks[TASK_COUNT] = {
|
|||
[TASK_CMS] = {
|
||||
.taskName = "CMS",
|
||||
.taskFunc = cmsHandler,
|
||||
.desiredPeriod = 1000000 / 60, // 60 Hz
|
||||
.desiredPeriod = TASK_PERIOD_HZ(60), // 60 Hz
|
||||
.staticPriority = TASK_PRIORITY_LOW,
|
||||
},
|
||||
#endif
|
||||
|
|
|
|||
|
|
@ -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);
|
||||
|
|
|
|||
|
|
@ -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)
|
||||
|
||||
|
|
|
|||
|
|
@ -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 {
|
||||
|
|
|
|||
|
|
@ -435,7 +435,8 @@ static const char * const lookupTableSerialRX[] = {
|
|||
"XB-B",
|
||||
"XB-B-RJ01",
|
||||
"IBUS",
|
||||
"JETIEXBUS"
|
||||
"JETIEXBUS",
|
||||
"CRSF"
|
||||
};
|
||||
#endif
|
||||
|
||||
|
|
|
|||
245
src/main/rx/crsf.c
Normal file
245
src/main/rx/crsf.c
Normal file
|
|
@ -0,0 +1,245 @@
|
|||
/*
|
||||
* 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 <http://www.gnu.org/licenses/>.
|
||||
*/
|
||||
|
||||
#include <stdbool.h>
|
||||
#include <stdint.h>
|
||||
#include <stdlib.h>
|
||||
#include <string.h>
|
||||
|
||||
#include "platform.h"
|
||||
|
||||
#ifdef SERIAL_RX
|
||||
|
||||
#include "build/build_config.h"
|
||||
#include "build/debug.h"
|
||||
|
||||
#include "common/maths.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"
|
||||
|
||||
#define CRSF_TIME_NEEDED_PER_FRAME_US 1000
|
||||
#define CRSF_TIME_BETWEEN_FRAMES_US 4000 // a frame is sent by the transmitter every 4 milliseconds
|
||||
|
||||
#define CRSF_DIGITAL_CHANNEL_MIN 172
|
||||
#define CRSF_DIGITAL_CHANNEL_MAX 1811
|
||||
|
||||
STATIC_UNIT_TESTED bool crsfFrameDone = false;
|
||||
STATIC_UNIT_TESTED crsfFrame_t crsfFrame;
|
||||
|
||||
STATIC_UNIT_TESTED uint32_t crsfChannelData[CRSF_MAX_CHANNEL];
|
||||
|
||||
static serialPort_t *serialPort;
|
||||
static uint32_t crsfFrameStartAt = 0;
|
||||
static uint8_t telemetryBuf[CRSF_FRAME_SIZE_MAX];
|
||||
static uint8_t telemetryBufLen = 0;
|
||||
|
||||
|
||||
/*
|
||||
* CRSF protocol
|
||||
*
|
||||
* CRSF protocol uses a single wire half duplex uart connection.
|
||||
* The master sends one frame every 4ms and the slave replies between two frames from the master.
|
||||
*
|
||||
* 420000 baud
|
||||
* not inverted
|
||||
* 8 Bit
|
||||
* 1 Stop bit
|
||||
* Big endian
|
||||
* 420000 bit/s = 46667 byte/s (including stop bit) = 21.43us per byte
|
||||
* Assume a max payload of 32 bytes (needs confirming with TBS), so max frame size of 36 bytes
|
||||
* A 36 byte frame can be transmitted in 771 microseconds.
|
||||
*
|
||||
* CRSF_TIME_NEEDED_PER_FRAME_US is set conservatively at 1000 microseconds
|
||||
*
|
||||
* Every frame has the structure:
|
||||
* <Device address> <Frame length> < Type> <Payload> < 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;
|
||||
|
||||
|
||||
// Receive ISR callback, called back from serial port
|
||||
STATIC_UNIT_TESTED void crsfDataReceive(uint16_t c)
|
||||
{
|
||||
static uint8_t crsfFramePosition = 0;
|
||||
const uint32_t now = micros();
|
||||
|
||||
#ifdef DEBUG_CRSF_PACKETS
|
||||
debug[2] = now - crsfFrameStartAt;
|
||||
#endif
|
||||
|
||||
if (now > crsfFrameStartAt + CRSF_TIME_NEEDED_PER_FRAME_US) {
|
||||
// We've received a character after max time needed to complete a frame,
|
||||
// so this must be the start of a new frame.
|
||||
crsfFramePosition = 0;
|
||||
}
|
||||
|
||||
if (crsfFramePosition == 0) {
|
||||
crsfFrameStartAt = now;
|
||||
}
|
||||
// assume frame is 5 bytes long until we have received the frame length
|
||||
// full frame length includes the length of the address and framelength fields
|
||||
const int fullFrameLength = crsfFramePosition < 3 ? 5 : crsfFrame.frame.frameLength + CRSF_FRAME_LENGTH_ADDRESS + CRSF_FRAME_LENGTH_FRAMELENGTH;
|
||||
|
||||
if (crsfFramePosition < fullFrameLength) {
|
||||
crsfFrame.bytes[crsfFramePosition++] = (uint8_t)c;
|
||||
crsfFrameDone = crsfFramePosition < fullFrameLength ? false : true;
|
||||
}
|
||||
}
|
||||
|
||||
STATIC_UNIT_TESTED uint8_t crsfFrameCRC(void)
|
||||
{
|
||||
// CRC includes type and payload
|
||||
uint8_t crc = crc8_dvb_s2(0, crsfFrame.frame.type);
|
||||
for (int ii = 0; ii < crsfFrame.frame.frameLength - CRSF_FRAME_LENGTH_TYPE_CRC; ++ii) {
|
||||
crc = crc8_dvb_s2(crc, crsfFrame.frame.payload[ii]);
|
||||
}
|
||||
return crc;
|
||||
}
|
||||
|
||||
STATIC_UNIT_TESTED uint8_t crsfFrameStatus(void)
|
||||
{
|
||||
if (crsfFrameDone) {
|
||||
crsfFrameDone = false;
|
||||
if (crsfFrame.frame.type == CRSF_FRAMETYPE_RC_CHANNELS_PACKED) {
|
||||
// CRC includes type and payload of each frame
|
||||
const uint8_t crc = crsfFrameCRC();
|
||||
if (crc != crsfFrame.frame.payload[CRSF_FRAME_RC_CHANNELS_PAYLOAD_SIZE]) {
|
||||
return RX_FRAME_PENDING;
|
||||
}
|
||||
crsfFrame.frame.frameLength = CRSF_FRAME_RC_CHANNELS_PAYLOAD_SIZE + CRSF_FRAME_LENGTH_TYPE_CRC;
|
||||
// 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_UNIT_TESTED 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;
|
||||
}
|
||||
|
||||
void crsfRxWriteTelemetryData(const void *data, int len)
|
||||
{
|
||||
len = MIN(len, (int)sizeof(telemetryBuf));
|
||||
memcpy(telemetryBuf, data, len);
|
||||
telemetryBufLen = len;
|
||||
}
|
||||
|
||||
void crsfRxSendTelemetryData(void)
|
||||
{
|
||||
// if there is telemetry data to write
|
||||
if (telemetryBufLen > 0) {
|
||||
// check that we are not currently receiving data (ie in the middle of an RX frame)
|
||||
// and that there is time to send the telemetry frame before the next RX frame arrives
|
||||
const uint32_t timeSinceStartOfFrame = micros() - crsfFrameStartAt;
|
||||
if ((timeSinceStartOfFrame > CRSF_TIME_NEEDED_PER_FRAME_US)
|
||||
&& (timeSinceStartOfFrame < CRSF_TIME_BETWEEN_FRAMES_US - CRSF_TIME_NEEDED_PER_FRAME_US)) {
|
||||
serialWriteBuf(serialPort, telemetryBuf, telemetryBufLen);
|
||||
telemetryBufLen = 0; // reset telemetry buffer
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
bool crsfRxInit(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->rcReadRawFn = crsfReadRawRC;
|
||||
rxRuntimeConfig->rcFrameStatusFn = crsfFrameStatus;
|
||||
|
||||
const serialPortConfig_t *portConfig = findSerialPortConfig(FUNCTION_RX_SERIAL);
|
||||
if (!portConfig) {
|
||||
return false;
|
||||
}
|
||||
|
||||
serialPort = openSerialPort(portConfig->identifier, FUNCTION_RX_SERIAL, crsfDataReceive, CRSF_BAUDRATE, CRSF_PORT_MODE, CRSF_PORT_OPTIONS);
|
||||
|
||||
return serialPort != NULL;
|
||||
}
|
||||
|
||||
bool crsfRxIsActive(void)
|
||||
{
|
||||
return serialPort != NULL;
|
||||
}
|
||||
#endif
|
||||
84
src/main/rx/crsf.h
Normal file
84
src/main/rx/crsf.h
Normal file
|
|
@ -0,0 +1,84 @@
|
|||
/*
|
||||
* 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 <http://www.gnu.org/licenses/>.
|
||||
*/
|
||||
|
||||
#pragma once
|
||||
|
||||
#define CRSF_BAUDRATE 420000
|
||||
#define CRSF_PORT_OPTIONS (SERIAL_STOPBITS_1 | SERIAL_PARITY_NO | SERIAL_BIDIR)
|
||||
#define CRSF_PORT_MODE MODE_RXTX
|
||||
|
||||
#define CRSF_MAX_CHANNEL 16
|
||||
|
||||
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_ADDRESS = 1, // length of ADDRESS field
|
||||
CRSF_FRAME_LENGTH_FRAMELENGTH = 1, // length of FRAMELENGTH field
|
||||
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
|
||||
};
|
||||
|
||||
enum {
|
||||
CRSF_ADDRESS_BROADCAST = 0x00,
|
||||
CRSF_ADDRESS_TBS_CORE_PNP_PRO = 0x8,
|
||||
CRSF_ADDRESS_RESERVED1 = 0x8A,
|
||||
CRSF_ADDRESS_CURRENT_SENSOR = 0xC0,
|
||||
CRSF_ADDRESS_TBS_BLACKBOX = 0xC4,
|
||||
CRSF_ADDRESS_COLIBRI_RACE_FC = 0xC8,
|
||||
CRSF_ADDRESS_RESERVED2 = 0xCA,
|
||||
CRSF_ADDRESS_RACE_TAG = 0xCC,
|
||||
CRSF_ADDRESS_RADIO_TRANSMITTER = 0xEA,
|
||||
CRSF_ADDRESS_CRSF_RECEIVER = 0xEC,
|
||||
CRSF_ADDRESS_CRSF_TRANSMITTER = 0xEE
|
||||
};
|
||||
|
||||
#define CRSF_PAYLOAD_SIZE_MAX 32 // !!TODO needs checking
|
||||
#define CRSF_FRAME_SIZE_MAX (CRSF_PAYLOAD_SIZE_MAX + 4)
|
||||
|
||||
typedef struct 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;
|
||||
|
||||
|
||||
void crsfRxWriteTelemetryData(const void *data, int len);
|
||||
void crsfRxSendTelemetryData(void);
|
||||
|
||||
struct rxConfig_s;
|
||||
struct rxRuntimeConfig_s;
|
||||
bool crsfRxInit(const struct rxConfig_s *initialRxConfig, struct rxRuntimeConfig_s *rxRuntimeConfig);
|
||||
bool crsfRxIsActive(void);
|
||||
|
|
@ -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,6 +188,11 @@ 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 = crsfRxInit(rxConfig, rxRuntimeConfig);
|
||||
break;
|
||||
#endif
|
||||
default:
|
||||
enabled = false;
|
||||
|
|
|
|||
|
|
@ -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;
|
||||
|
|
|
|||
|
|
@ -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
|
||||
|
|
|
|||
414
src/main/telemetry/crsf.c
Normal file
414
src/main/telemetry/crsf.c
Normal file
|
|
@ -0,0 +1,414 @@
|
|||
/*
|
||||
* 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 <http://www.gnu.org/licenses/>.
|
||||
*/
|
||||
|
||||
#include <stdbool.h>
|
||||
#include <stdint.h>
|
||||
#include <string.h>
|
||||
|
||||
#include "platform.h"
|
||||
|
||||
#ifdef TELEMETRY
|
||||
|
||||
#include "config/feature.h"
|
||||
#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 "common/streambuf.h"
|
||||
#include "common/utils.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/imu.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 CRSF_CYCLETIME_US 100000 // 100ms, 10 Hz
|
||||
|
||||
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_ADDRESS_BROADCAST);
|
||||
}
|
||||
|
||||
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 crsfSerializeData(sbuf_t *dst, const uint8_t *data, int len)
|
||||
{
|
||||
for (int ii = 0; ii< len; ++ii) {
|
||||
crsfSerialize8(dst, data[ii]);
|
||||
}
|
||||
}
|
||||
|
||||
static void crsfFinalize(sbuf_t *dst)
|
||||
{
|
||||
sbufWriteU8(dst, crsfCrc);
|
||||
sbufSwitchToReader(dst, crsfFrame);
|
||||
// write the telemetry frame to the receiver.
|
||||
crsfRxWriteTelemetryData(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> <Frame length> <Type> <Payload> <CRC>
|
||||
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 ( Nullterminated string )
|
||||
*/
|
||||
void crsfFrameFlightMode(sbuf_t *dst)
|
||||
{
|
||||
// just do Angle for the moment as a placeholder
|
||||
// write zero for frame length, since we don't know it yet
|
||||
uint8_t *lengthPtr = sbufPtr(dst);
|
||||
sbufWriteU8(dst, 0);
|
||||
crsfSerialize8(dst, CRSF_FRAMETYPE_FLIGHT_MODE);
|
||||
|
||||
// use same logic as OSD, so telemetry displays same flight text as OSD
|
||||
const char *flightMode = "ACRO";
|
||||
if (isAirmodeActive()) {
|
||||
flightMode = "AIR";
|
||||
}
|
||||
if (FLIGHT_MODE(FAILSAFE_MODE)) {
|
||||
flightMode = "!FS";
|
||||
} else if (FLIGHT_MODE(ANGLE_MODE)) {
|
||||
flightMode = "STAB";
|
||||
} else if (FLIGHT_MODE(HORIZON_MODE)) {
|
||||
flightMode = "HOR";
|
||||
}
|
||||
crsfSerializeData(dst, (const uint8_t*)flightMode, strlen(flightMode));
|
||||
crsfSerialize8(dst, 0); // zero terminator for string
|
||||
// write in the length
|
||||
*lengthPtr = sbufPtr(dst) - lengthPtr;
|
||||
}
|
||||
|
||||
#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 initCrsfTelemetry(void)
|
||||
{
|
||||
// check if there is a serial port open for CRSF telemetry (ie opened by the CRSF RX)
|
||||
// and feature is enabled, if so, set CRSF telemetry enabled
|
||||
crsfTelemetryEnabled = crsfRxIsActive();
|
||||
}
|
||||
|
||||
bool checkCrsfTelemetryState(void)
|
||||
{
|
||||
return crsfTelemetryEnabled;
|
||||
}
|
||||
|
||||
/*
|
||||
* Called periodically by the scheduler
|
||||
*/
|
||||
void handleCrsfTelemetry(uint32_t currentTime)
|
||||
{
|
||||
static uint32_t crsfLastCycleTime;
|
||||
|
||||
if (!crsfTelemetryEnabled) {
|
||||
return;
|
||||
}
|
||||
// Give the receiver a chance to send any outstanding telemetry data.
|
||||
// This needs to be done at high frequency, to enable the RX to send the telemetry frame
|
||||
// in between the RX frames.
|
||||
crsfRxSendTelemetryData();
|
||||
|
||||
// Actual telemetry data only needs to be sent at a low frequency, ie 10Hz
|
||||
if (currentTime >= crsfLastCycleTime + CRSF_CYCLETIME_US) {
|
||||
crsfLastCycleTime = currentTime;
|
||||
processCrsf();
|
||||
}
|
||||
}
|
||||
|
||||
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
|
||||
37
src/main/telemetry/crsf.h
Normal file
37
src/main/telemetry/crsf.h
Normal file
|
|
@ -0,0 +1,37 @@
|
|||
/*
|
||||
* 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 <http://www.gnu.org/licenses/>.
|
||||
*/
|
||||
|
||||
#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);
|
||||
bool checkCrsfTelemetryState(void);
|
||||
void handleCrsfTelemetry(uint32_t currentTime);
|
||||
|
||||
int getCrsfFrame(uint8_t *frame, crsfFrameType_e frameType);
|
||||
|
||||
|
|
@ -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
|
||||
|
|
|
|||
|
|
@ -60,4 +60,3 @@ bool telemetryDetermineEnabledState(portSharing_e portSharing);
|
|||
void telemetryUseConfig(telemetryConfig_t *telemetryConfig);
|
||||
|
||||
#define TELEMETRY_SHAREABLE_PORT_FUNCTIONS_MASK (FUNCTION_TELEMETRY_FRSKY | FUNCTION_TELEMETRY_LTM)
|
||||
|
||||
|
|
|
|||
|
|
@ -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,63 @@ $(OBJECT_DIR)/rx_rx_unittest : \
|
|||
|
||||
$(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)/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)/rx_crsf_unittest.o \
|
||||
$(OBJECT_DIR)/common/maths.o \
|
||||
$(OBJECT_DIR)/gtest_main.a
|
||||
|
||||
$(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)/rx/crsf.o \
|
||||
$(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_ranges_unittest.o : \
|
||||
$(TEST_DIR)/rx_ranges_unittest.cc \
|
||||
$(USER_DIR)/rx/rx.h \
|
||||
|
|
|
|||
|
|
@ -52,6 +52,11 @@ typedef struct
|
|||
void *test;
|
||||
} GPIO_TypeDef;
|
||||
|
||||
typedef struct
|
||||
{
|
||||
void* test;
|
||||
} TIM_TypeDef;
|
||||
|
||||
typedef struct {
|
||||
void* test;
|
||||
} DMA_Channel_TypeDef;
|
||||
|
|
@ -60,6 +65,11 @@ uint8_t DMA_GetFlagStatus(void *);
|
|||
void DMA_Cmd(DMA_Channel_TypeDef*, FunctionalState );
|
||||
void DMA_ClearFlag(uint32_t);
|
||||
|
||||
typedef struct
|
||||
{
|
||||
void* test;
|
||||
} USART_TypeDef;
|
||||
|
||||
#define WS2811_DMA_TC_FLAG (void *)1
|
||||
#define WS2811_DMA_HANDLER_IDENTIFER 0
|
||||
|
||||
|
|
|
|||
283
src/test/unit/rx_crsf_unittest.cc
Normal file
283
src/test/unit/rx_crsf_unittest.cc
Normal file
|
|
@ -0,0 +1,283 @@
|
|||
/*
|
||||
* 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 <http://www.gnu.org/licenses/>.
|
||||
*/
|
||||
|
||||
#include <stdint.h>
|
||||
#include <stdbool.h>
|
||||
|
||||
#include <limits.h>
|
||||
#include <algorithm>
|
||||
|
||||
extern "C" {
|
||||
#include <platform.h>
|
||||
|
||||
#include "build/debug.h"
|
||||
|
||||
#include "common/maths.h"
|
||||
#include "common/utils.h"
|
||||
|
||||
#include "io/serial.h"
|
||||
|
||||
#include "rx/rx.h"
|
||||
#include "rx/crsf.h"
|
||||
|
||||
void crsfDataReceive(uint16_t c);
|
||||
uint8_t crsfFrameCRC(void);
|
||||
uint8_t crsfFrameStatus(void);
|
||||
uint16_t crsfReadRawRC(const rxRuntimeConfig_t *rxRuntimeConfig, uint8_t chan);
|
||||
|
||||
extern bool crsfFrameDone;
|
||||
extern crsfFrame_t crsfFrame;
|
||||
extern uint32_t crsfChannelData[CRSF_MAX_CHANNEL];
|
||||
|
||||
uint32_t dummyTimeUs;
|
||||
}
|
||||
|
||||
#include "unittest_macros.h"
|
||||
#include "gtest/gtest.h"
|
||||
|
||||
// CRC8 implementation with polynom = x^8+x^7+x^6+x^4+x^2+1 (0xD5)
|
||||
const 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<len; i++) {
|
||||
crc = crc8tab[crc ^ *ptr++];
|
||||
}
|
||||
return crc;
|
||||
}
|
||||
|
||||
uint8_t crc8_dvb_s2_buf(const uint8_t * ptr, uint8_t len)
|
||||
{
|
||||
uint8_t crc = 0;
|
||||
for (uint8_t i=0; i<len; i++) {
|
||||
crc = crc8_dvb_s2(crc, *ptr++);
|
||||
}
|
||||
return crc;
|
||||
}
|
||||
|
||||
TEST(CrossFireTest, CRC)
|
||||
{
|
||||
static const uint8_t buf1[] ="abcdefghijklmnopqrstuvwxyz";
|
||||
|
||||
uint8_t crc1 = 0;
|
||||
uint8_t crc2 = 0;
|
||||
|
||||
crc1 = crc8tab[1];
|
||||
crc2 = crc8_dvb_s2(0, 1);
|
||||
EXPECT_EQ(crc1, crc2);
|
||||
|
||||
crc1 = crc8tab[2];
|
||||
crc2 = crc8_dvb_s2(0, 2);
|
||||
EXPECT_EQ(crc1, crc2);
|
||||
|
||||
crc1 = crc8_buf(buf1, 26);
|
||||
crc2 = crc8_dvb_s2_buf(buf1, 26);
|
||||
EXPECT_EQ(crc1, crc2);
|
||||
}
|
||||
|
||||
TEST(CrossFireTest, TestCrsfFrameStatus)
|
||||
{
|
||||
crsfFrameDone = true;
|
||||
crsfFrame.frame.deviceAddress = CRSF_ADDRESS_CRSF_RECEIVER;
|
||||
crsfFrame.frame.frameLength = 0;
|
||||
crsfFrame.frame.type = CRSF_FRAMETYPE_RC_CHANNELS_PACKED;
|
||||
memset(crsfFrame.frame.payload, 0, CRSF_FRAME_RC_CHANNELS_PAYLOAD_SIZE);
|
||||
const uint8_t crc = crsfFrameCRC();
|
||||
crsfFrame.frame.payload[CRSF_FRAME_RC_CHANNELS_PAYLOAD_SIZE] = crc;
|
||||
|
||||
const uint8_t status = crsfFrameStatus();
|
||||
EXPECT_EQ(RX_FRAME_COMPLETE, status);
|
||||
EXPECT_EQ(false, crsfFrameDone);
|
||||
|
||||
EXPECT_EQ(CRSF_ADDRESS_CRSF_RECEIVER, crsfFrame.frame.deviceAddress);
|
||||
EXPECT_EQ(CRSF_FRAME_RC_CHANNELS_PAYLOAD_SIZE + CRSF_FRAME_LENGTH_TYPE_CRC, crsfFrame.frame.frameLength);
|
||||
EXPECT_EQ(CRSF_FRAMETYPE_RC_CHANNELS_PACKED, crsfFrame.frame.type);
|
||||
for (int ii = 0; ii < CRSF_MAX_CHANNEL; ++ii) {
|
||||
EXPECT_EQ(0, crsfChannelData[ii]);
|
||||
}
|
||||
}
|
||||
|
||||
/*
|
||||
* Frame is of form
|
||||
* <Device address> <Frame length> < Type> <Payload> < CRC>
|
||||
* So RC channels frame is:
|
||||
* <0x00> <0x18> <0x16> <22-bytes payload> < CRC>
|
||||
* 26 bytes altogther.
|
||||
*/
|
||||
TEST(CrossFireTest, TestCrsfFrameStatusUnpacking)
|
||||
{
|
||||
crsfFrameDone = true;
|
||||
crsfFrame.frame.deviceAddress = CRSF_ADDRESS_CRSF_RECEIVER;
|
||||
crsfFrame.frame.frameLength = CRSF_FRAME_RC_CHANNELS_PAYLOAD_SIZE + CRSF_FRAME_LENGTH_TYPE_CRC;;
|
||||
crsfFrame.frame.type = CRSF_FRAMETYPE_RC_CHANNELS_PACKED;
|
||||
// 16 11-bit channels packed into 22 bytes of data
|
||||
crsfFrame.frame.payload[0] = 0xFF; // bits 0-7
|
||||
crsfFrame.frame.payload[1] = 0xFF; // bits 8-15
|
||||
crsfFrame.frame.payload[2] = 0x00; // bits 16-23
|
||||
crsfFrame.frame.payload[3] = 0x00; // bits 24-31
|
||||
crsfFrame.frame.payload[4] = 0x58; // bits 32-39 0101100.
|
||||
crsfFrame.frame.payload[5] = 0x01; // bits 40-47 ....0001
|
||||
crsfFrame.frame.payload[6] = 0x00; // bits 48-55 0.......
|
||||
crsfFrame.frame.payload[7] = 0xf0; // bits 56-64 11110000
|
||||
crsfFrame.frame.payload[8] = 0x01; // bits 65-71 ......01
|
||||
crsfFrame.frame.payload[9] = 0x60; // bits 72-79 011.....
|
||||
crsfFrame.frame.payload[10] = 0xe2; // bits 80-87 11100010
|
||||
crsfFrame.frame.payload[11] = 0;
|
||||
crsfFrame.frame.payload[12] = 0;
|
||||
crsfFrame.frame.payload[13] = 0;
|
||||
crsfFrame.frame.payload[14] = 0;
|
||||
crsfFrame.frame.payload[15] = 0;
|
||||
crsfFrame.frame.payload[16] = 0;
|
||||
crsfFrame.frame.payload[17] = 0;
|
||||
crsfFrame.frame.payload[18] = 0;
|
||||
crsfFrame.frame.payload[19] = 0;
|
||||
crsfFrame.frame.payload[20] = 0;
|
||||
crsfFrame.frame.payload[21] = 0;
|
||||
const uint8_t crc = crsfFrameCRC();
|
||||
crsfFrame.frame.payload[CRSF_FRAME_RC_CHANNELS_PAYLOAD_SIZE] = crc;
|
||||
|
||||
const uint8_t status = crsfFrameStatus();
|
||||
EXPECT_EQ(RX_FRAME_COMPLETE, status);
|
||||
EXPECT_EQ(false, crsfFrameDone);
|
||||
|
||||
EXPECT_EQ(CRSF_ADDRESS_CRSF_RECEIVER, crsfFrame.frame.deviceAddress);
|
||||
EXPECT_EQ(CRSF_FRAME_RC_CHANNELS_PAYLOAD_SIZE + CRSF_FRAME_LENGTH_TYPE_CRC, crsfFrame.frame.frameLength);
|
||||
EXPECT_EQ(CRSF_FRAMETYPE_RC_CHANNELS_PACKED, crsfFrame.frame.type);
|
||||
EXPECT_EQ(0x7ff, crsfChannelData[0]);
|
||||
EXPECT_EQ(0x1f, crsfChannelData[1]);
|
||||
EXPECT_EQ(0, crsfChannelData[2]);
|
||||
EXPECT_EQ(172, crsfChannelData[3]); // 172 = 0x0ac, 0001 0101100, bits 33-43
|
||||
EXPECT_EQ(0, crsfChannelData[4]);
|
||||
EXPECT_EQ(992, crsfChannelData[5]); // 992 = 0x3e0, 01 1110000 0, bits 55-65
|
||||
EXPECT_EQ(0, crsfChannelData[6]);
|
||||
EXPECT_EQ(1811, crsfChannelData[7]); // 1811 = 0x713, 1110 0010 011, bits 77-87
|
||||
EXPECT_EQ(0, crsfChannelData[8]);
|
||||
EXPECT_EQ(0, crsfChannelData[9]);
|
||||
EXPECT_EQ(0, crsfChannelData[10]);
|
||||
EXPECT_EQ(0, crsfChannelData[11]);
|
||||
EXPECT_EQ(0, crsfChannelData[12]);
|
||||
EXPECT_EQ(0, crsfChannelData[13]);
|
||||
EXPECT_EQ(0, crsfChannelData[14]);
|
||||
EXPECT_EQ(0, crsfChannelData[15]);
|
||||
}
|
||||
|
||||
const uint8_t capturedData[] = {
|
||||
0x00,0x18,0x16,0xBD,0x08,0x9F,0xF4,0xAE,0xF7,0xBD,0xEF,0x7D,0xEF,0xFB,0xAD,0xFD,0x45,0x2B,0x5A,0x01,0x00,0x00,0x00,0x00,0x00,0x6C,
|
||||
0x00,0x18,0x16,0xBD,0x08,0x9F,0xF4,0xAA,0xF7,0xBD,0xEF,0x7D,0xEF,0xFB,0xAD,0xFD,0x45,0x2B,0x5A,0x01,0x00,0x00,0x00,0x00,0x00,0x94,
|
||||
};
|
||||
|
||||
typedef struct crsfRcChannelsFrame_s {
|
||||
uint8_t deviceAddress;
|
||||
uint8_t frameLength;
|
||||
uint8_t type;
|
||||
uint8_t payload[CRSF_FRAME_RC_CHANNELS_PAYLOAD_SIZE + CRSF_FRAME_LENGTH_CRC];
|
||||
} crsfRcChannelsFrame_t;
|
||||
|
||||
|
||||
TEST(CrossFireTest, TestCapturedData)
|
||||
{
|
||||
//const int frameCount = sizeof(capturedData) / sizeof(crsfRcChannelsFrame_t);
|
||||
const crsfRcChannelsFrame_t *framePtr = (const crsfRcChannelsFrame_t*)capturedData;
|
||||
crsfFrame = *(const crsfFrame_t*)framePtr;
|
||||
crsfFrameDone = true;
|
||||
uint8_t status = crsfFrameStatus();
|
||||
EXPECT_EQ(RX_FRAME_COMPLETE, status);
|
||||
EXPECT_EQ(false, crsfFrameDone);
|
||||
EXPECT_EQ(RX_FRAME_COMPLETE, status);
|
||||
EXPECT_EQ(false, crsfFrameDone);
|
||||
EXPECT_EQ(CRSF_ADDRESS_BROADCAST, crsfFrame.frame.deviceAddress);
|
||||
EXPECT_EQ(CRSF_FRAME_RC_CHANNELS_PAYLOAD_SIZE + CRSF_FRAME_LENGTH_TYPE_CRC, crsfFrame.frame.frameLength);
|
||||
EXPECT_EQ(CRSF_FRAMETYPE_RC_CHANNELS_PACKED, crsfFrame.frame.type);
|
||||
EXPECT_EQ(189, crsfChannelData[0]);
|
||||
EXPECT_EQ(993, crsfChannelData[1]);
|
||||
EXPECT_EQ(978, crsfChannelData[2]);
|
||||
EXPECT_EQ(983, crsfChannelData[3]);
|
||||
uint8_t crc = crsfFrameCRC();
|
||||
EXPECT_EQ(crc, crsfFrame.frame.payload[CRSF_FRAME_RC_CHANNELS_PAYLOAD_SIZE]);
|
||||
EXPECT_EQ(999, crsfReadRawRC(NULL, 0));
|
||||
EXPECT_EQ(1501, crsfReadRawRC(NULL, 1));
|
||||
EXPECT_EQ(1492, crsfReadRawRC(NULL, 2));
|
||||
EXPECT_EQ(1495, crsfReadRawRC(NULL, 3));
|
||||
|
||||
++framePtr;
|
||||
crsfFrame = *(const crsfFrame_t*)framePtr;
|
||||
crsfFrameDone = true;
|
||||
status = crsfFrameStatus();
|
||||
EXPECT_EQ(RX_FRAME_COMPLETE, status);
|
||||
EXPECT_EQ(false, crsfFrameDone);
|
||||
EXPECT_EQ(RX_FRAME_COMPLETE, status);
|
||||
EXPECT_EQ(false, crsfFrameDone);
|
||||
EXPECT_EQ(CRSF_ADDRESS_BROADCAST, crsfFrame.frame.deviceAddress);
|
||||
EXPECT_EQ(CRSF_FRAME_RC_CHANNELS_PAYLOAD_SIZE + CRSF_FRAME_LENGTH_TYPE_CRC, crsfFrame.frame.frameLength);
|
||||
EXPECT_EQ(CRSF_FRAMETYPE_RC_CHANNELS_PACKED, crsfFrame.frame.type);
|
||||
EXPECT_EQ(189, crsfChannelData[0]);
|
||||
EXPECT_EQ(993, crsfChannelData[1]);
|
||||
EXPECT_EQ(978, crsfChannelData[2]);
|
||||
EXPECT_EQ(981, crsfChannelData[3]);
|
||||
crc = crsfFrameCRC();
|
||||
EXPECT_EQ(crc, crsfFrame.frame.payload[CRSF_FRAME_RC_CHANNELS_PAYLOAD_SIZE]);
|
||||
}
|
||||
|
||||
|
||||
TEST(CrossFireTest, TestCrsfDataReceive)
|
||||
{
|
||||
crsfFrameDone = false;
|
||||
const uint8_t *pData = capturedData;
|
||||
for (unsigned int ii = 0; ii < sizeof(crsfRcChannelsFrame_t); ++ii) {
|
||||
crsfDataReceive(*pData++);
|
||||
}
|
||||
EXPECT_EQ(true, crsfFrameDone);
|
||||
EXPECT_EQ(CRSF_ADDRESS_BROADCAST, crsfFrame.frame.deviceAddress);
|
||||
EXPECT_EQ(CRSF_FRAME_RC_CHANNELS_PAYLOAD_SIZE + CRSF_FRAME_LENGTH_TYPE_CRC, crsfFrame.frame.frameLength);
|
||||
EXPECT_EQ(CRSF_FRAMETYPE_RC_CHANNELS_PACKED, crsfFrame.frame.type);
|
||||
uint8_t crc = crsfFrameCRC();
|
||||
for (int ii = 0; ii < CRSF_FRAME_RC_CHANNELS_PAYLOAD_SIZE; ++ii) {
|
||||
EXPECT_EQ(capturedData[ii + 3], crsfFrame.frame.payload[ii]);
|
||||
}
|
||||
EXPECT_EQ(crc, crsfFrame.frame.payload[CRSF_FRAME_RC_CHANNELS_PAYLOAD_SIZE]);
|
||||
}
|
||||
|
||||
// STUBS
|
||||
|
||||
extern "C" {
|
||||
|
||||
int16_t debug[DEBUG16_VALUE_COUNT];
|
||||
uint32_t micros(void) {return dummyTimeUs;}
|
||||
serialPort_t *openSerialPort(serialPortIdentifier_e, serialPortFunction_e, serialReceiveCallbackPtr, uint32_t, portMode_t, portOptions_t) {return NULL;}
|
||||
serialPortConfig_t *findSerialPortConfig(serialPortFunction_e ) {return NULL;}
|
||||
void serialWriteBuf(serialPort_t *, const uint8_t *, int) {}
|
||||
bool telemetryCheckRxPortShared(const serialPortConfig_t *) {return false;}
|
||||
serialPort_t *telemetrySharedPort = NULL;
|
||||
}
|
||||
|
|
@ -23,7 +23,25 @@
|
|||
#define BARO
|
||||
#define GPS
|
||||
#define DISPLAY
|
||||
#define SERIAL_RX
|
||||
#define USE_RX_MSP
|
||||
#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
|
||||
#define USE_SERIALRX_JETIEXBUS
|
||||
#define USE_SERIALRX_SUMD // Graupner Hott protocol
|
||||
#define USE_SERIALRX_SUMH // Graupner legacy protocol
|
||||
#define USE_SERIALRX_XBUS // JR
|
||||
#define TELEMETRY
|
||||
#define TELEMETRY_CRSF
|
||||
#define TELEMETRY_FRSKY
|
||||
#define TELEMETRY_HOTT
|
||||
#define TELEMETRY_IBUS
|
||||
#define TELEMETRY_JETIEXBUS
|
||||
#define TELEMETRY_LTM
|
||||
#define TELEMETRY_MAVLINK
|
||||
#define TELEMETRY_SMARTPORT
|
||||
#define LED_STRIP
|
||||
#define USE_SERVOS
|
||||
#define TRANSPONDER
|
||||
|
|
|
|||
328
src/test/unit/telemetry_crsf_unittest.cc
Normal file
328
src/test/unit/telemetry_crsf_unittest.cc
Normal file
|
|
@ -0,0 +1,328 @@
|
|||
/*
|
||||
* 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 <http://www.gnu.org/licenses/>.
|
||||
*/
|
||||
|
||||
#include <stdbool.h>
|
||||
#include <stdint.h>
|
||||
#include <string.h>
|
||||
|
||||
#include <limits.h>
|
||||
|
||||
extern "C" {
|
||||
#include "build/debug.h"
|
||||
|
||||
#include <platform.h>
|
||||
|
||||
#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"
|
||||
|
||||
bool airMode;
|
||||
serialPort_t *telemetrySharedPort;
|
||||
}
|
||||
|
||||
#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
|
||||
|
||||
*/
|
||||
#define FRAME_HEADER_FOOTER_LEN 4
|
||||
|
||||
TEST(TelemetryCrsfTest, TestGPS)
|
||||
{
|
||||
uint8_t frame[CRSF_FRAME_SIZE_MAX];
|
||||
|
||||
int frameLen = getCrsfFrame(frame, CRSF_FRAME_GPS);
|
||||
EXPECT_EQ(CRSF_FRAME_GPS_PAYLOAD_SIZE + FRAME_HEADER_FOOTER_LEN, frameLen);
|
||||
EXPECT_EQ(CRSF_ADDRESS_BROADCAST, 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 + FRAME_HEADER_FOOTER_LEN, frameLen);
|
||||
EXPECT_EQ(CRSF_ADDRESS_BROADCAST, 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 + FRAME_HEADER_FOOTER_LEN, frameLen);
|
||||
EXPECT_EQ(CRSF_ADDRESS_BROADCAST, 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 + FRAME_HEADER_FOOTER_LEN, frameLen);
|
||||
EXPECT_EQ(CRSF_ADDRESS_BROADCAST, 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];
|
||||
|
||||
// nothing set, so ACRO mode
|
||||
airMode = false;
|
||||
int frameLen = getCrsfFrame(frame, CRSF_FRAME_FLIGHT_MODE);
|
||||
EXPECT_EQ(5 + FRAME_HEADER_FOOTER_LEN, frameLen);
|
||||
EXPECT_EQ(CRSF_ADDRESS_BROADCAST, frame[0]); // address
|
||||
EXPECT_EQ(7, frame[1]); // length
|
||||
EXPECT_EQ(0x21, frame[2]); // type
|
||||
EXPECT_EQ('A', frame[3]);
|
||||
EXPECT_EQ('C', frame[4]);
|
||||
EXPECT_EQ('R', frame[5]);
|
||||
EXPECT_EQ('O', frame[6]);
|
||||
EXPECT_EQ(0, frame[7]);
|
||||
EXPECT_EQ(crfsCrc(frame, frameLen), frame[8]);
|
||||
|
||||
|
||||
enableFlightMode(ANGLE_MODE);
|
||||
EXPECT_EQ(ANGLE_MODE, FLIGHT_MODE(ANGLE_MODE));
|
||||
frameLen = getCrsfFrame(frame, CRSF_FRAME_FLIGHT_MODE);
|
||||
EXPECT_EQ(5 + FRAME_HEADER_FOOTER_LEN, frameLen);
|
||||
EXPECT_EQ(CRSF_ADDRESS_BROADCAST, frame[0]); // address
|
||||
EXPECT_EQ(7, frame[1]); // length
|
||||
EXPECT_EQ(0x21, frame[2]); // type
|
||||
EXPECT_EQ('S', frame[3]);
|
||||
EXPECT_EQ('T', frame[4]);
|
||||
EXPECT_EQ('A', frame[5]);
|
||||
EXPECT_EQ('B', frame[6]);
|
||||
EXPECT_EQ(0, frame[7]);
|
||||
EXPECT_EQ(crfsCrc(frame, frameLen), frame[8]);
|
||||
|
||||
disableFlightMode(ANGLE_MODE);
|
||||
enableFlightMode(HORIZON_MODE);
|
||||
EXPECT_EQ(HORIZON_MODE, FLIGHT_MODE(HORIZON_MODE));
|
||||
frameLen = getCrsfFrame(frame, CRSF_FRAME_FLIGHT_MODE);
|
||||
EXPECT_EQ(4 + FRAME_HEADER_FOOTER_LEN, frameLen);
|
||||
EXPECT_EQ(CRSF_ADDRESS_BROADCAST, frame[0]); // address
|
||||
EXPECT_EQ(6, frame[1]); // length
|
||||
EXPECT_EQ(0x21, frame[2]); // type
|
||||
EXPECT_EQ('H', frame[3]);
|
||||
EXPECT_EQ('O', frame[4]);
|
||||
EXPECT_EQ('R', frame[5]);
|
||||
EXPECT_EQ(0, frame[6]);
|
||||
EXPECT_EQ(crfsCrc(frame, frameLen), frame[7]);
|
||||
|
||||
disableFlightMode(HORIZON_MODE);
|
||||
airMode = true;
|
||||
frameLen = getCrsfFrame(frame, CRSF_FRAME_FLIGHT_MODE);
|
||||
EXPECT_EQ(4 + FRAME_HEADER_FOOTER_LEN, frameLen);
|
||||
EXPECT_EQ(CRSF_ADDRESS_BROADCAST, frame[0]); // address
|
||||
EXPECT_EQ(6, frame[1]); // length
|
||||
EXPECT_EQ(0x21, frame[2]); // type
|
||||
EXPECT_EQ('A', frame[3]);
|
||||
EXPECT_EQ('I', frame[4]);
|
||||
EXPECT_EQ('R', frame[5]);
|
||||
EXPECT_EQ(0, frame[6]);
|
||||
EXPECT_EQ(crfsCrc(frame, frameLen), frame[7]);
|
||||
}
|
||||
|
||||
// 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 micros(void) {return 0;}
|
||||
|
||||
uint32_t serialRxBytesWaiting(const serialPort_t *) {return 0;}
|
||||
uint32_t serialTxBytesFree(const serialPort_t *) {return 0;}
|
||||
uint8_t serialRead(serialPort_t *) {return 0;}
|
||||
void serialWrite(serialPort_t *, uint8_t) {}
|
||||
void serialWriteBuf(serialPort_t *, const uint8_t *, int) {}
|
||||
void serialSetMode(serialPort_t *, portMode_t ) {}
|
||||
serialPort_t *openSerialPort(serialPortIdentifier_e, serialPortFunction_e, serialReceiveCallbackPtr, uint32_t, portMode_t, portOptions_t) {return NULL;}
|
||||
void closeSerialPort(serialPort_t *) {}
|
||||
|
||||
serialPortConfig_t *findSerialPortConfig(serialPortFunction_e) {return NULL;}
|
||||
|
||||
bool telemetryDetermineEnabledState(portSharing_e) {return true;}
|
||||
bool telemetryCheckRxPortShared(const serialPortConfig_t *) {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;}
|
||||
bool isAirmodeActive(void) {return airMode;}
|
||||
|
||||
}
|
||||
|
||||
Loading…
Add table
Add a link
Reference in a new issue