ArduPilot開源代碼之AP_OSD

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();
  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: 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設計

本文來自互聯網用戶投稿,該文觀點僅代表作者本人,不代表本站立場。本站僅提供信息存儲空間服務,不擁有所有權,不承擔相關法律責任。
如若轉載,請注明出處:http://www.pswp.cn/pingmian/71971.shtml
繁體地址,請注明出處:http://hk.pswp.cn/pingmian/71971.shtml
英文地址,請注明出處:http://en.pswp.cn/pingmian/71971.shtml

如若內容造成侵權/違法違規/事實不符,請聯系多彩編程網進行投訴反饋email:809451989@qq.com,一經查實,立即刪除!

相關文章

qt open3dAlpha重建

qt open3dAlpha重建 效果展示二、流程三、代碼效果展示 二、流程 創建動作,鏈接到槽函數,并把動作放置菜單欄 參照前文 三、代碼 1、槽函數實現 void on_actionAlpha_triggered();//alpha重建 void MainWindow::

Deepseek可以通過多種方式幫助CAD加速工作

自動化操作&#xff1a;通過Deepseek的AI能力&#xff0c;可以編寫腳本來自動化重復性任務。例如&#xff0c;使用Python腳本調用Deepseek API&#xff0c;在CAD中實現自動化操作。 插件開發&#xff1a;結合Deepseek進行二次開發&#xff0c;可以創建自定義的CAD插件。例如&a…

Centos的ElasticSearch安裝教程

由于我們是用于校園學習&#xff0c;所以最好是關閉防火墻 systemctl stop firewalld systemctl disable firewalld 個人喜歡安裝在opt臨時目錄&#xff0c;大家可以隨意 在opt目錄下創建一個es-standonely-docker目錄 mkdir es-standonely-docker 進入目錄編輯yml文件 se…

c++ 調用 gurobi 庫,cmake,mac

gurobi 一般使用 python 調用&#xff0c;官方的培訓會議及資料大部分也都基于 python。 由于最近上手了 c&#xff0c;因此想試試 c 怎么調用 gurobi。但我發現&#xff0c;c 調用第三方庫比 python 或 java 要復雜不少。python 中直接 import 第三方庫&#xff0c;java 加載…

Python基于Django的醫用耗材網上申領系統【附源碼、文檔說明】

博主介紹&#xff1a;?Java老徐、7年大廠程序員經歷。全網粉絲12w、csdn博客專家、掘金/華為云/阿里云/InfoQ等平臺優質作者、專注于Java技術領域和畢業項目實戰? &#x1f345;文末獲取源碼聯系&#x1f345; &#x1f447;&#x1f3fb; 精彩專欄推薦訂閱&#x1f447;&…

Python中很常用的100個函數整理

Python 內置函數提供了強大的工具&#xff0c;涵蓋數據處理、數學運算、迭代控制、類型轉換等。本文總結了 100 個常用內置函數&#xff0c;并配備示例代碼&#xff0c;提高編程效率。 1. abs() 取絕對值 print(abs(-10)) # 10 2. all() 判斷所有元素是否為真 print(all([…

Python畢業設計選題:基于django+vue的疫情數據可視化分析系統

開發語言&#xff1a;Python框架&#xff1a;djangoPython版本&#xff1a;python3.7.7數據庫&#xff1a;mysql 5.7數據庫工具&#xff1a;Navicat11開發軟件&#xff1a;PyCharm 系統展示 管理員登錄 管理員功能界面 用戶管理 員工管理 疫情信息管理 檢測預約管理 檢測結果…

C#程序結構及基本組成說明

C# 程序的結構主要由以下幾個部分組成,以下是對其結構的詳細說明和示例: 1. 基本組成部分 命名空間 (Namespace) 用于組織代碼,避免命名沖突。通過 using 引入其他命名空間。 using System; // 引入 System 命名空間類 (Class) C# 是面向對象的語言,所有代碼必須定義在類或…

Python 編程題 第八節:字符串變形、壓縮字符串、三個數的最大乘積、判定字符是否唯一、IP地址轉換

字符串變形 swapcase()方法將字符串大小寫轉換&#xff1b;split()方法將字符串以括號內的符號分隔并以列表形式返回 sinput() ls.split(" ") ll[::-1] s"" for i in l:ai.swapcase()sas" " print(s[0:len(s)-1]) 壓縮字符串 很巧妙的方法 …

大語言模型學習--向量數據庫基礎知識

1.向量 向量是多維數據空間中的一個坐標點。 向量類型 圖像向量 文本向量 語音向量 Embedding 非結構化數據轉換為向量過程 通過深度學習訓練&#xff0c;將真實世界離散數據&#xff0c;投影到高維數據空間上&#xff0c;通過數據在空間中間的距離體現真實世界的相似度 V…

項目工坊 | Python驅動淘寶信息爬蟲

目錄 前言 1 完整代碼 2 代碼解讀 2.1 導入模塊 2.2 定義 TaoBao 類 2.3 search_infor_price_from_web 方法 2.3.1 獲取下載路徑 2.3.2 設置瀏覽器選項 2.3.3 反爬蟲處理 2.3.4 啟動瀏覽器 2.3.5 修改瀏覽器屬性 2.3.6 設置下載行為 2.3.7 打開淘寶登錄頁面 2.3.…

藍橋杯題型

藍橋杯 藍橋杯題型分類語法基礎藝術與籃球&#xff08;日期問題&#xff09;時間顯示&#xff08;時間問題&#xff09;跑步計劃&#xff08;日期問題&#xff09;偶串(字符&#xff09;最長子序列&#xff08;字符&#xff09;字母數&#xff08;進制轉換&#xff09;6個0&…

【C語言】文件操作篇

目錄 文件的基本概念文本文件和二進制文件的差異 文件指針FILE 結構體文件指針的初始化和賦值 文件打開與關閉常見操作文件的打開文件的關閉 常見問題打開文件時的路徑問題打開文件失敗的常見原因fclose 函數的重要性 文件讀寫操作常見操作字符讀寫字符串讀寫格式化讀寫二進制讀…

【leetcode hot 100 21】合并兩個有序鏈表

解法一&#xff1a;新建一個鏈表存放有序的合并鏈表。當list1和list2至少有一個非空時&#xff0c;返回非空的&#xff1b;否則找出兩個鏈表的最小值作為新鏈表的頭&#xff0c;然后依次比較兩鏈表&#xff0c;每次都先插入小的值。 /*** Definition for singly-linked list.*…

Ubuntu 24.04.2 安裝 PostgreSQL 16 、PostGIS 3

安裝 PostgreSQL 16 apt install postgresql-16passwd postgres&#xff0c;修改 postgres 用戶密碼su postgrespsql -U postgres, 以 postgres 的身份登錄數據庫alter user postgres with password abc123;\q 退出/etc/postgresql/16/main/postgresql.conf 可修改 #listen_ad…

Spring Boot框架總結(超級詳細)

前言 本篇文章包含Springboot配置文件解釋、熱部署、自動裝配原理源碼級剖析、內嵌tomcat源碼級剖析、緩存深入、多環境部署等等&#xff0c;如果能耐心看完&#xff0c;想必會有不少收獲。 一、Spring Boot基礎應用 Spring Boot特征 概念&#xff1a; 約定優于配置&#…

postgresql14編譯安裝腳本

#!/bin/bash####################################readme################################### #先上傳postgresql源碼包&#xff0c;再配置yum源&#xff0c;然后執行腳本 #備份官方yum源配置文件&#xff1a; #cp /etc/yum.repos.d/CentOS-Base.repo /etc/yum.repos.d/CentOS…

AI開發利器:miniforge3無感平替Anaconda3

相信有和我遭遇一樣的同學吧&#xff0c;之前裝了anaconda用的挺好的&#xff08;可以參考AI開發利器&#xff1a;Anaconda&#xff09;&#xff0c;但是考慮到有可能收到軟件侵權的律師函的風險&#xff0c;還是果斷找個替代品把anaconda卸載掉。miniforge就是在這樣的背景下發…

Reactor中的Flux和Mono的區別

Reactor中的Flux和Mono的區別 在Reactor框架中&#xff0c;Flux 和 Mono 是兩個核心的類型&#xff0c;分別用于處理不同的數據流場景。理解它們之間的區別是掌握響應式編程的關鍵。 1. 基本概念 Flux: 表示一個異步、非阻塞的流&#xff0c;能夠發布零個或多個元素。它適用于…

AI-NAS:當存儲遇上智能,開啟數據管理新紀元

在數據爆炸的時代&#xff0c;NAS&#xff08;網絡附加存儲&#xff09;已成為個人和企業存儲海量數據的利器。然而&#xff0c;面對日益龐大的數據量&#xff0c;傳統的NAS系統在文件管理和搜索效率上逐漸力不從心。AI-NAS應運而生&#xff0c;它將NAS與人工智能&#xff08;A…