iNavFlight之RC遥控CRSF协议
iNavFlight之RC遥控CRSF协议
- 1. 遥控器电传框架设计
-
- 1.1 场景分析
- 1.2 逻辑框架
-
- 1.2.1 电传信息获取
- 1.2.2 电传信息处理
- 1.2.3 电传初始化
- 1.3 模块化设计
- 2. CRSF电传报文
-
- 2.1 CRSF电传报文格式
- 2.2 CRSF电传报文内容
-
- 2.2.1 CRSF_FRAMETYPE_ATTITUDE
- 2.2.2 CRSF_FRAMETYPE_BATTERY_SENSOR
- 2.2.3 CRSF_FRAMETYPE_FLIGHT_MODE
- 2.2.4 CRSF_FRAMETYPE_GPS
- 2.2.5 CRSF_FRAMETYPE_VARIO_SENSOR
- 3. CRSF摇杆代码设计
-
- 3.1 crsfRxInit
- 3.2 crsfFrameStatus
- 3.3 crsfReadRawRC
- 3.4 crsfDataReceive
- 4. 参考资料
本章重点介绍RC遥控CRSF协议，因为博主现在用的遥控器是TX12，外接了ELRS 915MHz发射器。
注：前面的坑就不说了，都是通信距离短短惹的货，最后就换遥控器，配大功率发射机。详细可参考【四轴飞控DIY集成FPV功能】。
关于【RC摇杆总体逻辑框架】【RC摇杆代码设计框架】这里我们都不再重复了，如果前面那篇没有看过的，点击【链接】。
1. 遥控器电传框架设计
通常遥控器都有电传功能：飞控可以配置使用遥控通信频道汇报链路状态，甚至GPS、高度等信息。这里补充下遥控电传的框架设计。
注1：基于MSP的RC遥控这部分内容是没有的，是一种存粹的遥控。
注2：通常MSP协议是通过TTL串口物理硬连接，且物理连线很短。所以设计时仅有一个20HZ的超时，也没有CRC校验，来确保链路的可靠性和数据的可靠性。
1.1 场景分析
除了控制飞控，同时也希望获得飞控当前的状态，比如：飞行姿态，高度，GPS位置等信息，以便地面人员更好的了解模型的飞行情况。
注：当然有些信息在OSD上也有显示，甚至显示的更加全面，但是遥控和电传比图传发展的更早。大家应该知道什么是目视飞行，目视飞行和FPV飞行最大的差异就是目视是以第三方视角来操控飞机，而FPV是第一人称方式操控飞机。
1.2 逻辑框架
从逻辑角度，需要三个步骤来完成电传的功能：
- 电传信息获取
- 电传信息处理
- 电传初始化
1.2.1 电传信息获取
- 电传信息主要来源于全局变量
- 当前全局变量一定是最近一次的数据更新
- iNav/BetaFlight飞控是一种特殊的多任务微系统(每个任务都会完整完成才会被切换)，数据完整性能够得到保证
所以，没有必要像多任务系统那样复杂封装接口来确保数据的有效性和完整性。
1.2.2 电传信息处理
电传信息主要来源于飞控内部，按时更新并反馈地面端即可，iNav/BetaFlight开启了电传任务来完成上述工作。
鉴于电传信息的汇报成功与否，并非会对飞行形成致命伤害，所以报文是否被地面端接受其实并不是太重要，反而间接的检验飞控与遥控器之间的通信链路是否稳定和可靠的一种补充。
当然，向433MHz的电传报文大部分就不是发送给遥控器，而是发给地面站软件，而这部分可能MAVLink通信报文比较多，后续我们有时间也会慢慢研读和介绍。
1.2.3 电传初始化
略, 这部分就不多展开，所有的嵌入式设计都会有初始化部分，航模飞控没有去初始化的部分，大体是因为我们通常是拔电池了，当然如果说自杀式无人机，估计也不需要去初始化，人家上天就不打算回来了，拔电池都省略了，呵呵。
注：要注意一点，如果有遥控器，通信端口在遥控器的初始化会挂在相应的报文接收函数。但是如果仅仅是电传，比如：MAVLink，那么初始化部分会提供通信端口的初始化，此时会挂在报文接收处理函数。
1.3 模块化设计
- 各种电传报文初始化部分
main
└──> init
└──> telemetryInit
├──> initFrSkyTelemetry
├──> initHoTTTelemetry
├──> initSmartPortTelemetry
├──> initLtmTelemetry
├──> initMAVLinkTelemetry
├──> initJetiExBusTelemetry
├──> initIbusTelemetry
├──> initSimTelemetry
├──> **initCrsfTelemetry**
├──> initSrxlTelemetry
├──> initGhstTelemetry
└──> telemetryCheckState
├──> checkFrSkyTelemetryState
├──> checkHoTTTelemetryState
├──> checkSmartPortTelemetryState
├──> checkLtmTelemetryState
├──> checkMAVLinkTelemetryState
├──> checkJetiExBusTelemetryState
├──> checkIbusTelemetryState
├──> checkSimTelemetryState
├──> **checkCrsfTelemetryState**
├──> checkSrxlTelemetryState
└──> checkGhstTelemetryState
- crsfRxSendTelemetryData发送电传报文 //永远发送的上一次打包的电传报文，鉴于频率是500Hz，2ms间隔粒度延迟
- processCrsf打包电传报文
taskTelemetry
└──> telemetryProcess
└──> handleCrsfTelemetry
├──> crsfRxSendTelemetryData
└──> processCrsf
#ifdef USE_TELEMETRY
[TASK_TELEMETRY] = {
.taskName = "TELEMETRY",
.taskFunc = taskTelemetry,
.desiredPeriod = TASK_PERIOD_HZ(500), // 500 Hz
.staticPriority = TASK_PRIORITY_IDLE,
},
#endif
2. CRSF电传报文
2.1 CRSF电传报文格式
+----+---------+---------+---------------------+-----+
| CRSF Frame Format 4 + payload(n) |
+----+---------+---------+---------------------+-----+
|0XC8| size(1) | type(1) | payload(size - 2) | CRC |
+----+---------+---------+---------------------+-----+
size = payload(n) + type(1) + crc(1)
2.2 CRSF电传报文内容
iNav支持以下CRSF电传报文
- CRSF_FRAMETYPE_ATTITUDE = 0x1E
- CRSF_FRAMETYPE_BATTERY_SENSOR = 0x08
- CRSF_FRAMETYPE_FLIGHT_MODE = 0x21
- CRSF_FRAMETYPE_GPS = 0x02
- CRSF_FRAMETYPE_VARIO_SENSOR = 0x07
2.2.1 CRSF_FRAMETYPE_ATTITUDE
飞控飞行姿态角度：Pitch/Roll/Yaw
/*
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) ((int16_t)(1000.0f * (angle) * RAD))
static 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));
}
2.2.2 CRSF_FRAMETYPE_BATTERY_SENSOR
电池状态：电压/电流/容量/剩余百分比
/*
0x08 Battery sensor
Payload:
uint16_t Voltage ( mV * 100 )
uint16_t Current ( mA * 100 )
uint24_t Capacity ( mAh )
uint8_t Battery remaining ( percent )
*/
static 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);
if (telemetryConfig()->report_cell_voltage) {
crsfSerialize16(dst, getBatteryAverageCellVoltage() / 10);
} else {
crsfSerialize16(dst, getBatteryVoltage() / 10); // vbat is in units of 0.01V
}
crsfSerialize16(dst, getAmperage() / 10);
const uint8_t batteryRemainingPercentage = calculateBatteryPercentage();
crsfSerialize8(dst, (getMAhDrawn() >> 16));
crsfSerialize8(dst, (getMAhDrawn() >> 8));
crsfSerialize8(dst, (uint8_t)getMAhDrawn());
crsfSerialize8(dst, batteryRemainingPercentage);
}
2.2.3 CRSF_FRAMETYPE_FLIGHT_MODE
飞行模式
/*
0x21 Flight mode text based
Payload:
char[] Flight mode ( Nullterminated string )
*/
static void crsfFrameFlightMode(sbuf_t *dst)
{
// just do "OK" 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 when armed
const char *flightMode = "OK";
if (ARMING_FLAG(ARMED)) {
if (STATE(AIRMODE_ACTIVE)) {
flightMode = "AIR";
} else {
flightMode = "ACRO";
}
if (FLIGHT_MODE(FAILSAFE_MODE)) {
flightMode = "!FS!";
} else if (ARMING_FLAG(ARMED) && IS_RC_MODE_ACTIVE(BOXHOMERESET) && !FLIGHT_MODE(NAV_RTH_MODE) && !FLIGHT_MODE(NAV_WP_MODE)) {
flightMode = "HRST";
} else if (FLIGHT_MODE(MANUAL_MODE)) {
flightMode = "MANU";
} else if (FLIGHT_MODE(NAV_RTH_MODE)) {
flightMode = "RTH";
} else if (FLIGHT_MODE(NAV_POSHOLD_MODE)) {
flightMode = "HOLD";
} else if (FLIGHT_MODE(NAV_COURSE_HOLD_MODE) && FLIGHT_MODE(NAV_ALTHOLD_MODE)) {
flightMode = "CRUZ";
} else if (FLIGHT_MODE(NAV_COURSE_HOLD_MODE)) {
flightMode = "CRSH";
} else if (FLIGHT_MODE(NAV_ALTHOLD_MODE)) {
flightMode = "AH";
} else if (FLIGHT_MODE(NAV_WP_MODE)) {
flightMode = "WP";
} else if (FLIGHT_MODE(ANGLE_MODE)) {
flightMode = "ANGL";
} else if (FLIGHT_MODE(HORIZON_MODE)) {
flightMode = "HOR";
}
#ifdef USE_GPS
} else if (feature(FEATURE_GPS) && navConfig()->general.flags.extra_arming_safety && (!STATE(GPS_FIX) || !STATE(GPS_FIX_HOME))) {
flightMode = "WAIT"; // Waiting for GPS lock
#endif
} else if (isArmingDisabled()) {
flightMode = "!ERR";
}
crsfSerializeData(dst, (const uint8_t*)flightMode, strlen(flightMode));
crsfSerialize8(dst, 0); // zero terminator for string
// write in the length
*lengthPtr = sbufPtr(dst) - lengthPtr;
}
2.2.4 CRSF_FRAMETYPE_GPS
全套GPS信息，含GPS坐标，高度，卫星数量等等。
/*
0x02 GPS
Payload:
int32_t Latitude ( degree / 10`000`000 )
int32_t Longitude (degree / 10`000`000 )
uint16_t Groundspeed ( km/h / 10 )
uint16_t GPS heading ( degree / 100 )
uint16 Altitude ( meter 1000m offset )
uint8_t Satellites in use ( counter )
*/
static 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, gpsSol.llh.lat); // CRSF and betaflight use same units for degrees
crsfSerialize32(dst, gpsSol.llh.lon);
crsfSerialize16(dst, (gpsSol.groundSpeed * 36 + 50) / 100); // gpsSol.groundSpeed is in cm/s
crsfSerialize16(dst, DECIDEGREES_TO_CENTIDEGREES(gpsSol.groundCourse)); // gpsSol.groundCourse is 0.1 degrees, need 0.01 deg
const uint16_t altitude = (getEstimatedActualPosition(Z) / 100) + 1000;
crsfSerialize16(dst, altitude);
crsfSerialize8(dst, gpsSol.numSat);
}
2.2.5 CRSF_FRAMETYPE_VARIO_SENSOR
计算得来的估算垂直速度
/*
0x07 Vario sensor
Payload:
int16 Vertical speed ( cm/s )
*/
static void crsfFrameVarioSensor(sbuf_t *dst)
{
// use sbufWrite since CRC does not include frame length
sbufWriteU8(dst, CRSF_FRAME_VARIO_SENSOR_PAYLOAD_SIZE + CRSF_FRAME_LENGTH_TYPE_CRC);
crsfSerialize8(dst, CRSF_FRAMETYPE_VARIO_SENSOR);
crsfSerialize16(dst, lrintf(getEstimatedActualVelocity(Z)));
}
3. CRSF摇杆代码设计
依然按照iNavFlight之RC遥控MSP协议里面关于遥控部分的逻辑思路和抽象化设计概念走。
- rcInit ==> crsfRxInit
- rcFrameStatus ==> crsfFrameStatus
- rcProcessFrame ==> 无
- rcReadRaw ==> crsfReadRawRC
- rcFrameReceive ==> crsfDataReceive
3.1 crsfRxInit
- 将crsfReadRawRC和crsfFrameStatus两个处理函数挂上统一处理框架
- 将crsfDataReceive报文处理函数挂上
- 支持17个RC摇杆通道
bool crsfRxInit(const rxConfig_t *rxConfig, rxRuntimeConfig_t *rxRuntimeConfig)
{
for (int ii = 0; ii < CRSF_MAX_CHANNEL; ++ii) {
crsfChannelData[ii] = (16 * PWM_RANGE_MIDDLE) / 10 - 1408;
}
rxRuntimeConfig->channelCount = CRSF_MAX_CHANNEL;
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,
NULL,
CRSF_BAUDRATE,
CRSF_PORT_MODE,
CRSF_PORT_OPTIONS | (tristateWithDefaultOffIsActive(rxConfig->halfDuplex) ? SERIAL_BIDIR : 0)
);
return serialPort != NULL;
}
#define CRSF_MAX_CHANNEL 17
3.2 crsfFrameStatus
- 两种状态：PENDING or COMPLETE
- 两种报文：CRSF_FRAMETYPE_RC_CHANNELS_PACKED or CRSF_FRAMETYPE_LINK_STATISTICS
STATIC_UNIT_TESTED uint8_t crsfFrameStatus(rxRuntimeConfig_t *rxRuntimeConfig)
{
UNUSED(rxRuntimeConfig);
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;
}
else if (crsfFrame.frame.type == CRSF_FRAMETYPE_LINK_STATISTICS) {
// CRC includes type and payload of each frame
const uint8_t crc = crsfFrameCRC();
if (crc != crsfFrame.frame.payload[CRSF_FRAME_LINK_STATISTICS_PAYLOAD_SIZE]) {
return RX_FRAME_PENDING;
}
crsfFrame.frame.frameLength = CRSF_FRAME_LINK_STATISTICS_PAYLOAD_SIZE + CRSF_FRAME_LENGTH_TYPE_CRC;
const crsfPayloadLinkStatistics_t* linkStats = (crsfPayloadLinkStatistics_t*)&crsfFrame.frame.payload;
const uint8_t crsftxpowerindex = (linkStats->uplinkTXPower < CRSF_POWER_COUNT) ? linkStats->uplinkTXPower : 0;
rxLinkStatistics.uplinkRSSI = -1* (linkStats->activeAntenna ? linkStats->uplinkRSSIAnt2 : linkStats->uplinkRSSIAnt1);
rxLinkStatistics.uplinkLQ = linkStats->uplinkLQ;
rxLinkStatistics.uplinkSNR = linkStats->uplinkSNR;
rxLinkStatistics.rfMode = linkStats->rfMode;
rxLinkStatistics.uplinkTXPower = crsfTxPowerStatesmW[crsftxpowerindex];
rxLinkStatistics.activeAntenna = linkStats->activeAntenna;
if (rxLinkStatistics.uplinkLQ > 0) {
int16_t uplinkStrength; // RSSI dBm converted to %
uplinkStrength = constrain((100 * sq((osdConfig()->rssi_dbm_max - osdConfig()->rssi_dbm_min)) - (100 * sq((osdConfig()->rssi_dbm_max - rxLinkStatistics.uplinkRSSI)))) / sq((osdConfig()->rssi_dbm_max - osdConfig()->rssi_dbm_min)),0,100);
if (rxLinkStatistics.uplinkRSSI >= osdConfig()->rssi_dbm_max )
uplinkStrength = 99;
else if (rxLinkStatistics.uplinkRSSI < osdConfig()->rssi_dbm_min)
uplinkStrength = 0;
lqTrackerSet(rxRuntimeConfig->lqTracker, scaleRange(uplinkStrength, 0, 99, 0, RSSI_MAX_VALUE));
}
else
lqTrackerSet(rxRuntimeConfig->lqTracker, 0);
// This is not RC channels frame, update channel value but don't indicate frame completion
return RX_FRAME_PENDING;
}
}
return RX_FRAME_PENDING;
}
3.3 crsfReadRawRC
获取当前某个通道的摇杆值。
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 (crsfChannelData[chan] * 1024 / 1639) + 881;
}
3.4 crsfDataReceive
支持以下报文：
- CRSF_FRAMETYPE_RC_CHANNELS_PACKED
- CRSF_FRAMETYPE_LINK_STATISTICS
- CRSF_FRAMETYPE_MSP_REQ and CRSF_FRAMETYPE_MSP_WRITE
以轮训遍历的方式发送电传报文，根据目前5个电传报文按照500Hz速率，10ms轮询一次。
// Receive ISR callback, called back from serial port
STATIC_UNIT_TESTED void crsfDataReceive(uint16_t c, void *rxCallbackData)
{
UNUSED(rxCallbackData);
static uint8_t crsfFramePosition = 0;
const timeUs_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;
if (crsfFrameDone) {
crsfFramePosition = 0;
if (crsfFrame.frame.type != CRSF_FRAMETYPE_RC_CHANNELS_PACKED) {
const uint8_t crc = crsfFrameCRC();
if (crc == crsfFrame.bytes[fullFrameLength - 1]) {
switch (crsfFrame.frame.type)
{
#if defined(USE_MSP_OVER_TELEMETRY)
case CRSF_FRAMETYPE_MSP_REQ:
case CRSF_FRAMETYPE_MSP_WRITE: {
uint8_t *frameStart = (uint8_t *)&crsfFrame.frame.payload + CRSF_FRAME_ORIGIN_DEST_SIZE;
if (bufferCrsfMspFrame(frameStart, CRSF_FRAME_RX_MSP_FRAME_SIZE)) {
crsfScheduleMspResponse();
}
break;
}
#endif
default:
break;
}
}
}
}
}
}
4. 参考资料
【1】iNavFlight之RC遥控MSP协议