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
然后進行編譯:
- ArduPilot飛控AOCODARC-H7DUAL固件編譯
- Ardupilot開源飛控工程項目編譯回顧
7. 參考資料
【1】ArduPilot開源飛控系統之簡單介紹
【2】ArduPilot之開源代碼Task介紹
【3】ArduPilot飛控啟動&運行過程簡介
【4】ArduPilot之開源代碼Library&Sketches設計
【5】ArduPilot之開源代碼Sensor Drivers設計