ArduPilot开源代码之AP_OSD_MSP_DisplayPort

1. 源由

前面讨论了

本章将针对具体的AP_OSD_MSP_DisplayPort类进行研讨。

2. 结构设计

2.1 成员变量

新增:

  • AP_MSP_Telem_Backend* _displayport;
  • bool _blink_on;

2.2 OSD 符号

显然,这里和 AP_OSD_Backend 不一致。

这也比较符合实际情况,不同的实现由于历史原因,或多或少都是不相同的,尤其开源的规划更加分散。

    // MSP DisplayPort symbols
    static const uint8_t SYM_M = 0x0C;
    static const uint8_t SYM_KM = 0x7D;
    static const uint8_t SYM_FT = 0x0F;
    static const uint8_t SYM_MI = 0x7E;
    static const uint8_t SYM_ALT_M = 0x0C;
    static const uint8_t SYM_ALT_FT = 0x0F;
    static const uint8_t SYM_BATT_FULL = 0x90;
    static const uint8_t SYM_RSSI = 0x01;

    static const uint8_t SYM_VOLT = 0x06;
    static const uint8_t SYM_AMP = 0x9A;
    static const uint8_t SYM_MAH = 0x07;
    static const uint8_t SYM_MS = 0x9F;
    static const uint8_t SYM_FS = 0x99;
    static const uint8_t SYM_KMH = 0x9E;
    static const uint8_t SYM_MPH = 0x9D;
    static const uint8_t SYM_DEGR = 0x08;
    static const uint8_t SYM_PCNT = 0x25;
    static const uint8_t SYM_RPM = 0x12;
    static const uint8_t SYM_ASPD = 0x41;
    static const uint8_t SYM_GSPD = 0x47;
    static const uint8_t SYM_WSPD = 0x57;
    static const uint8_t SYM_VSPD = 0x5E;
    static const uint8_t SYM_WPNO = 0x23;
    static const uint8_t SYM_WPDIR = 0xE6;
    static const uint8_t SYM_WPDST = 0xE7;
    static const uint8_t SYM_FTMIN = 0xE8;
    static const uint8_t SYM_FTSEC = 0x99;

    static const uint8_t SYM_SAT_L = 0x1E;
    static const uint8_t SYM_SAT_R = 0x1F;
    static const uint8_t SYM_HDOP_L = 0x48;
    static const uint8_t SYM_HDOP_R = 0x44;

    static const uint8_t SYM_HOME = 0x11;
    static const uint8_t SYM_WIND = 0x57;

    static const uint8_t SYM_ARROW_START = 0x60;
    static const uint8_t SYM_ARROW_COUNT = 16;
    static const uint8_t SYM_AH_H_START = 0x80;
    static const uint8_t SYM_AH_H_COUNT = 9;

    static const uint8_t SYM_AH_V_START = 0x82;
    static const uint8_t SYM_AH_V_COUNT = 6;

    static const uint8_t SYM_AH_CENTER_LINE_LEFT = 0x84;
    static const uint8_t SYM_AH_CENTER_LINE_RIGHT = 0x84;
    static const uint8_t SYM_AH_CENTER = 0x2B;

    static const uint8_t SYM_HEADING_N = 0x18;
    static const uint8_t SYM_HEADING_S = 0x19;
    static const uint8_t SYM_HEADING_E = 0x1A;
    static const uint8_t SYM_HEADING_W = 0x1B;
    static const uint8_t SYM_HEADING_DIVIDED_LINE = 0x1C;
    static const uint8_t SYM_HEADING_LINE = 0x1D;

    static const uint8_t SYM_UP_UP = 0x68;
    static const uint8_t SYM_UP = 0x68;
    static const uint8_t SYM_DOWN = 0x60;
    static const uint8_t SYM_DOWN_DOWN = 0x60;

    static const uint8_t SYM_DEGREES_C = 0x0E;
    static const uint8_t SYM_DEGREES_F = 0x0D;
    static const uint8_t SYM_GPS_LAT = 0x89;
    static const uint8_t SYM_GPS_LONG = 0x98;
    static const uint8_t SYM_ARMED = 0x00;
    static const uint8_t SYM_DISARMED = 0x2A;
    static const uint8_t SYM_ROLL0 = 0x2D;
    static const uint8_t SYM_ROLLR = 0x64;
    static const uint8_t SYM_ROLLL = 0x6C;
    static const uint8_t SYM_PTCH0 = 0x7C;
    static const uint8_t SYM_PTCHUP = 0x68;
    static const uint8_t SYM_PTCHDWN = 0x60;
    static const uint8_t SYM_XERR = 0x21;
    static const uint8_t SYM_KN = 0xF0;
    static const uint8_t SYM_NM = 0xF1;
    static const uint8_t SYM_DIST = 0x04;
    static const uint8_t SYM_FLY = 0x9C;
    static const uint8_t SYM_EFF = 0xF2;
    static const uint8_t SYM_AH = 0xF3;
    static const uint8_t SYM_MW = 0xF4;
    static const uint8_t SYM_CLK = 0x08;
    static const uint8_t SYM_KILO = 0x4B;
    static const uint8_t SYM_TERALT = 0x7F;
    static const uint8_t SYM_FENCE_ENABLED = 0xF5;
    static const uint8_t SYM_FENCE_DISABLED = 0xF6;
    static const uint8_t SYM_RNGFD = 0x7F;
    static const uint8_t SYM_LQ = 0xF8;

    static const uint8_t SYM_SIDEBAR_L_ARROW = 0x02;
    static const uint8_t SYM_SIDEBAR_R_ARROW = 0x03;
    static const uint8_t SYM_SIDEBAR_A = 0x13;
    static const uint8_t SYM_SIDEBAR_B = SYM_SIDEBAR_A;
    static const uint8_t SYM_SIDEBAR_C = SYM_SIDEBAR_A;
    static const uint8_t SYM_SIDEBAR_D = SYM_SIDEBAR_A;
    static const uint8_t SYM_SIDEBAR_E = SYM_SIDEBAR_A;
    static const uint8_t SYM_SIDEBAR_F = SYM_SIDEBAR_A;
    static const uint8_t SYM_SIDEBAR_G = SYM_SIDEBAR_A;
    static const uint8_t SYM_SIDEBAR_H = SYM_SIDEBAR_A;
    static const uint8_t SYM_SIDEBAR_I = SYM_SIDEBAR_A;
    static const uint8_t SYM_SIDEBAR_J = SYM_SIDEBAR_A;


    static constexpr uint8_t symbols[AP_OSD_NUM_SYMBOLS] {
        SYM_M,
        SYM_KM,
        SYM_FT,
        SYM_MI,
        SYM_ALT_M,
        SYM_ALT_FT,
        SYM_BATT_FULL,
        SYM_RSSI,
        SYM_VOLT,
        SYM_AMP,
        SYM_MAH,
        SYM_MS,
        SYM_FS,
        SYM_KMH,
        SYM_MPH,
        SYM_DEGR,
        SYM_PCNT,
        SYM_RPM,
        SYM_ASPD,
        SYM_GSPD,
        SYM_WSPD,
        SYM_VSPD,
        SYM_WPNO,
        SYM_WPDIR,
        SYM_WPDST,
        SYM_FTMIN,
        SYM_FTSEC,
        SYM_SAT_L,
        SYM_SAT_R,
        SYM_HDOP_L,
        SYM_HDOP_R,
        SYM_HOME,
        SYM_WIND,
        SYM_ARROW_START,
        SYM_ARROW_COUNT,
        SYM_AH_H_START,
        SYM_AH_H_COUNT,
        SYM_AH_V_START,
        SYM_AH_V_COUNT,
        SYM_AH_CENTER_LINE_LEFT,
        SYM_AH_CENTER_LINE_RIGHT,
        SYM_AH_CENTER,
        SYM_HEADING_N,
        SYM_HEADING_S,
        SYM_HEADING_E,
        SYM_HEADING_W,
        SYM_HEADING_DIVIDED_LINE,
        SYM_HEADING_LINE,
        SYM_UP_UP,
        SYM_UP,
        SYM_DOWN,
        SYM_DOWN_DOWN,
        SYM_DEGREES_C,
        SYM_DEGREES_F,
        SYM_GPS_LAT,
        SYM_GPS_LONG,
        SYM_ARMED,
        SYM_DISARMED,
        SYM_ROLL0,
        SYM_ROLLR,
        SYM_ROLLL,
        SYM_PTCH0,
        SYM_PTCHUP,
        SYM_PTCHDWN,
        SYM_XERR,
        SYM_KN,
        SYM_NM,
        SYM_DIST,
        SYM_FLY,
        SYM_EFF,
        SYM_AH,
        SYM_MW,
        SYM_CLK,
        SYM_KILO,
        SYM_TERALT,
        SYM_FENCE_ENABLED,
        SYM_FENCE_DISABLED,
        SYM_RNGFD,
        SYM_LQ,
        SYM_SIDEBAR_L_ARROW,
        SYM_SIDEBAR_R_ARROW,
        SYM_SIDEBAR_A,
        SYM_SIDEBAR_B,
        SYM_SIDEBAR_C,
        SYM_SIDEBAR_D,
        SYM_SIDEBAR_E,
        SYM_SIDEBAR_F,
        SYM_SIDEBAR_G,
        SYM_SIDEBAR_H,
        SYM_SIDEBAR_I,
        SYM_SIDEBAR_J,
    };

3. 接口设计

3.1 一次性接口

3.1.1 init

查询配置为SerialProtocol_MSP_DisplayPort的UART端口,进行相关_displayport端口及配置初始化。

// initialise backend
bool AP_OSD_MSP_DisplayPort::init(void)
{
    // check if we have a DisplayPort backend to use
    const AP_MSP *msp = AP::msp();
    if (msp == nullptr) {
        GCS_SEND_TEXT(MAV_SEVERITY_WARNING,"MSP backend not available");
        return false;
    }
    _displayport = msp->find_protocol(AP_SerialManager::SerialProtocol_MSP_DisplayPort);
    if (_displayport == nullptr) {
        GCS_SEND_TEXT(MAV_SEVERITY_WARNING,"MSP DisplayPort uart not available");
        return false;
    }
    // re-init port here for use in this thread
    _displayport->init_uart();
    return true;
}

3.1.2 osd_thread_run_once

前面3.1.1函数在这里进行一次性的接口初始化

// called by the OSD thread once
void AP_OSD_MSP_DisplayPort::osd_thread_run_once()
{
    if (_displayport != nullptr) {
        _displayport->init_uart();
    }
}

3.1.3 init_symbol_set

若选择配置了DISPLAYPORT_BTFL_SYMBOLS,则用继承类的符号表;否则,用Ardupilot原生(AP_OSD_Backend)的符号表。

void AP_OSD_MSP_DisplayPort::init_symbol_set(uint8_t *lookup_table, const uint8_t size)
{
    const AP_MSP *msp = AP::msp();
    // do we use backend specific symbols table?
    if (msp && msp->is_option_enabled(AP_MSP::Option::DISPLAYPORT_BTFL_SYMBOLS)) {
        memcpy(lookup_table, symbols, size);
    } else {
        memcpy(lookup_table, AP_OSD_Backend::symbols, size);
    }
}

3.1.4 probe

最初的初始化,直接在AP_OSD::init_backend中进行,详见:ArduPilot开源代码之AP_OSD

AP_OSD_Backend *AP_OSD_MSP_DisplayPort::probe(AP_OSD &osd)
{
    AP_OSD_MSP_DisplayPort *backend = NEW_NOTHROW AP_OSD_MSP_DisplayPort(osd);
    if (!backend) {
        return nullptr;
    }
    if (!backend->init()) {
        delete backend;
        return nullptr;
    }
    return backend;
}

3.2 基本操作

3.2.1 write

直接调用了AP_MSP_Telem_Backend的写字符串函数处理。

void AP_OSD_MSP_DisplayPort::write(uint8_t x, uint8_t y, const char* text)
{
    _displayport->msp_displayport_write_string(x, y, 0, text);
}

3.2.2 flush

直接调用了AP_MSP_Telem_Backend的API函数处理。

void AP_OSD_MSP_DisplayPort::flush(void)
{
    // grab the screen and force a redraw
    _displayport->msp_displayport_grab();
    _displayport->msp_displayport_draw_screen();

    // ok done processing displayport data
    // let's process incoming MSP frames (and reply if needed)
    _displayport->process_incoming_data();
}

3.2.3 clear

直接调用了AP_MSP_Telem_Backend的API函数处理,并且_blink_on做了一个定期翻转。

void AP_OSD_MSP_DisplayPort::clear(void)
{
    // check if we need to enable some options
    // but only for actual OSD screens
    if (_osd.get_current_screen() < AP_OSD_NUM_DISPLAY_SCREENS) {
        const uint8_t txt_resolution = _osd.screen[_osd.get_current_screen()].get_txt_resolution();
        const uint8_t font_index = _osd.screen[_osd.get_current_screen()].get_font_index();
        _displayport->msp_displayport_set_options(font_index, txt_resolution);
    }

    // clear remote MSP screen
    _displayport->msp_displayport_clear_screen();

    // toggle flashing @1Hz
    const uint32_t now = AP_HAL::millis();
    if ((uint32_t(now * 0.004) & 0x01) != _blink_on) {
        _blink_on = !_blink_on;
        blink_phase = (blink_phase+1)%4;
    }
}

按照以下方式修改,可能会更加清晰:

  • 每秒闪烁一次,即每 500ms 切换一次状态
  • 由于 now 是毫秒单位,now * 0.004 约等于每 250ms 变化一次整数
  • & 0x01 会让这个值每500ms翻转一次,实现1Hz的闪烁效果
now (ms)now * 0.004整数值& 0x01_blink_on
00000
2501111 (翻转)
5002200 (翻转)
7503311 (翻转)
10004400 (翻转)
const uint32_t now = AP_HAL::millis();
if ((now / 500) % 2 != _blink_on) {
    _blink_on = !_blink_on;
    blink_phase = (blink_phase + 1) % 4;
}

3.3 功能函数

3.3.1 is_compatible_with_backend_type

  • 不兼容:AP_OSD_MSP/AP_OSD_MSP_DISPLAYPORT
  • 兼容:AP_OSD_MAX7456/AP_OSD_SITL
    bool is_compatible_with_backend_type(AP_OSD::osd_types type) const override {
        switch(type) {
        case AP_OSD::osd_types::OSD_MSP:
        case AP_OSD::osd_types::OSD_MSP_DISPLAYPORT:
            return false;
        case AP_OSD::osd_types::OSD_NONE:
        case AP_OSD::osd_types::OSD_TXONLY:
        case AP_OSD::osd_types::OSD_MAX7456:
        case AP_OSD::osd_types::OSD_SITL:
            return true;
        }
        return false;
    }

3.3.2 get_backend_type

    AP_OSD::osd_types get_backend_type() const override {
        return AP_OSD::osd_types::OSD_MSP_DISPLAYPORT;
    }

3.3.3 get_aspect_ratio_correction

不懂,什么原因要设置这个比率,请知道的朋友,评论告知,谢谢! 请参考:ArduPilot开源代码之AP_OSD_MAX7456 对应章节解释。

// return a correction factor used to display angles correctly
float AP_OSD_MSP_DisplayPort::get_aspect_ratio_correction() const
{
    return 12.0/18.0;
}

4. 总结

AP_OSD_MSP_DisplayPort类有点类似设备对象(DO),然后协议层面是靠AP_MSP_Telem_Backend进行,应用层在AP_OSD处理。

5. 参考资料

【1】ArduPilot开源飞控系统之简单介绍
【2】ArduPilot之开源代码Task介绍
【3】ArduPilot飞控启动&运行过程简介
【4】ArduPilot之开源代码Library&Sketches设计
【5】ArduPilot之开源代码Sensor Drivers设计
【6】ArduPilot开源代码之AP_OSD_Backend

评论
添加红包

请填写红包祝福语或标题

红包个数最小为10个

红包金额最低5元

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

抵扣说明:

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

余额充值