平時工作忙，也沒有太多時間寫部落格，最近研究ardupilot源碼，做了個實驗，往源碼裡面加一條測試指令，作為調試方法的驗證，特記錄一下。
1.在ArduCopter檔案裡，建立subMenuTest.cpp源檔案，用來實作指令所進行的操作。如圖：
2.在建立的檔案中，添加以下代碼:
/*
* subMenuTest.cpp
*
* Created on: 2016-4-29
* Author: zwk
*/
//#include<AP_Menu.h>
#include "Copter.h"
//當鍵入指令test時，執行對應的操作函數menuTest，此函數統一在Copter.h檔案中定義。
int8_t Copter::menuTest(uint8_t argc,const Menu::arg *argv)
{
hal.console->printf("Hello World\n");
return ;
}
//當鍵入指令tell時，執行對應的操作函數menuTell，此函數統一在Copter.h檔案中定義。
int8_t Copter::menuTell(uint8_t argc,const Menu::arg *argv)
{
for(int i = ;i < argc;++i)
{
hal.console->printf("%s ",argv[i].str);
}
hal.console->printf("\n");
return ;
}
//command function table for the subMenuTest Menu
static const struct Menu::command subMenuTestCommands[] = {
{"test", MENU_FUNC(menuTest)}, //menuTest為指令test對應的操作函數
{"tell", MENU_FUNC(menuTell)}, //menuTell為指令tell對應的操作函數
};
// Create the subMenuTest object.
MENU(subMenuTest,"subMenuTest",subMenuTestCommands);
//當鍵入指令subMenuTest時，執行對應的操作函數subMenuTest_mode，此函數統一在Copter.h檔案中定義。
int8_t Copter::subMenuTest_mode(uint8_t argc,const Menu::arg *argv)
{
hal.console->printf("Commands:\n"
"test\n"
"tell"
"\n");
subMenuTest.set_limits(,);
subMenuTest.run();
return ;
}
3.打開檔案system.cpp,分别在main_menu_help和main_menu_commands函數中，将建立的指令subMenuTest添加進去。代碼如下
（1）main_menu_help函數
// This is the help function
int8_t Copter::main_menu_help(uint8_t argc, const Menu::arg *argv)
{
cliSerial->printf("Commands:\n"
" logs\n"
" setup\n"
" test\n"
" subMenuTest"
" reboot\n"
"\n");
return();
}
（2）main_menu_commands函數
// Command/function table for the top-level menu.
const struct Menu::command main_menu_commands[] = {
// command function called
// ======= ===============
{"logs", MENU_FUNC(process_logs)},
{"setup", MENU_FUNC(setup_mode)},
{"test", MENU_FUNC(test_mode)},
{"subMenuTest", MENU_FUNC(subMenuTest_mode)},
{"reboot", MENU_FUNC(reboot_board)},
{"help", MENU_FUNC(main_menu_help)},
};
4.打開Copter.h檔案，在合适的地方，添加新增指令對應函數的聲明，代碼為：
int8_t subMenuTest_mode(uint8_t argc,const Menu::arg *argv);
int8_t menuTest(uint8_t argc,const Menu::arg *argv);
int8_t menuTell(uint8_t argc,const Menu::arg *argv);
5.此時，添加指令的代碼部分基本完成，後面對整個工程進行編譯，依次點選makeTarget中的px4-clean和px4-v2,編譯過程如圖所示：
樓主電腦限制，編譯時間比較長。。
6.打開MissionPlaner，下載下傳編譯好的固件（下載下傳固件時不能連接配接mavlink）,如圖：
7.修改參數。MissionPlane中，如果要使用終端子產品進行調試的話，要使參數CLI_ENABLED的值為1=Enabled.不然終端調試出來的資料是亂碼。設定方法如圖所示：
8.參數修改後，重新連接配接飛控到電腦，打開終端，電機連接配接，即可進入終端控制模式。下面是我根據新加的指令，做的一些測試。
最後，這些方法也是我在google上學習後加以修改的，如果有什麼不對的地方，還望大家能夠包含并批評指正。