ArduPilot开源代码之AP

ArduPilot开源代码之AP_OSD

1. 源由
2. 简介
3. 补丁
4. 框架设计
- 4.1 启动代码 (AP_OSD::init)
- 4.2 任务代码 (AP_OSD::osd_thread)
- 4.3 实例初始化 (AP_OSD::init_backend)
5. 重要例程
- 5.1 AP_OSD::update_stats
- 5.2 AP_OSD::update_current_screen
- 5.3 AP_OSD::update_osd
6. 总结
7. 参考资料

1. 源由

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

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

Is it possible for two OSD resolution working at the same time?
How to setup two VTX (one for analog camera, another for digital camera)

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

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

2. 简介

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

#define OSD_MAX_INSTANCES 2

基于AP_OSD_Backend支持以下四种OSD类型：

OSD_MAX7456: AP_OSD_MAX7456
OSD_SITL: AP_OSD_SITL
OSD_MSP: AP_OSD_MSP
OSD_MSP_DISPLAYPORT: AP_OSD_MSP_DisplayPort

3. 补丁

但是通过AP_OSD: add two osd resolution concurrently support #29149 已经打破该困局，支持同一时刻，两种尺寸的OSD的同时显示：

Git Repo下如何制作一个patch文件

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

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

4. 框架设计

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

4.1 启动代码 (AP_OSD::init)

这里不做过多解释，详见：启动代码流程-ArduPilot飞控启动&运行过程简介

Copter::init_ardupilot
 └──> osd.init();

根据配置内容直接对实例类型进行赋值
然后针对对应的四种类型OSD进行初始化
创建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)

例程会首先执行osd_thread_run_once
然后每隔100ms唤醒对2个OSD实例进行更新
若disable OSD，则无需更新状态
对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()

6. 总结

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

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

不过，对于我们的测试样机：

ArduPilot开源飞控之lida2003-H743-5inch套机配置
ArduPilot开源飞控之lida2003-H743-5inch配置调整

这里已经整理了代码：

AP_OSD: add two osd resolution concurrently support
hwdef: enable two osd resolution concurrently feature for Aocoda-RC H743 target

$ 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

然后进行编译：