網頁

2014年9月2日 星期二

Pixhawk Autopilot 第四步 讀取感測器的值(未完待續,9/10更新)

程序間溝通:
程序(Nodes)間的溝通訊息,是透過名為主題(Topic)的渠道來傳遞。一個Topic只包含一種訊息格式,程序可以在Topic上發佈訊息(傳送),也可以從Topic上訂閱訊息(接收),但他們並不知道自己在跟誰溝通。同一個Topic上可以有很多人發佈訊息,也可以很多人訂閱訊息。為增加效率,一個渠道上只有一個訊息,無駐列。

訊息的發送接收是利用Micro Object Request Broker(uORB)來達成,而uORB用來執行一個簡單的發佈-訂閱流程。


  1. 發佈一個主題(Topic):
    發佈一個主題包含三個動作:定義(define)、廣告(advertise)、發佈更新(publish updates)。
    1. 定義(define)
                寫一個.h檔(標頭檔),內容包含兩個動作:
      (1)用ORB_DECLARE()宣告主題的名稱。
      (2)定義主題的內容結構(structure)。
           範例:topic.h

      /* declare the topic */ORB_DECLARE(random_integer);

      /* define the data structure that will be published where subscribers can see it */

      struct random_integer_data{
             
               int r;
      };
             
      注意,主題的名稱最好有意義,並且利用底線隔在字與字之間,把最一般性的字眼擺在最前面,例如raw sensor data,最好取名為sensor_raw。

               定義一個主題,除了標頭檔,還必須在發佈主題的程式碼裡用ORB_DEFINE()建構主題資料結構。(解釋:"宣告"只是告訴編譯器變數的存在,"定義"是賦予變數初值,可以在同一行程式碼同時宣告並定義一個變數,但是在標頭檔裡,最好只是宣告變數而不要定義變數,否則若不只一個程式碼含入標頭檔時,會重複定義變數而產生錯誤訊息。在此處,標頭檔宣告有一個主題存在,但是在發佈主題的程式碼裡面才真正建立主題,而且一個主題只能建立一次,否則會發生錯誤。)

      主題的資料結構包含:主題名稱、資料大小。
          範例:publisher.c
      #include <topic.h>

      /* create topic metadata */
      ORB_DEFINE(random_integer, struct random_integer_data);
    2. 廣告(advertise)
              發佈主題前,需用orb_advertise()來廣告主題。

      int orb_advertise(const struct orb_metadata *meta, const void *data);
      廣告的同時,也發布了第一筆資料。

              第一個參數是一個指標,指向用ORB_DEFINE()建立出來的結構,這個指標由ORB_ID()函式來提供,ORB_ID()的輸入參數為主題的名稱。第二個參數為所要發佈的資料(須符合標頭檔定義的主題內容結構)。
      廣告主題不能用於中斷程序中。同一時間只能由一個發佈者廣告一個主題,且用close()結束之後才可由另一個發佈者廣告,close()的輸入參數為orb_advertise()函數的回傳值(handler)。
    3. 發佈主題(publish updates)
      orb_advertise()函式的回傳值(handler)可用來將新資料發佈。使用以下的函示:

      int orb_publish(const struct orb_metadata *meta, int handle, const void *data);

      第一個參數為ORB_ID()函式提供之指標。
      第二個參數為orb_advertise()函式的回傳值(handle)。
      第三個參數為所要發佈的資料。

              ORB並無資料暫存,多個發佈者發布資料時,訂閱者只能看到最新的一筆發佈資料。
  2. 訂閱(Subscribing)
    1. 訂閱(Subscribing)
              訂閱主題需先將定義主題的標頭檔含入,利用以下函式訂閱:

      int orb_subscribe(const struct orb_metadata *meta);
      輸入參數為ORB_ID()函式提供之指標。

              使用以下函式來取消訂閱:

      int orb_unsubscribe(int handle);
      輸入參數為orb_subscribe()函式的回傳值(handler)。
    2. 從主題複製資料
              使用者先宣告'一個用來儲存資料的變數(記憶體空間),再將主題上的資料複製到這個變數,利用以下函式來複製資料:

      int orb_copy(const struct orb_metadata *meta, int handle, void *buffer);
      第二個參數是orb_subscribe()函式的回傳值(handle)。
      第三個參數為預先宣告用來儲存資料的記憶體空間。

               複製的動作極微小且快速,可以保證取得的資料是最新的值。
    3. 檢查是否有更新
               利用以下函式,使用者可以檢查最後一次使用orb_copy()之後,主題資料是否有更新。

      int orb_check(int handle, bool *updated);
      第二個參數是orb_subscribe()函式的回傳值(handle)。
      第三個參數回傳false(未更新)、true(已更新)。
             
              利用以下函式,可以得知主題最後更新資料的時間。
      int orb_stat(int handle, uint64_t *time);
    4. 等待更新
               美一個主題的訂閱及發布,都可視為是對一個檔案的讀取和寫入,可以用poll()函式來判斷一個檔案是否已經可以讀取/寫入,poll()會在設定的timeout時間內等待並測是否個事件是否發生,等待期間城市不會繼續執行而是停留在poll()函式(block),直到poll()結束並返回。如果偵測到特定事件的發生,poll()返回並回傳值>0值,如果timeout,poll()返回並回傳0,如果錯誤poll()返回並回傳-1。

      #include <poll.h>
      int poll(struct pollfd fds[], nfds_t nfds, int timeout);
      第一個參數是一個包含檔案編號(file descriptor)的pollfd結構陣列。
      第二個參數是檔案個數。
      第三個參數是等待時間(毫秒,ms)。

              pollfd結構至少包含下列三個成員:
      int                 fd           檔案編號(file descriptor)
      short      events            輸入事件旗標
      short     revents            輸出事件旗標


              用poll()等待資料更新的使用方法如下:
      先訂閱主題並取得回傳值(handler),將回傳值(handler)作為檔案編號(file descriptor)定義pollfd結構陣列,並設定需要偵測的事件為POLLIN,POLLIN為資料可讀取,每當主題上的資料被更新,POLLIN事件即會發生。程式範例如下:

      int sensor_sub_fd = orb_subscribe(ORB_ID(snesor_combined));

      struct pollfd fds[] = {.fd = sensor_sub_fd, .events = POLLIN};

      while(true){
               int poll_ret  = poll(fds, 1, 1000);
               if(poll_ret > 0){
                      if(fds[0].revents & POLLIN){
                             ... ...
                      }
               }
      }
    5. 限制更新速率
      //////////////////未完待續//////////////////

2014年8月27日 星期三

Pixhawk Autopilot 第三步 建立一個背景執行緒(Daemon:task/thread)


  1. 建立一個背景執行緒
    1. 建立應用程式入口函式(管理函式),如:
      __EXPORT int px4_daemon_app_main(int argc, char *argv[]);
      int px4_daemon_app_main(int argc, char *argv[])
      {
         ...
      }
    2. 在入口函式中px4_daemon_app_main利用task_spawn_cmd函式建立一個新任務。輸入參數如下:
      task_spawn_cmd(
              const char *name,        ← 任務名稱
              int scheduler,                ← 排序方式:先進先出SCHED_FIFO或是SCHED_RR
              int priority,                    ← 優先順序
              int stack_size,               ←  設定一記憶體區塊,用來儲存執行緒位址及變數
              main_t entry,                 ← 執行緒本體函式名稱
              const char *argv[]  )      ← 輸入執行緒參數
    3. 建立執行緒本體函式,如:
      int px4_daemon_thread_main(int argc, char *argv[])
      int px4_daemon_thread_main(int argc, char *argv[])
      {
            while(ture){
                    ... ...
                    sleep(10);
                    if(thread_should_exit)  break;
            }
            return 0;
      }
    4. 在入口函式(管理函式)中加入指令用來控制執行緒,如:
      if (!strcmp(argv[1], "start")) {
              ... ...
              daemon_task = task_spawn_cmd(  ... ... );
              ... ...
      }

      上述程式碼表示開始執行緒任務。

      執行時輸入以下指令:
      NSH>   px4_daemon_app   start

      if (!strcmp(argv[1], "stop")) {
              ... ...
      }

      上述程式碼表示停止執行緒任務。

      執行時輸入以下指令:
      NSH>   px4_daemon_app   stop
      本範例執行後會每隔10ms顯示Hello daemon
    5. 查詢背景執行程式,如下:
      1. 執行時輸入以下指令:
        NSH>   top
      2. 顯示目前背景執行程序:
      3. 執行daemon後,再次執行top,將發現新的執行任務:
      4. 跳離top顯示畫面請按鍵盤Ctrl+C,此時輸入以下指令:
        NSH>   px4_daemon_app stop
        因為daemon每10ms自動顯示Hello daemon 所以當輸入指令被中斷時請忽視。
  2. 執行週期
             範例建立了兩個函式,一個是入口函式px4_daemon_app_main,二是執行緒本體函式px4_daemon_thread_main。入口函式只會執行一次,當執行相對應的指令完畢後就會消失。執行緒本體函式則利用while無窮迴圈持續執行,直到入口函式再次被執行,並以stop指令結束執行緒。
             所有執行緒本體函式所需用到的變數宣告及定義,都需要撰寫在執行緒本體函式中執行週期內看的到的地方,不可撰寫在入口函式內。

    註:此範例程式已存在px4/Firmeware/src/examples/px4_daemon_app資料夾下。完整程式請參考範例。

2014年8月24日 星期日

Pixhawk Autopilot 第二步 建立一個應用程式


  1. 檔案設定
    1. 若有更新OS程式碼,至少執行一次make archives
    2. 在Eclipse開發環境下,進入 Firmware/src/modules資料夾中,建立一個新資料夾,例如"px4_test_app",

      在這個資料夾中建立一個新make檔,檔名為"module.mk"。
      在module.mk中加入下列程式碼:
         MODULE_COMMAND = px4_test_app
         SRCS                            = px4_test_app.c
  2. 撰寫程式
    1. 在 Firmeware/src/module/px4_test.app/ 新增C程式碼檔。例如"px4_test_app"。

      Firmware
             └   src
                     └  module
                               └  px4_test_app
                                                └  module.mk
                                                └  px4_test_app.c
    2. 編輯C程式碼,最小程式碼須包含下列程式碼:
      /**
      * @file px4_test_app.c
      * Minimal appliction example for PX4 autopilot
      */
      #include <nuttx/config.h>
      #include <stdio.h>
      #include <errno.h>

      __EXPORT int px4_test_app_main(int argc, char *argv[]);

      int px4_test_app_main(int argc, char *argv[])
      {
           printf("Hello BC!\n");
           return OK;
      }
  3. 向NuttShell註冊應用程式並編譯
    1. 打開 "Firmware/makefiles/" 資料夾中 "config_px4fmu-v2_default.mk" 文件。
    2. 在文件中新增一行程式碼:
      MODULES         += moudles/px4_test_app
    3. 編譯程式碼:執行 clean → px4fmu-v2_defualt。如果沒有註冊新應用程式,只須執行px4fmu-v2_defualt。
    4. 上傳:執行 upload px4fmu-v2_defualt。當出現以下訊息的時候,重置Pixhawk飛控板(FMU Reset 按鈕長押 or USB重新拔插):
          Generating/Users/user/src/Firmware/Images/px4fmu.px4
          Loaded firmware for 9,0, waiting for the bootloader...

      出現以下訊息代表上傳成功:
          Found board 5,0 on /dev/tty.usbmodem1
          erase...
          program...
          verify...
          done, rebooting.
  4. 執行應用程式
    開啟TeraTerm,NSH> 下輸入 px4_test_app。

    註:此範例程式可參考 px4\Firmware\src\example\px4_simple_app資料夾下。完整程式請參考範例。

Pixhawk Autopilot 第一步 安裝及設定開發環境

  1. 安裝
    1. 更新JAVA:第7版第65次更新以上的版本。
    2. 安裝PX4 Toolchain for Windows。
      備註:作業系統 Windows 7
  2. 設定
    1. 取得起始程式碼:選擇"程式集 -> PX4 Toolchain -> PX4 Software Download"。
      備註:程式將在預設路徑建立下列資料夾:
      C:\px4
      └ Firmware:PX4 所有模組的韌體(可新增程式的地方),NuttX RTOS(不須更動)。
      └ libopencm3:Cortex M4 晶片的函式庫,Bootloader用。(不須更動)
      └ Bootloader:(不須更動)
    2. 第一次啟動:"程式集 -> PX4 Toolchain -> PX4 Eclipse"。
    3. 設定工作磁碟區:使用預設路徑C:\px4\workspace
    4. 設定專案(匯入專案Import):Flie -> New -> Makefile Project with Existing Code
      →瀏覽(Brows)並選擇資料夾 C:\px4\Firmware
      →在"Toolchain for Indexer Setting" 中選擇"Cross GCC"後按結束。
      本範例使用路徑為D:\
    5. 在Eclipse環境右方視窗中選擇"Make Target"標籤,選擇"Firmware"資料夾,並按下上方"New Make Target"的按鈕出現設定Make目標的視窗,在視窗中Target name中輸入目標名稱後按OK,勾選same as the target name。
    6. 重複執行上一步驟,設定下列目標(Target)名稱:
      distclean:清理所有程式,包含OS。
      archives:編譯NuttX OS。
      all:根據archives編譯自動駕駛系統全部軟體。
      clean:清理自動駕駛軟體,不包含OS。
      px4fmu-v2_default:編譯自動駕駛軟體。
      upload px4fmu-v2_default:上傳編譯後程式碼至PX4FMU v2.x 飛控板。
    7. Eclipse右方"Make Target" 視窗Firmware資料下出現上一步所建立之Make目標,雙擊滑鼠左鍵可以執行。
      依序執行 distclean→archives→all→upload px4fmu-v2_default
      註:
      若有新增使用者自定義app程式,須執行
      clean→px4fmu-v2_default→upload px4fmu-v2_default。不須重新編譯OS。
      若只是修改現有app程式,只須執行
      px4fmu-v2_default→upload px4fmu-v2_default。不須執行clean
    8. 上網下載最新Firmware程式碼。(可透過git,在此先不討論)
  3. 透過NuttShell(NSH)與飛控板OS溝通
    1. 利用USB連接電腦及Pixhawk飛控板。
    2. 開啟軟體:程式集 -> PX4 Toolchain -> TeraTerm
    3. 按Enter鍵。出現NSH> 符號。可輸入指令,按Enter執行。
  4. 參考文獻
    1. JAVA Installer 載點
    2. Toolchain Installer for Windows 載點
    3. Toolchain Installer 安裝步驟(原文)
    4. Toolchain Installer Video 影片