目录

ArduPilot开源代码之AP_OSD

ArduPilot开源代码之AP_OSD

1. 源由

因为自己有两个摄像头:模拟+数字;而数字OpenIPC地面端Jetson-fpv还不太成熟,所以暂时还想使用模拟的飞,等稳定了切换成数字。

问题是数字录像还是要的,以便更好的了解OpenIPC作为数字图传的效果。

从代码和咨询的角度看,似乎Ardupilot并不支持同时两个OSD在不同分辨率的情况下工作。

所以,还是需要从代码入手,DIY玩的就是“心跳”,对吧。

2. 简介

最多支持两种 OSD 实例切换,不支持同一时刻,两种OSD同时使用。

#define OSD_MAX_INSTANCES 2

基于 支持以下四种OSD类型:

3. 补丁

但是通过 已经打破该困局,支持同一时刻,两种尺寸的OSD的同时显示:

patch 分享/更好的差异化比较,减少宝贵Review时间浪费,也是对代码熟悉程度的体现。

另外,也可以作为系统集成的差异化补丁,快速实现本地集成、编译、测试、验证等。

4. 框架设计

接下来,我们来看下该模块的设计。

4.1 启动代码 (AP_OSD::init)

这里不做过多解释,详见:启动代码流程-

Copter::init_ardupilot
 └──> osd.init();
  1. 根据配置内容直接对实例类型进行赋值
  2. 然后针对对应的四种类型OSD进行初始化
  3. 创建 osd_thread 任务,依据优先级执行例程
AP_OSD::init
 ├──> const AP_OSD::osd_types types[OSD_MAX_INSTANCES] = {
       osd_types(osd_type.get()),
       osd_types(osd_type2.get())
   };
 ├──> for <instance < OSD_MAX_INSTANCES>
    ├──> init_backend(types[instance], instance)
    └──> _backend_count++;
 └──> <_backend_count > 0>
     └──> hal.scheduler->thread_create(FUNCTOR_BIND_MEMBER(&AP_OSD::osd_thread, void), "OSD", 1280, AP_HAL::Scheduler::PRIORITY_IO, 1);

4.2 任务代码 (AP_OSD::osd_thread)

  1. 例程会首先执行 osd_thread_run_once
  2. 然后每隔100ms唤醒对2个OSD实例进行更新
  3. disable OSD,则无需更新状态
  4. 对OSD Overlay进行刷新

 ├──> for <instance < OSD_MAX_INSTANCES>
    └──> _backends[instance]->osd_thread_run_once()
 └──> <loop>
     ├──> hal.scheduler->delay(100)
     ├──> <!_disable>
        ├──> update_stats();
        └──> update_current_screen();
     └──> update_osd()

4.3 实例初始化 (AP_OSD::init_backend)

每种实例需要对软硬件进行除此环境设定,这里采用类似 probe 的方式执行。

  • 若,probe失败,则该OSD实例为空指针
  • 若,该实例与第一个默认OSD实例冲突,则第二个实例不进行初始化
AP_OSD::init_backend
 ├──> <instance > 0 && _backends[0] && !_backends[0]->is_compatible_with_backend_type(type)>
    └──> return false; // 第二种类型OSD与默认第一类OSD不兼容
 ├──> <switch>
    ├──> <OSD_NONE> break
    ├──> <OSD_TXONLY> break
    ├──> <OSD_MAX7456> <HAL_WITH_SPI_OSD> <HAL_WITH_OSD_BITMAP>
       ├──> AP_HAL::OwnPtr<AP_HAL::Device> spi_dev = std::move(hal.spi->get_device("osd"));
       ├──> _backends[instance] = AP_OSD_MAX7456::probe(*this, std::move(spi_dev))
       └──>  break
    ├──> <WITH_SITL_OSD> 
       ├──> _backends[instance] = AP_OSD_SITL::probe(*this)
       └──>  break
    ├──> <OSD_MSP> 
       ├──> _backends[instance] = AP_OSD_MSP::probe(*this);
       └──>  break
    └──> <OSD_MSP_DISPLAYPORT> <HAL_WITH_MSP_DISPLAYPORT>
        ├──> _backends[instance] = AP_OSD_MSP_DisplayPort::probe(*this)
        └──>  break
 ├──> <OSD_ENABLED && _backends[instance] != nullptr>
    ├──> _backends[instance]->init_symbol_set(AP_OSD_AbstractScreen::symbols_lookup_table, AP_OSD_NUM_SYMBOLS)
    └──> return true;
 └──> return false;

5. 重要例程

5.1 AP_OSD::update_stats

对系统内部的参数进行定期更新

  • 地速
  • 位置
  • 高度
  • 空速
  • 飞行距离
  • 最大地面速度
  • 最大高度
  • 最大直线与HOME的距离
  • 最大电流
  • 最小电压
  • 最小RSSI
  • 最大空速
  • 最大ESC温度
AP_OSD::update_stats
 ├──> WITH_SEMAPHORE(_sem);
 ├──> uint32_t now = AP_HAL::millis();
 ├──> <!AP_Notify::flags.armed> //没有启动,则无需更新状态
    ├──> _stats.last_update_ms = now;
    └──> return;
 
 ├──> [更新delta_ms]
    ├──> uint32_t delta_ms = now - _stats.last_update_ms;
    ├──> _stats.last_update_ms = now;
    
    ├──> Vector2f v;
    ├──> Location loc {};
    ├──> Location home_loc;
    ├──> bool home_is_set;
    ├──> bool have_airspeed_estimate;
    ├──> float alt;
    ├──> float aspd_mps = 0.0f;
    └──> [获取地速、HOME以及当前位置、高度、空速]
        ├──> AP_AHRS &ahrs = AP::ahrs();
        ├──> WITH_SEMAPHORE(ahrs.get_semaphore()); // minimize semaphore scope
        ├──> v = ahrs.groundspeed_vector();
        ├──> home_is_set = ahrs.get_location(loc) && ahrs.home_is_set();
        ├──> <home_is_set>
           └──>home_loc = ahrs.get_home();
        ├──> ahrs.get_relative_position_D_home(alt);
        └──> have_airspeed_estimate = ahrs.airspeed_estimate(aspd_mps);
 
 ├──> [飞行距离更新]
    ├──> float speed = v.length();
    ├──> <speed < 0.178>
       └──> speed = 0.0;
    ├──> float dist_m = (speed * delta_ms)*0.001;
    └──> _stats.last_distance_m += dist_m;
 
 ├──> [最大地面速度更新]
    └──> _stats.max_speed_mps = fmaxf(_stats.max_speed_mps,speed);
 
 ├──> [最大距离HOME位置更新]<home_is_set>
    ├──> float distance = home_loc.get_distance(loc);
    └──> _stats.max_dist_m = fmaxf(_stats.max_dist_m, distance);
 
 ├──> [最大高度更新]
    ├──> alt = -alt;
    └──> _stats.max_alt_m = fmaxf(_stats.max_alt_m, alt);
 
 ├──> <AP_BATTERY_ENABLED>
    ├──> [最大电流更新]
       ├──> AP_BattMonitor &battery = AP::battery();
       ├──> float amps;
       └──> <battery.current_amps(amps)>  _stats.max_current_a = fmaxf(_stats.max_current_a, amps)
    └──> [最小电压更新]
        ├──> float voltage = battery.voltage();
        └──> <voltage > 0> _stats.min_voltage_v = fminf(_stats.min_voltage_v, voltage)
 
 ├──> <AP_RSSI_ENABLED>
    └──> [最小RSSI更新]
        ├──> AP_RSSI *ap_rssi = AP_RSSI::get_singleton();
        └──> <ap_rssi> _stats.min_rssi = fminf(_stats.min_rssi, ap_rssi->read_receiver_rssi());
 
 ├──> [最大空速更新]
    └──> <have_airspeed_estimate> _stats.max_airspeed_mps = fmaxf(_stats.max_airspeed_mps, aspd_mps);
 
 └──> <HAL_WITH_ESC_TELEM>
     └──> [最大ESC温度更新]
         ├──> AP_ESC_Telem& telem = AP::esc_telem();
         ├──> int16_t highest_temperature = 0;
         ├──> telem.get_highest_temperature(highest_temperature);
         └──> _stats.max_esc_temp = MAX(_stats.max_esc_temp, highest_temperature);

5.2 AP_OSD::update_current_screen

默认配置情况下:

  • arm_scr = 0
  • disarm_scr = 0
  • failsafe_scr = 0
  • rc_channel = 0
    // @Param: _CHAN
    // @DisplayName: Screen switch transmitter channel
    // @Description: This sets the channel used to switch different OSD screens.
    // @Values: 0:Disable,5:Chan5,6:Chan6,7:Chan7,8:Chan8,9:Chan9,10:Chan10,11:Chan11,12:Chan12,13:Chan13,14:Chan14,15:Chan15,16:Chan16
    // @User: Standard
    AP_GROUPINFO("_CHAN", 2, AP_OSD, rc_channel, 0),
    
    // @Param: _ARM_SCR
    // @DisplayName: Arm screen
    // @Description: Screen to be shown on Arm event. Zero to disable the feature.
    // @Range: 0 4
    // @User: Standard
    AP_GROUPINFO("_ARM_SCR", 17, AP_OSD, arm_scr, 0),

    // @Param: _DSARM_SCR
    // @DisplayName: Disarm screen
    // @Description: Screen to be shown on disarm event. Zero to disable the feature.
    // @Range: 0 4
    // @User: Standard
    AP_GROUPINFO("_DSARM_SCR", 18, AP_OSD, disarm_scr, 0),

    // @Param: _FS_SCR
    // @DisplayName: Failsafe screen
    // @Description: Screen to be shown on failsafe event. Zero to disable the feature.
    // @Range: 0 4
    // @User: Standard
    AP_GROUPINFO("_FS_SCR", 19, AP_OSD, failsafe_scr, 0),

所以,默认情况以下代码不执行。

AP_OSD::update_current_screen
 ├──> [Switch on ARM/DISARM event]
    ├──> <AP_Notify::flags.armed>
       ├──> <!was_armed && arm_scr > 0 
              && arm_scr <= AP_OSD_NUM_DISPLAY_SCREENS 
              && get_screen(arm_scr-1).enabled>
          └──> current_screen = arm_scr-1;
       └──> was_armed = true;
    └──> <was_armed>
        ├──> <disarm_scr > 0>
               && disarm_scr <= AP_OSD_NUM_DISPLAY_SCREENS 
               && get_screen(disarm_scr-1).enabled>
           └──> current_screen = disarm_scr-1;
        └──> was_armed = false;
 
 ├──> [Switch on failsafe event]
    ├──> <AP_Notify::flags.failsafe_radio 
         || AP_Notify::flags.failsafe_battery>
       ├──> <!was_failsafe && failsafe_scr > 0 
              && failsafe_scr <= AP_OSD_NUM_DISPLAY_SCREENS 
              && get_screen(failsafe_scr-1).enabled>
          ├──> pre_fs_screen = current_screen;
          └──> current_screen = failsafe_scr-1;
       └──> was_failsafe = true;
    └──> <was_failsafe>
        ├──> <get_screen(pre_fs_screen).enabled>
           └──> current_screen = pre_fs_screen;
        └──> was_failsafe = false;
 
 ├──> <rc_channel == 0> return
 
 └──> <AP_RC_CHANNEL_ENABLED>
     ├──> RC_Channel *channel = RC_Channels::rc_channel(rc_channel-1);
     ├──> <channel == nullptr> return;
     ├──> int16_t channel_value = channel->get_radio_in();
     ├──> [switch (sw_method)] 
        ├──> <default/TOGGLE> //switch to next screen if channel value was changed
           ├──> <previous_channel_value == 0>
              └──> previous_channel_value = channel_value; //do not switch to the next screen just after initialization
           └──> <abs(channel_value-previous_channel_value) > 200>
               ├──> switch_debouncer) {
                  ├──> next_screen();
                  └──> previous_channel_value = channel_value;
               └──> <else>
                   ├──> switch_debouncer = true;
                   └──> return;
        
        ├──> <PWM_RANGE> //select screen based on pwm ranges specified
           └──> <for (int i=0; i<AP_OSD_NUM_SCREENS; i++>
               └──> <get_screen(i).enabled 
                    && get_screen(i).channel_min <= channel_value 
                    && get_screen(i).channel_max > channel_value>
                   ├──> <previous_pwm_screen == i>
                      └──> break;
                   └──> <else>
                       └──> current_screen = previous_pwm_screen = i;
        
        └──> <AUTO_SWITCH> //switch to next screen after low to high transition and every 1s while channel value is high
            ├──> <channel_value > channel->get_radio_trim()>
               ├──> <switch_debouncer>
                  ├──> uint32_t now = AP_HAL::millis();
                  └──> <now - last_switch_ms > 1000>
                      ├──> next_screen();
                      └──> last_switch_ms = now;
               └──> <else>
                   ├──> switch_debouncer = true;
                   └──> return;
            └──> <else>
                └──>last_switch_ms = 0;
     └──> switch_debouncer = false;

5.3 AP_OSD::update_osd

默认初始化时, current_screen = 0

所以,从这里可以看出,始终更行 current_screen 对应的OSD。

AP_OSD::update_osd
 └──> for <instance < _backend_count>
     ├──> _backends[instance]->clear()
     ├──> <!_disable>
        ├──> get_screen(current_screen).set_backend(_backends[instance])
        └──> <_backends[instance]->get_backend_type() != OSD_MSP> get_screen(current_screen).draw()
     └──> _backends[instance]->flush()

注:应用层面OSD更新在 get_screen(current_screen).draw() 中实现,这里不再展开,后续将在其他章节介绍。

6. 总结

为了让Ardupilot代码支持模拟+数字OSD同时显示更新,需定制固件。

目前,前面提及的补丁尚未合入,且存在一个模拟越来越少使用的问题,要合入可能也存在一定的苦难。

不过,对于我们的测试样机:

这里已经整理了代码:

$ git clone git@github.com:SnapDragonfly/ardupilot.git
or
$ git clone https://github.com/SnapDragonfly/ardupilot.git
$ cd ardupilot
$ git checkout Copter-4.5-lida2003

然后进行编译:

7. 参考资料

【1】

【2】

【3】

【4】

【5】