iNavFlight之RC遥控CRSF协议

本章重点介绍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. 电传信息处理
  3. 电传初始化

1.2.1 电传信息获取

  1. 电传信息主要来源于全局变量
  2. 当前全局变量一定是最近一次的数据更新
  3. 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电传报文

  1. CRSF_FRAMETYPE_ATTITUDE = 0x1E
  2. CRSF_FRAMETYPE_BATTERY_SENSOR = 0x08
  3. CRSF_FRAMETYPE_FLIGHT_MODE = 0x21
  4. CRSF_FRAMETYPE_GPS = 0x02
  5. 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 ( Null­terminated 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协议里面关于遥控部分的逻辑思路和抽象化设计概念走。

  1. rcInit ==> crsfRxInit
  2. rcFrameStatus ==> crsfFrameStatus
  3. rcProcessFrame ==> 无
  4. rcReadRaw ==> crsfReadRawRC
  5. rcFrameReceive ==> crsfDataReceive

3.1 crsfRxInit

  1. 将crsfReadRawRC和crsfFrameStatus两个处理函数挂上统一处理框架
  2. 将crsfDataReceive报文处理函数挂上
  3. 支持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

  1. 两种状态:PENDING or COMPLETE
  2. 两种报文: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

支持以下报文:

  1. CRSF_FRAMETYPE_RC_CHANNELS_PACKED
  2. CRSF_FRAMETYPE_LINK_STATISTICS
  3. 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协议

  • 2
    点赞
  • 27
    收藏
    觉得还不错? 一键收藏
  • 4
    评论

“相关推荐”对你有帮助么?

  • 非常没帮助
  • 没帮助
  • 一般
  • 有帮助
  • 非常有帮助
提交
评论 4
添加红包

请填写红包祝福语或标题

红包个数最小为10个

红包金额最低5元

当前余额3.43前往充值 >
需支付:10.00
成就一亿技术人!
领取后你会自动成为博主和红包主的粉丝 规则
hope_wisdom
发出的红包
实付
使用余额支付
点击重新获取
扫码支付
钱包余额 0

抵扣说明:

1.余额是钱包充值的虚拟货币,按照1:1的比例进行支付金额的抵扣。
2.余额无法直接购买下载,可以购买VIP、付费专栏及课程。

余额充值