ArduCopter调试
Posted bbzz2
tags:
篇首语：本文由小常识网(cha138.com)小编为大家整理，主要介绍了ArduCopter调试相关的知识，希望对你有一定的参考价值。
1.ArduPilot_main我们知道，在 C语言中最经典的程序是 “Hello World!”，这应该是我们在 C语言中最早接触的一个程序了。而在单片机中，最经典的一个程序当属 LED了。那么这里我们为什么不去分析 “Hello World！”或者是分析 LED呢？这个，其实我没有看到 ArduPilot的相关源码中有这么一个程序，而且，LED的话我目前还没有看到 PC4中是怎么使用的。只知道 PX4是通过 I2C控制 LED的，而且这是通过看 PCB得出的，跟我们经典的 LED并不一样。
前面我们多次看到过 g_builtins数组，其中有一个成员 ArduPilot，这个成员比较特殊，从其名字就可以看出。其对应的入口函数为： “ArduPilot_main”，但实际上我们找不到这样一个函数。我们却可以匹配到这样的信息：
bitcraze@bitcraze-vm:~/apm/ardupilot/ArduCopter$ grep -nr ArduPilot_main ../mk/
../mk/vrbrain_targets.mk:44:SKETCHFLAGS=$(SKETCHLIBINCLUDES) -I$(PWD) -DARDUPILOT_BUILD -DCONFIG_HAL_BOARD=HAL_BOARD_VRBRAIN -DSKETCHNAME="\\\\\\"$(SKETCH)\\\\\\"" -DSKETCH_MAIN=ArduPilot_main -DAPM_BUILD_DIRECTORY=APM_BUILD_$(SKETCH)
../mk/px4_targets.mk:39:SKETCHFLAGS=$(SKETCHLIBINCLUDES) -I$(PWD) -DARDUPILOT_BUILD -DCONFIG_HAL_BOARD=HAL_BOARD_PX4 -DSKETCHNAME="\\\\\\"$(SKETCH)\\\\\\"" -DSKETCH_MAIN=ArduPilot_main -DAPM_BUILD_DIRECTORY=APM_BUILD_$(SKETCH)
bitcraze@bitcraze-vm:~/apm/ardupilot/ArduCopter$
“-DSKETCH_MAIN=ArduPilot_main”就是这里的关键。在 C语言中我们通常通过 “#define”来定义一个宏，但是熟悉 Makefile的都知道实际上我们也可以在编译命令中通过 “-D”来定义一个宏。通过匹配，我们可以得到如下信息：
bitcraze@bitcraze-vm:~/apm/ardupilot/ArduCopter$ grep -nr SKETCH_MAIN ../../
../../ardupilot/mk/vrbrain_targets.mk:44:SKETCHFLAGS=$(SKETCHLIBINCLUDES) -I$(PWD) -DARDUPILOT_BUILD -DCONFIG_HAL_BOARD=HAL_BOARD_VRBRAIN -DSKETCHNAME="\\\\\\"$(SKETCH)\\\\\\"" -DSKETCH_MAIN=ArduPilot_main -DAPM_BUILD_DIRECTORY=APM_BUILD_$(SKETCH)
../../ardupilot/mk/px4_targets.mk:39:SKETCHFLAGS=$(SKETCHLIBINCLUDES) -I$(PWD) -DARDUPILOT_BUILD -DCONFIG_HAL_BOARD=HAL_BOARD_PX4 -DSKETCHNAME="\\\\\\"$(SKETCH)\\\\\\"" -DSKETCH_MAIN=ArduPilot_main -DAPM_BUILD_DIRECTORY=APM_BUILD_$(SKETCH)
../../ardupilot/libraries/AP_HAL_PX4/AP_HAL_PX4_Main.h:7: extern "C" __EXPORTint SKETCH_MAIN(int argc, char *const argv[]); \\
../../ardupilot/libraries/AP_HAL_PX4/AP_HAL_PX4_Main.h:8: int SKETCH_MAIN(int argc,char * const argv[]) \\
../../ardupilot/libraries/AP_HAL_VRBRAIN/AP_HAL_VRBRAIN_Main.h:7: extern "C" __EXPORTint SKETCH_MAIN(int argc, char *const argv[]); \\
../../ardupilot/libraries/AP_HAL_VRBRAIN/AP_HAL_VRBRAIN_Main.h:8: int SKETCH_MAIN(int argc,char * const argv[]) \\
bitcraze@bitcraze-vm:~/apm/ardupilot/ArduCopter$
我们可以到对应的头文件中去查看具体的源码：
#ifndef __AP_HAL_PX4_MAIN_H__
#define __AP_HAL_PX4_MAIN_H__
#if CONFIG_HAL_BOARD == HAL_BOARD_PX4
#define AP_HAL_MAIN() \\
extern "C" __EXPORT int SKETCH_MAIN(int argc,char * const argv[]); \\
int SKETCH_MAIN(int argc, char *const argv[]) \\
hal.init(argc, argv); \\
return OK; \\
#endif
#endif // __AP_HAL_PX4_MAIN_H__
#ifndef __AP_HAL_VRBRAIN_MAIN_H__
#define __AP_HAL_VRBRAIN_MAIN_H__
#if CONFIG_HAL_BOARD == HAL_BOARD_VRBRAIN
#define AP_HAL_MAIN() \\
extern "C" __EXPORT int SKETCH_MAIN(int argc,char * const argv[]); \\
int SKETCH_MAIN(int argc, char *const argv[]) \\
hal.init(argc, argv); \\
return OK; \\
#endif
#endif // __AP_HAL_VRBRAIN_MAIN_H__
因为不管宏 "CONFIG_HAL_BOARD"的定义是什么这里的源码都是一样的，至于其它部分会不会改变源码的编译这个只有去匹配源码了。
bitcraze@bitcraze-vm:~/apm$ grep -nr CONFIG_HAL_BOARD ./ | grep define
./linux/ardupilot/libraries/AP_BattMonitor/AP_BattMonitor.h:37:#elif CONFIG_HAL_BOARD == HAL_BOARD_PX4 &&defined(CONFIG_ARCH_BOARD_PX4FMU_V1)
./linux/ardupilot/libraries/AP_BattMonitor/AP_BattMonitor.h:43:#elif CONFIG_HAL_BOARD == HAL_BOARD_PX4 &&defined(CONFIG_ARCH_BOARD_PX4FMU_V2)
./linux/ardupilot/libraries/AP_BattMonitor/AP_BattMonitor.h:54:#elif CONFIG_HAL_BOARD == HAL_BOARD_VRBRAIN &&defined(CONFIG_ARCH_BOARD_VRBRAIN_V40)
./linux/ardupilot/libraries/AP_BattMonitor/AP_BattMonitor.h:59:#elif CONFIG_HAL_BOARD == HAL_BOARD_VRBRAIN &&defined(CONFIG_ARCH_BOARD_VRBRAIN_V45)
./linux/ardupilot/libraries/AP_BattMonitor/AP_BattMonitor.h:64:#elif CONFIG_HAL_BOARD == HAL_BOARD_VRBRAIN &&defined(CONFIG_ARCH_BOARD_VRBRAIN_V50)
./linux/ardupilot/libraries/AP_BattMonitor/AP_BattMonitor.h:69:#elif CONFIG_HAL_BOARD == HAL_BOARD_VRBRAIN &&defined(CONFIG_ARCH_BOARD_VRBRAIN_V51)
./linux/ardupilot/libraries/AP_BattMonitor/AP_BattMonitor.h:74:#elif CONFIG_HAL_BOARD == HAL_BOARD_VRBRAIN &&defined(CONFIG_ARCH_BOARD_VRUBRAIN_V51)
./linux/ardupilot/libraries/AP_BattMonitor/AP_BattMonitor.h:79:#elif CONFIG_HAL_BOARD == HAL_BOARD_VRBRAIN &&defined(CONFIG_ARCH_BOARD_VRHERO_V10)
./linux/ardupilot/ArduCopter/config.h:48:#error CONFIG_HAL_BOARD must be defined to build ArduCopter
./ardupilot/libraries/AP_BattMonitor/AP_BattMonitor.h:37:#elif CONFIG_HAL_BOARD == HAL_BOARD_PX4 &&defined(CONFIG_ARCH_BOARD_PX4FMU_V1)
./ardupilot/libraries/AP_BattMonitor/AP_BattMonitor.h:43:#elif CONFIG_HAL_BOARD == HAL_BOARD_PX4 &&defined(CONFIG_ARCH_BOARD_PX4FMU_V2)
./ardupilot/libraries/AP_BattMonitor/AP_BattMonitor.h:54:#elif CONFIG_HAL_BOARD == HAL_BOARD_VRBRAIN &&defined(CONFIG_ARCH_BOARD_VRBRAIN_V40)
./ardupilot/libraries/AP_BattMonitor/AP_BattMonitor.h:59:#elif CONFIG_HAL_BOARD == HAL_BOARD_VRBRAIN &&defined(CONFIG_ARCH_BOARD_VRBRAIN_V45)
./ardupilot/libraries/AP_BattMonitor/AP_BattMonitor.h:64:#elif CONFIG_HAL_BOARD == HAL_BOARD_VRBRAIN &&defined(CONFIG_ARCH_BOARD_VRBRAIN_V50)
./ardupilot/libraries/AP_BattMonitor/AP_BattMonitor.h:69:#elif CONFIG_HAL_BOARD == HAL_BOARD_VRBRAIN &&defined(CONFIG_ARCH_BOARD_VRBRAIN_V51)
./ardupilot/libraries/AP_BattMonitor/AP_BattMonitor.h:74:#elif CONFIG_HAL_BOARD == HAL_BOARD_VRBRAIN &&defined(CONFIG_ARCH_BOARD_VRUBRAIN_V51)
./ardupilot/libraries/AP_BattMonitor/AP_BattMonitor.h:79:#elif CONFIG_HAL_BOARD == HAL_BOARD_VRBRAIN &&defined(CONFIG_ARCH_BOARD_VRHERO_V10)
./ardupilot/ArduCopter/config.h:48:#error CONFIG_HAL_BOARD must be defined to build ArduCopter
bitcraze@bitcraze-vm:~/apm$
我们看到并没有匹配到，开始我也以为是没有定义，但这根本就说不过去。前面我们也说过，定义一个宏并不一定就得用 “define”，那么可想而知一定是在 mk文件中定义的。于是：
bitcraze@bitcraze-vm:~/apm$ grep -nr CONFIG_HAL_BOARD ./ardupilot/mk/
./ardupilot/mk/vrbrain_targets.mk:44:SKETCHFLAGS=$(SKETCHLIBINCLUDES) -I$(PWD) -DARDUPILOT_BUILD -DCONFIG_HAL_BOARD=HAL_BOARD_VRBRAIN -DSKETCHNAME="\\\\\\"$(SKETCH)\\\\\\"" -DSKETCH_MAIN=ArduPilot_main -DAPM_BUILD_DIRECTORY=APM_BUILD_$(SKETCH)
./ardupilot/mk/board_avr.mk:12:DEFINES += -DCONFIG_HAL_BOARD=$(HAL_BOARD)
./ardupilot/mk/board_native.mk:11:DEFINES += -DCONFIG_HAL_BOARD=$(HAL_BOARD) -DCONFIG_HAL_BOARD_SUBTYPE=$(HAL_BOARD_SUBTYPE)
./ardupilot/mk/board_flymaple.mk:37:DEFINES += -DCONFIG_HAL_BOARD=$(HAL_BOARD)
./ardupilot/mk/px4_targets.mk:39:SKETCHFLAGS=$(SKETCHLIBINCLUDES) -I$(PWD) -DARDUPILOT_BUILD -DCONFIG_HAL_BOARD=HAL_BOARD_PX4 -DSKETCHNAME="\\\\\\"$(SKETCH)\\\\\\"" -DSKETCH_MAIN=ArduPilot_main -DAPM_BUILD_DIRECTORY=APM_BUILD_$(SKETCH)
bitcraze@bitcraze-vm:~/apm$
这样我们就知道宏 "CONFIG_HAL_BOARD"为 "HAL_BOARD_PX4"。
然后我们可以在 "ardupilot/ArduCopter/ArduCopter.pde"源文件中看到这样一段源码：
static void tuning()
AP_HAL_MAIN();
这里很容易就把 "AP_HAL_MAIN();"当成是一个函数调用，但是你要清楚这里是一个宏。将其展开就是：
extern "C" __EXPORT int ArduPilot_main (int argc,char * const argv[]);
int ArduPilot_main (int argc, char *const argv[])
hal.init(argc, argv);
return OK;
这时候 "ArduPilot_main"函数终于浮出水面了。到这里我们也就清楚为什么在源码总找不到这个函数，因为这个函数根本就不是在源码中写的。
那么这里的 "hal"又是什么呢？
2. hal
这时候我们就不要用 "grep"命令去匹配了，匹配就够躲到可以让你疯掉。
于是这里用到另外一个工具： Source Insight。反正我是挺喜欢用。
通过 Source Insight我们可以找到下面的信息：
这里我们找到两个名为 "ArduCopter"的源文件，一个是 "ped"文件，另一个是 "cpp"文件。这两个文件是有关系的。这就又涉及到编译脚本了。
bitcraze@bitcraze-vm:~/apm/ardupilot/ArduCopter$ grep -nr pde ../mk/
../mk/sketch_sources.mk:8:SKETCHPDESRCS := $(wildcard $(SRCROOT)/*.pde $(SRCROOT)/*.ino)
../mk/sketch_sources.mk:10:SKETCHPDE := $(wildcard $(SRCROOT)/$(SKETCH).pde $(SRCROOT)/$(SKETCH).ino)
../mk/sketch_sources.mk:13:$(error ERROR: sketch $(SKETCH) must contain exactly one of $(SKETCH).pde or $(SKETCH).ino)
../mk/sketch_sources.mk:90:# header and body of the concatenated .pde/.ino files. It also
../mk/sketch_sources.mk:126:# prototypes from the concantenated .pde/.ino files.
bitcraze@bitcraze-vm:~/apm/ardupilot/ArduCopter$
我们找到这一点 Makefile内容：
# Sketch source files
SKETCHPDESRCS := $(wildcard $(SRCROOT)/*.pde $(SRCROOT)/*.ino)
SKETCHSRCS := $(wildcard $(addprefix $(SRCROOT)/,$(SRCSUFFIXES)))
SKETCHPDE := $(wildcard $(SRCROOT)/$(SKETCH).pde $(SRCROOT)/$(SKETCH).ino)
SKETCHCPP := $(BUILDROOT)/$(SKETCH).cpp
ifneq ($(words $(SKETCHPDE)),1)
$(error ERROR: sketch $(SKETCH) must contain exactly one of $(SKETCH).pde or $(SKETCH).ino)
endif
那么 "SKETCHCPP"就是这里的关键了。
#
# Build the sketch.cpp file
$(SKETCHCPP): showflags $(SKETCHCPP_SRC)
@echo "building $(SKETCHCPP)"
$(RULEHDR)
$(v)$(AWK) -v mode=header '$(SKETCH_SPLITTER)' $(SKETCHCPP_SRC) > $@.new
$(v)echo "#line 1 \\"autogenerated\\"" >> $@.new
$(v)$(AWK) '$(SKETCH_PROTOTYPER)' $(SKETCHCPP_SRC) >> $@.new
$(v)$(AWK) -v mode=body '$(SKETCH_SPLITTER)' $(SKETCHCPP_SRC) >> $@.new
$(v)cmp $@ $@.new > /dev/null 2>&1 || mv $@.new $@
$(v)rm -f $@.new
从这段编译脚本中我们知道，这里会生成一个新的文件，结合前面的内容我们知道这里是一个 ".cpp"源文件.其实就是将所有用到的 ".pde"文件通过重定向得到一个源文件,所以我们在阅读 ".pde"源码的时候经常会发现这样的现象，明明是一个 "static"的全局变量，却偏偏在其它源文件中可以使用。原因就在这里。至于这里的生成的到底是哪一个 ".cpp"文件，我们将其打印出来就知道了，而不必挨个去分析脚本。
实际上从这里我们也知道， ".ped"源文件是不会被编译的，真正编译的也只是处理过后的 ".cpp"源文件。那么同样的道理，刚下载来的源码或者是执行了 "make clean"命令的源码中通常没有该文件。
而现在，我们关注的东西已经变成了 "AP_HAL_BOARD_DRIVER"，从其命名来看应该是负责跟硬件打交道的一些接口，说是 API其实也没错。
bitcraze@bitcraze-vm:~/apm/ardupilot/ArduCopter$ grep -nr AP_HAL_BOARD_DRIVER ../ | grepdefine
../libraries/AP_HAL/AP_HAL_Boards.h:79: define AP_HAL_BOARD_DRIVER to the right hal typefor this
../libraries/AP_HAL/AP_HAL_Boards.h:85:#define AP_HAL_BOARD_DRIVER AP_HAL_AVR_APM1
../libraries/AP_HAL/AP_HAL_Boards.h:95:#define AP_HAL_BOARD_DRIVER AP_HAL_AVR_APM2
../libraries/AP_HAL/AP_HAL_Boards.h:109:#define AP_HAL_BOARD_DRIVER AP_HAL_AVR_SITL
../libraries/AP_HAL/AP_HAL_Boards.h:121:#define AP_HAL_BOARD_DRIVER AP_HAL_FLYMAPLE
../libraries/AP_HAL/AP_HAL_Boards.h:132:#define AP_HAL_BOARD_DRIVER AP_HAL_PX4
../libraries/AP_HAL/AP_HAL_Boards.h:145:#define AP_HAL_BOARD_DRIVER AP_HAL_Linux
../libraries/AP_HAL/AP_HAL_Boards.h:164:#define AP_HAL_BOARD_DRIVER AP_HAL_Empty
../libraries/AP_HAL/AP_HAL_Boards.h:174:#define AP_HAL_BOARD_DRIVER AP_HAL_VRBRAIN
bitcraze@bitcraze-vm:~/apm/ardupilot/ArduCopter$
看到这个匹配结构我们和容易想到这是通过宏 "CONFIG_HAL_BOARD"进行条件编译的。
#elif CONFIG_HAL_BOARD == HAL_BOARD_PX4
#define AP_HAL_BOARD_DRIVER AP_HAL_PX4
#define HAL_BOARD_NAME "PX4"
#define HAL_CPU_CLASS HAL_CPU_CLASS_150
#define HAL_OS_POSIX_IO 1
#define HAL_STORAGE_SIZE 4096
#define HAL_STORAGE_SIZE_AVAILABLE HAL_STORAGE_SIZE
#define HAL_BOARD_LOG_DIRECTORY "/fs/microsd/APM/LOGS"
#define HAL_INS_DEFAULT HAL_INS_PX4
#define HAL_BARO_DEFAULT HAL_BARO_PX4
#define HAL_COMPASS_DEFAULT HAL_COMPASS_PX4
#define HAL_SERIAL0_BAUD_DEFAULT 115200
这里是硬件的一些配置信息。最简单的我们知道串口波特率，知道日志存放的路径，还知道的堆栈大小。
最后我们找到 "AP_HAL_BOARD_DRIVER"的定义为： "const HAL_PX4 AP_HAL_PX4;"。那么最终我们得到了 "hal.init(argc, argv);"这条语句所调用的函数为：
void HAL_PX4::init(int argc, char *const argv[]) const
int i;
const char *deviceA = UARTA_DEFAULT_DEVICE;
const char *deviceC = UARTC_DEFAULT_DEVICE;
const char *deviceD = UARTD_DEFAULT_DEVICE;
const char *deviceE = UARTE_DEFAULT_DEVICE;
if (argc < 1)
printf("%s: missing command (try '%s start')",
SKETCHNAME, SKETCHNAME);
usage();
exit(1);
for (i=0; i<argc; i++)
if (strcmp(argv[i], "start") == 0)
if (thread_running)
printf("%s already running\\n", SKETCHNAME);
/* this is not an error */
exit(0);
uartADriver.set_device_path(deviceA);
uartCDriver.set_device_path(deviceC);
uartDDriver.set_device_path(deviceD);
uartEDriver.set_device_path(deviceE);
printf("Starting %s uartA=%s uartC=%s uartD=%s uartE=%s\\n",
SKETCHNAME, deviceA, deviceC, deviceD, deviceE);
_px4_thread_should_exit = false;
daemon_task = task_spawn_cmd(SKETCHNAME,
SCHED_FIFO,
APM_MAIN_PRIORITY,
8192,
main_loop,
NULL);
exit(0);
if (strcmp(argv[i], "stop") == 0)
_px4_thread_should_exit = true;
exit(0);
if (strcmp(argv[i], "status") == 0)
if (_px4_thread_should_exit && thread_running)
printf("\\t%s is exiting\\n", SKETCHNAME);
else if (thread_running)
printf("\\t%s is running\\n", SKETCHNAME);
else
printf("\\t%s is not started\\n", SKETCHNAME);
exit(0);
if (strcmp(argv[i], "-d") == 0)
// set terminal device
if (argc > i + 1)
deviceA = strdup(argv[i+1]);
else
printf("missing parameter to -d DEVICE\\n");
usage();
exit(1);
if (strcmp(argv[i], "-d2") == 0)
// set uartC terminal device
if (argc > i + 1)
deviceC = strdup(argv[i+1]);
else
printf("missing parameter to -d2 DEVICE\\n");
usage();
exit(1);
if (strcmp(argv[i], "-d3") == 0)
// set uartD terminal device
if (argc > i + 1)
deviceD = strdup(argv[i+1]);
else
printf("missing parameter to -d3 DEVICE\\n");
usage();
exit(1);
if (strcmp(argv[i], "-d4") == 0)
// set uartE 2nd GPS device
if (argc > i + 1)
deviceE = strdup(argv[i+1]);
else
printf("missing parameter to -d4 DEVICE\\n");
usage();
exit(1);
usage();
exit(1);
我们先来看下启动 "ArduPilot"的脚本：
echo Starting ArduPilot $deviceA $deviceC $deviceD
if ArduPilot -d $deviceA -d2 $deviceC -d3 $deviceD start
then
echo ArduPilot started OK
else
sh /etc/init.d/rc.error
fi
那么这里的 "Init"函数我们就比较好理解，设置串口，并启动一个新的任务，说是线程其实也差不多。另外停止运行跟查询状态这两个个还是比较容易看懂。
新的任务，其入口函数自然就是 "main_loop"了。但是这个时候如果你按照看 C语言的习惯直接就去看 "main_loop"函数你可能会觉得莫名其妙，因为：
static int main_loop(int argc, char **argv)
extern void setup(void);
extern void loop(void);
hal.uartA->begin(115200);
hal.uartB->begin(38400);
hal.uartC->begin(57600);
hal.uartD->begin(57600);
hal.uartE->begin(57600);
hal.scheduler->init(NULL);
hal.rcin->init(NULL);
hal.rcout->init(NULL);
hal.analogin->init(NULL);
hal.gpio->init();
/*
run setup() at low priority to ensure CLI doesn't hang the
system, and to allow initial sensor read loops to run
*/
set_priority(APM_STARTUP_PRIORITY);
schedulerInstance.hal_initialized();
setup();
hal.scheduler->system_initialized();
perf_counter_t perf_loop = perf_alloc(PC_ELAPSED, "APM_loop");
perf_counter_t perf_overrun = perf_alloc(PC_COUNT, "APM_overrun");
struct hrt_call loop_overtime_call;
thread_running = true;
/*
switch to high priority for main loop
*/
set_priority(APM_MAIN_PRIORITY);
while (!_px4_thread_should_exit)
perf_begin(perf_loop);
/*
this ensures a tight loop waiting on a lower priority driver
will eventually give up some time for the driver to run. It
will only ever be called if a loop() call runs for more than
0.1 second
*/
hrt_call_after(&loop_overtime_call, 100000, (hrt_callout)loop_overtime, NULL);
loop();
if (px4_ran_overtime)
/*
we ran over 1s in loop(), and our priority was lowered
to let a driver run. Set it back to high priority now.
*/
set_priority(APM_MAIN_PRIORITY);
perf_count(perf_overrun);
px4_ran_overtime = false;
perf_end(perf_loop);
/*
give up 500 microseconds of time, to ensure drivers get a
chance to run. This relies on the accurate semaphore wait
using hrt in semaphore.cpp
*/
hal.scheduler->delay_microseconds(500);
thread_running = false;
return 0;
然后你肯定会回过头去查看刚看过的代码，结果：
class HAL_PX4 : public AP_HAL::HAL
public:
HAL_PX4();
void init(int argc, char *const argv[]) const;
;
由于 C++存在继承，那么我们很自然就想到刚才那些指针应该是其父类定义了的。实际情况也正是这样：
class AP_HAL::HAL
public:
HAL(AP_HAL::UARTDriver* _uartA, // console
AP_HAL::UARTDriver* _uartB, // 1st GPS
AP_HAL::UARTDriver* _uartC, // telem1
AP_HAL::UARTDriver* _uartD, // telem2
AP_HAL::UARTDriver* _uartE, // 2nd GPS
AP_HAL::I2CDriver* _i2c,
AP_HAL::SPIDeviceManager* _spi,
AP_HAL::AnalogIn* _analogin,
AP_HAL::Storage* _storage,
AP_HAL::UARTDriver* _console,
AP_HAL::GPIO* _gpio,
AP_HAL::RCInput* _rcin,
AP_HAL::RCOutput* _rcout,
AP_HAL::Scheduler* _scheduler,
AP_HAL::Util* _util)
:
uartA(_uartA),
uartB(_uartB),
uartC(_uartC),
uartD(_uartD),
uartE(_uartE),
i2c(_i2c),
spi(_spi),
analogin(_analogin),
storage(_storage),
console(_console),
gpio(_gpio),
rcin(_rcin),
rcout(_rcout),
scheduler(_scheduler),
util(_util)
virtual void init(int argc, char *const argv[]) const = 0;
AP_HAL::UARTDriver* uartA;
AP_HAL::UARTDriver* uartB;
AP_HAL::UARTDriver* uartC;
AP_HAL::UARTDriver* uartD;
AP_HAL::UARTDriver* uartE;
AP_HAL::I2CDriver* i2c;
AP_HAL::SPIDeviceManager* spi;
AP_HAL::AnalogIn* analogin;
AP_HAL::Storage* storage;
AP_HAL::UARTDriver* console;
AP_HAL::GPIO* gpio;
AP_HAL::RCInput* rcin;
AP_HAL::RCOutput* rcout;
AP_HAL::Scheduler* scheduler;
AP_HAL::Util* util;
;
稍微了解一点 C++都知道 C++的类中有一个默认的构造函数即不带参数的，并且不需要显示地定义。所以只有调用这里定义好的构造函数才能初始化这些指针。但是我们好像没有看到调用这个构造函数。那么，问题在哪里？
答案还是在 "HAL_PX4"这个类，既然是一个类，那么很明显它也应该有自己的构造函数，那么它的构造函数呢？
HAL_PX4::HAL_PX4() :
AP_HAL::HAL(
&uartADriver, /* uartA */
&uartBDriver, /* uartB */
&uartCDriver, /* uartC */
&uartDDriver, /* uartD */
&uartEDriver, /* uartE */
&i2cDriver, /* i2c */
&spiDeviceManager, /* spi */
&analogIn, /* analogin */
&storageDriver, /* storage */
&uartADriver, /* console */
&gpioDriver, /* gpio */
&rcinDriver, /* rcinput */
&rcoutDriver, /* rcoutput */
&schedulerInstance, /* scheduler */
&utilInstance) /* util */
哈哈，这下终于清楚了，原来都是通过构造函数在玩这些鬼把戏的。
这下我们就知道 "main_loop"函数中用的那些指针都不是空指针了。
在 "main_loop"函数中我比较关系的就两个家伙：
extern void setup(void);
extern void loop(void);
玩过早起 APM的或者用过 arduino的肯定都知道里边就有这两个函数，而且我们认为用户程序就是从这两个函数开始的，就好像我们写 C认为 main函数是起点是一样的。其中 setup是用来做初始化工作的，而 loop就是主循环，实际上应该认为是一轮循环。
void setup()
cliSerial = hal.console;
// Load the default values of variables listed in var_info[]s
AP_Param::setup_sketch_defaults();
init_ardupilot();
// initialise the main loop scheduler
scheduler.init(&scheduler_tasks[0], sizeof(scheduler_tasks)/sizeof(scheduler_tasks[0]));
void loop()
// wait for an INS sample
// 等待数据,AP_InertialSensor_PX4 ins
if (!ins.wait_for_sample(1000))
Log_Write_Error(ERROR_SUBSYSTEM_MAIN, ERROR_CODE_MAIN_INS_DELAY);
return;
uint32_t timer = micros();
// check loop time
// 计算最大的循环时间..
perf_info_check_loop_time(timer - fast_loopTimer);
// used by PI Loops
G_Dt = (float)(timer - fast_loopTimer) / 1000000.f;
fast_loopTimer = timer; //记录当前循环时间...
// for mainloop failure monitoring
mainLoop_count++;
// Execute the fast loop
// ---------------------
fast_loop();
// tell the scheduler one tick has passed
scheduler.tick();
// run all the tasks that are due to run. Note that we only
// have to call this once per loop, as the tasks are scheduled
// in multiples of the main loop tick. So if they don't run on
// the first call to the scheduler they won't run on a later
// call until scheduler.tick() is called again
uint32_t time_available = (timer + MAIN_LOOP_MICROS) - micros(); //任务周期..
scheduler.run(time_available); //调度..
早期 APM是基于 arduino开发的，所以我们都是从这两个函数开始看。那么可以想见， ArduPilot绝对是PX4中最核心的一个任务了。当然它肯定也是最复杂的，这个就需要专门讨论了。
说到这里，可能我们都还没有搞明白 hal到底是什么。从 hal的定义我们知道里边除了一个初始化函数，全部都是硬件接口指针，也就是说是用来跟硬件打交道的。而 HAL跟据我个人推测应该是： "hardware abstract layer"，即硬件抽象层。所以 hal的上面是 ArduPilot，下面则是硬件。
1.loop
我们这里没有先去分析 setup，主要是初始化过程中很多东西说不清楚，既然说不清，那就不如不说。
当然，我这里也并不是真的为了分析 loop而来，而是想知道它是怎么去读取 MPU6050这个传感器的，为我们后面分析 mpu6050做准备。
那么，为什么是 MPU6050呢？因为在所有的这些传感器中 MPU6050相对来说是我比较熟悉的，之前在小四轴上用得也最多。人嘛不都是这样，做什么事都习惯从自己最熟悉的入手，要是实在没办法，那也只好硬着头皮上了。
void loop()
// wait for an INS sample
// 等待数据,AP_InertialSensor_PX4 ins
if (!ins.wait_for_sample(1000))
Log_Write_Error(ERROR_SUBSYSTEM_MAIN, ERROR_CODE_MAIN_INS_DELAY);
return;
uint32_t timer = micros();
// check loop time
// 计算最大的循环时间..
perf_info_check_loop_time(timer - fast_loopTimer);
// used by PI Loops
G_Dt = (float)(timer - fast_loopTimer) / 1000000.f;
fast_loopTimer = timer; //记录当前循环时间...
// for mainloop failure monitoring
mainLoop_count++;
// Execute the fast loop
// ---------------------
fast_loop();
// tell the scheduler one tick has passed
scheduler.tick();
// run all the tasks that are due to run. Note that we only
// have to call this once per loop, as the tasks are scheduled
// in multiples of the main loop tick. So if they don't run on
// the first call to the scheduler they won't run on a later
// call until scheduler.tick() is called again
uint32_t time_available = (timer + MAIN_LOOP_MICROS) - micros(); //任务周期..
scheduler.run(time_available); //调度..
主循环中我个人是把它分为这么四部分的：等数据、计算周期、快速循环、任务调度。
可能写过 Linux应用程序的人会觉得很奇怪，操作系统本身就有一个任务调度，为什么这里又有一个任务调度？通常在 Linux应用编程中除非我们基于某种目的主动放弃 CPU，否则是不需要调用调度相关的函数的。那么，这里干的是啥事呢？
要回答这个问题就涉及到前面的快速循环。既然有快速循环，那么相对地就有一个慢速循环。而慢速循环就是通过这里的调度来实现的。也就是说这里的任务跟 Nuttx系统是没有关系的，它是运行在 ArduPilot之内。或者你这样想， arduino不是跑系统的，那么你又要实现一个多任务你应该怎么做呢？因为没有人为你提供任务调度，那么你也只好自己去实现。有一种相对廉价的方式就是轮询。而这里用的就是轮询。
计算周期呢这个最简单，就是当前循环时间跟上一次循环时间做差，然后换算一些单位。
那么最后还有一个等数据，那么等的是啥数据呢？
#if CONFIG_INS_TYPE == HAL_INS_MPU6000
AP_InertialSensor_MPU6000 ins;
#elif CONFIG_INS_TYPE == HAL_INS_PX4
AP_InertialSensor_PX4 ins; //这里被编译...
#elif CONFIG_INS_TYPE == HAL_INS_VRBRAIN
AP_InertialSensor_VRBRAIN ins;
#elif CONFIG_INS_TYPE == HAL_INS_HIL
AP_InertialSensor_HIL ins;
#elif CONFIG_INS_TYPE == HAL_INS_OILPAN
AP_InertialSensor_Oilpan ins( &apm1_adc );
#elif CONFIG_INS_TYPE == HAL_INS_FLYMAPLE
AP_InertialSensor_Flymaple ins;
#elif CONFIG_INS_TYPE == HAL_INS_L3G4200D
AP_InertialSensor_L3G4200D ins;
#elif CONFIG_INS_TYPE == HAL_INS_MPU9250
AP_InertialSensor_MPU9250 ins;
#else
#error Unrecognised CONFIG_INS_TYPE setting.
#endif // CONFIG_INS_TYPE
这个时候我们已经不用测试都知道 ins实际上就是 "AP_InertialSensor_PX4"类型的。所以我们等数据的时候运行的是下面的代码：
bool AP_InertialSensor_PX4::wait_for_sample(uint16_t timeout_ms)
if (_sample_available())
return true;
uint32_t start = hal.scheduler->millis();
while ((hal.scheduler->millis() - start) < timeout_ms)
uint64_t tnow = hrt_absolute_time();
// we spin for the last timing_lag microseconds. Before that
// we yield the CPU to allow IO to happen
const uint16_t timing_lag = 400;
if (_last_sample_timestamp + _sample_time_usec > tnow+timing_lag)
hal.scheduler->delay_microseconds(_last_sample_timestamp + _sample_time_usec - (tnow+timing_lag));
if (_sample_available())
return true;
return false;
然后我回过头去看它的形参：1000，妈呀，居然是整整 1s！那么它真的会傻傻地在哪里等 1s吗？ 怎么可能！那早都炸机了。所以秘密就在 "_sample_available()"这个函数里边。
bool AP_InertialSensor_PX4::_sample_available(void)
uint64_t tnow = hrt_absolute_time();
while (tnow - _last_sample_timestamp > _sample_time_usec)
_have_sample_available = true;
_last_sample_timestamp += _sample_time_usec;
return _have_sample_available;
所以说 "_sample_time_usec"才是我们真正的等待时间。那是多长时间呢？我们可以去看其初始化：
uint16_t AP_InertialSensor_PX4::_init_sensor( Sample_rate sample_rate )
// assumes max 2 instances
_accel_fd[0] = open(ACCEL_DEVICE_PATH, O_RDONLY);
_accel_fd[1] = open(ACCEL_DEVICE_PATH "1", O_RDONLY);
_accel_fd[2] = open(ACCEL_DEVICE_PATH "2", O_RDONLY);
_gyro_fd[0] = open(GYRO_DEVICE_PATH, O_RDONLY);
_gyro_fd[1] = open(GYRO_DEVICE_PATH "1", O_RDONLY);
_gyro_fd[2] = open(GYRO_DEVICE_PATH "2", O_RDONLY);
_num_accel_instances = 0;
_num_gyro_instances = 0;
for (uint8_t i=0; i<INS_MAX_INSTANCES; i++)
if (_accel_fd[i] >= 0)
_num_accel_instances = i+1;
if (_gyro_fd[i] >= 0)
_num_gyro_instances = i+1;
if (_num_accel_instances == 0)
hal.scheduler->panic("Unable to open accel device " ACCEL_DEVICE_PATH);
if (_num_gyro_instances == 0)
hal.scheduler->panic("Unable to open gyro device " GYRO_DEVICE_PATH);
switch (sample_rate)
case RATE_50HZ:
_default_filter_hz = 15;
_sample_time_usec = 20000;
break;
case RATE_100HZ:
_default_filter_hz = 30;
_sample_time_usec = 10000;
break;
case RATE_200HZ:
_default_filter_hz = 30;
_sample_time_usec = 5000;
break;
case RATE_400HZ:
default:
_default_filter_hz = 30;
_sample_time_usec = 2500;
break;
_set_filter_frequency(_mpu6000_filter);
#if defined(CONFIG_ARCH_BOARD_PX4FMU_V2)
return AP_PRODUCT_ID_PX4_V2;
#else
return AP_PRODUCT_ID_PX4;
#endif
所以主要取决于我们选择多快的速率。但是在这里，我却以外地发现了另外两个东西： "_default_filter_hz"跟 "_accel_fd"。我们很容易就想到前者是滤波用的，而后者是用来操作加计的。那么现在的问题是，我们到底设置了一个多高的速率？
在 AP_InertialSensor_PX4类的父类中有这样一个函数：
void AP_InertialSensor::init( Start_style style,
Sample_rate sample_rate)
_product_id = _init_sensor(sample_rate);
// check scaling
for (uint8_t i=0; i<get_accel_count(); i++)
if (_accel_scale[i].get().is_zero())
_accel_scale[i].set(Vector3f(1,1,1));
if (WARM_START != style)
// do cold-start calibration for gyro only
_init_gyro();
不用我解释大家肯定都已经看出来这是一个初始化函数。有时候我不太喜欢 C++就是这个原因，父类子类，子类父类，真的可以把人都给整晕掉。那么这个初始化函数肯定也是有人去调用的。
void setup() -->
static void init_ardupilot() -->
static void startup_ground(bool force_gyro_cal)
//******************************************************************************
//This function does all the calibrations, etc. that we need during a ground start
//******************************************************************************
static void startup_ground(bool force_gyro_cal)
gcs_send_text_P(SEVERITY_LOW,PSTR("GROUND START"));
// initialise ahrs (may push imu calibration into the mpu6000 if using that device).
ahrs.init();
ahrs.set_vehicle_class(AHRS_VEHICLE_COPTER);
// Warm up and read Gyro offsets
// -----------------------------
ins.init(force_gyro_cal?AP_InertialSensor::COLD_START:AP_InertialSensor::WARM_START,
ins_sample_rate);
#if CLI_ENABLED == ENABLED
report_ins();
#endif
// setup fast AHRS gains to get right attitude
ahrs.set_fast_gains(true);
// set landed flag
set_land_complete(true);
结合注释可以知道这里是在起飞之前用来校准的。 ins_sample_rate就是我们的速率，它又是多少呢？
// the rate we run the main loop at
#if MAIN_LOOP_RATE == 400
static const AP_InertialSensor::Sample_rate ins_sample_rate = AP_InertialSensor::RATE_400HZ;
#else
static const AP_InertialSensor::Sample_rate ins_sample_rate = AP_InertialSensor::RATE_100HZ;
#endif
实际上我们这里只有两个速率， 400Hz跟 100Hz。而这个宏是在配置文件： "ardupilot/ArduCopter/config.h"中定义的。
#if HAL_CPU_CLASS < HAL_CPU_CLASS_75 || CONFIG_HAL_BOARD == HAL_BOARD_AVR_SITL
// low power CPUs (APM1, APM2 and SITL)
# define MAIN_LOOP_RATE 100
# define MAIN_LOOP_SECONDS 0.01
# define MAIN_LOOP_MICROS 10000
#elif CONFIG_HAL_BOARD == HAL_BOARD_LINUX
// Linux boards
# define MAIN_LOOP_RATE 200
# define MAIN_LOOP_SECONDS 0.005
# define MAIN_LOOP_MICROS 5000
#else
// high power CPUs (Flymaple, PX4, Pixhawk, VRBrain)
# define MAIN_LOOP_RATE 400
# define MAIN_LOOP_SECONDS 0.0025
# define MAIN_LOOP_MICROS 2500
#endif
我们是 PX4，所以是 400Hz。以前我一直以为 PX4的周期是 10ms，现在我想才知道其实是 2.5ms。别人都说 PX4的效果要比 APM好，速率都提高了4倍效果还不好那才真是见了鬼了。
而从宏定义可以看出，这里我们所说的速率指的是循环速率，即 loop这的循环以 400Hz的速率在运行。
然而，通过前面的分析我们已经清楚，获取 mpu6050的数据显然应该通过 ins。
2. ins
那么什么是 "ins"？根据我的理解， "ins"是 "Inertial Sensor"，也就是指惯性传感器。
其实从前面的分析我们也看到， ins中只有加计跟陀螺。所以我认为 "ins"应该是这吗理解的。
但是我们现在关心的是 mpu6050中的数据是怎么读的。前面我们说过， loop可以分为四个部分，而现在已经只剩下两个部分了，即快速循环跟调度。那么你觉得应该在哪里去读取 mpu6050的数据呢？自然是快速循环，我就不多做解释了。
// Main loop - 100hz
static void fast_loop()
// IMU DCM Algorithm
// --------------------
read_AHRS(); //姿态计算
// run low level rate controllers that only require IMU data
attitude_control.rate_controller_run(); //PID
#if FRAME_CONFIG == HELI_FRAME
update_heli_control_dynamics();
#endif //HELI_FRAME
// write out the servo PWM values
// ------------------------------
set_servos_4(); //电机输出
// Inertial Nav
// --------------------
read_inertia();
// run the attitude controllers
//更新控制模式,并计算本级控制输出下级控制输入...
update_flight_mode();
// optical flow
// --------------------
#if OPTFLOW == ENABLED
if(g.optflow_enabled)
update_optical_flow();
#endif // OPTFLOW == ENABLED
以前我一直以为 PX4的周期是 10ms就是因为这里注释写的 100Hz。现在看来这应该是保留的 APM的注释了。
看这段代码，我们很容易误以为是在 "read_inertia"函数中读取 mpu6050，但实际情况却并不是这样，因为真正的读取是在 "read_AHRS"函数中完成的， "read_inertia"函数只是把数据拿过来用而已。
static void read_AHRS(void)
// Perform IMU calculations and get attitude info
//-----------------------------------------------
#if HIL_MODE != HIL_MODE_DISABLED
// update hil before ahrs update
gcs_check_input();
#endif
ahrs.update();
// Inertial Navigation EKF
#if AP_AHRS_NAVEKF_AVAILABLE
AP_AHRS_NavEKF ahrs(ins, barometer, gps);
#else
AP_AHRS_DCM ahrs(ins, barometer, gps);
#endif
根据测试， AP_AHRS_NAVEKF_AVAILABLE是有定义的，也就是说我们有用卡尔曼滤波。而 DCM实际上是指方向余弦矩阵，也就是指用方向余弦矩阵来计算的。
但是我们看它的初始化会发现它好像少了一个东西，对的，就是地磁！为什么这里没有地磁呢？我以前所小四轴 AHRS中没有地磁是因为板子上根本就没地磁，如果说 PX4不带地磁你信吗？反正我是不信 。
class AP_AHRS_NavEKF : public AP_AHRS_DCM
public:
// Constructor
AP_AHRS_NavEKF(AP_InertialSensor &ins, AP_Baro &baro, AP_GPS &gps) :
AP_AHRS_DCM(ins, baro, gps),
EKF(this, baro),
ekf_started(false),
startup_delay_ms(10000)
所以我们看到，他们之间实际上是继承的关系。而我们的 update函数：
void AP_AHRS_NavEKF::update(void)
//AP_AHRS_NavEKF使用四元数方式进行更新...
// 父类使用方向余弦矩阵方式更新...
AP_AHRS_DCM::update();
// keep DCM attitude available for get_secondary_attitude()
// 向量初始化...
_dcm_attitude(roll, pitch, yaw);
// 卡尔曼滤波未开启...
if (!ekf_started)
所以我们还是使用了方向雨轩矩阵，至于用了多少，这里就不分析了，因为这不是我们今天的重点。
// run a full DCM update round
void
AP_AHRS_DCM::update(void)
float delta_t;
// tell the IMU to grab some data
// ...
_ins.update();
// ask the IMU how much time this sensor reading represents
delta_t = _ins.get_delta_time();
所以我们看到，最终是通过 update函数读取 mpu6050的数据的。所以这里我们需要记住 ins的类型是 AP_InertialSensor_PX4，然后找到我们对应的代码。
bool AP_InertialSensor_PX4::update(void)
if (!wait_for_sample(100))
return false;
// get the latest sample from the sensor drivers
_get_sample();
for (uint8_t k=0; k<_num_accel_instances; k++)
_previous_accel[k] = _accel[k];
_accel[k] = _accel_in[k];
// 这里不是真实的旋转,只是方向处理...
_accel[k].rotate(_board_orientation);
_accel[k].x *= _accel_scale[k].get().x;
_accel[k].y *= _accel_scale[k].get().y;
_accel[k].z *= _accel_scale[k].get().z;
_accel[k] -= _accel_offset[k];
for (uint8_t k=0; k<_num_gyro_instances; k++)
_gyro[k] = _gyro_in[k];
_gyro[k].rotate(_board_orientation);
_gyro[k] -= _gyro_offset[k];
if (_last_filter_hz != _mpu6000_filter)
_set_filter_frequency(_mpu6000_filter);
_last_filter_hz = _mpu6000_filter;
_have_sample_available = false;
return true;
wait函数我们前面已经看过了，就是等待我们设计好的一个时间，其实程序运行到这里基本上都不需要继续等待，因为在 loop中已经进行了等待。
而这里调用的另外一个函数 _get_sample就是我们这里真正用来读取数据的。在看它的源码之前呢我们不妨先看看数据读取上来之后这里都做了些什么。
首先，我们看到 rotate很容易以为这里在对数据进行旋转，但实际上这里只是对数据的方向进行处理，因为传感器的方向实际可能跟我们板子的方向不一致，这样我们就需要对方向做一个处理，这个工作就是由 rotate方法来完成的。方向处理完了之后加计跟陀螺都有减去一个偏移量。这个如果你自己写过四轴代码你就会很清楚，就是传感器都会存在误差，当数据应该为 0的时候实际输出并不等于 0，这个值就是偏移。但我们还看到加计跟陀螺的处理又有不同，那么 _accel_scale又是什么？用过 PX4的都知道， PX4校准加计的时候是通过各个方向的翻转来进行校准的，这样就可以计算出单位重力在各方向上的输出，而 PX4认为加计的每一个方向存在差异，即单位重力输出不同，这样 PX4就计算了一个比例系数来保证单位重力个方向的输出一致。最后 _set_filter_frequency是用来设置传感器过滤的：
/*
set the filter frequency
*/
void AP_InertialSensor_PX4::_set_filter_frequency(uint8_t filter_hz)
if (filter_hz == 0)
filter_hz = _default_filter_hz;
for (uint8_t i=0; i<_num_gyro_instances; i++)
ioctl(_gyro_fd[i], GYROIOCSLOWPASS, filter_hz);
for (uint8_t i=0; i<_num_accel_instances; i++)
ioctl(_accel_fd[i], ACCELIOCSLOWPASS, filter_hz);
可以看到这里是通过 ioctl接口进行设置的，具体的细节就需要去查看驱动了。当然根据我目前对 PX4的了解，这里的驱动实际上就是 mpu6000这个应用程序。这一点自然跟 Linux做法不一样。
void AP_InertialSensor_PX4::_get_sample(void)
for (uint8_t i=0; i<_num_accel_instances; i++)
struct accel_report accel_report;
while (_accel_fd[i] != -1 &&
::read(_accel_fd[i], &accel_report, sizeof(accel_report)) ==sizeof(accel_report) &&
accel_report.timestamp != _last_accel_timestamp[i])
_accel_in[i] = Vector3f(accel_report.x, accel_report.y, accel_report.z);
_last_accel_timestamp[i] = accel_report.timestamp;
for (uint8_t i=0; i<_num_gyro_instances; i++)
struct gyro_report gyro_report;
while (_gyro_fd[i] != -1 &&
::read(_gyro_fd[i], &gyro_report, sizeof(gyro_report)) ==sizeof(gyro_report) &&
gyro_report.timestamp != _last_gyro_timestamp[i])
_gyro_in[i] = Vector3f(gyro_report.x, gyro_report.y, gyro_report.z);
_last_gyro_timestamp[i] = gyro_report.timestamp;
_last_get_sample_timestamp = hrt_absolute_time();
这一段代码就是真正去读取我们 mpu6050数据的代码。它通过 Nuttx中的系统调用 read读取数据，然后构造向量。
那么就下来我们就需要到驱动里边去看相应的操作了。
PX4 FMU [6] mpu6050
1. start
前面我们已经分析到了 ArduPilot最终是通过 read调用读取 mpu6050数据的，而且我认为在 read调用的底层实际上是调用了 mpu6050这个应用程序中对应的 read接口来最终完成数据读取的。对应的文件是 "PX4Firmware/src/drivers/mpu6000/mpu6000.cpp"。
因为我的是 V2版本，所以在启动脚本中第一条关于 mpu6050的命令是： "mpu6000 -X -R 4 start"，我们需要到该应用程序的入口函数即 mpu6000_main函数中去查看该命令具体做了哪些事情。
void mpu6000_usage()
warnx("missing command: try 'start', 'info', 'test', 'reset'");
warnx("options:");
warnx(" -X (external bus)");
warnx(" -R rotation");
int mpu6000_main(int argc, char *argv[])
bool external_bus = false;
int ch;
enum Rotation rotation = ROTATION_NONE;
/* jump over start/off/etc and look at options first */
while ((ch = getopt(argc, argv, "XR:")) != EOF)