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: StandardAP_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: StandardAP_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: StandardAP_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: StandardAP_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同时显示更新，需定制固件。

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

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

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

然后进行编译：