ArduCopter相关
我们知道，在 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
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，下面则是硬件。
PX4 FMU [5] Loop
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
_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
_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) {
switch (ch) {
case 'X':
external_bus = true;
break;
case 'R':
rotation = (enum Rotation)atoi(optarg);
break;
default:
mpu6000_usage();
exit(0);
}
}
const char *verb = argv[optind];
/*
* Start/load the driver.
*/
if (!strcmp(verb, "start"))
mpu6000::start(external_bus, rotation);
/*
* Test the driver/device.
*/
if (!strcmp(verb, "test"))
mpu6000::test(external_bus);
/*
* Reset the driver.
*/
if (!strcmp(verb, "reset"))
mpu6000::reset(external_bus);
/*
* Print driver information.
*/
if (!strcmp(verb, "info"))
mpu6000::info(external_bus);
errx(1, "unrecognized command, try 'start', 'test', 'reset' or 'info'");
}
这里先是解析命令行参数，然后就是执行对应的命令了。在解析命令行参数的时候，我们确确实实看到支持外部传感器，换句话说 mpu6050支持外置。 不过这里需要说明的是实际上 PX4中都是 mpu6000，我只是由于个人习惯一直都是用 mpu6050。我们可以粗略地看下这些命令都做了些什么。
/**
* Print a little info about the driver.
*/
void info(bool external_bus)
{
MPU6000 **g_dev_ptr = external_bus?&g_dev_ext:&g_dev_int;
if (*g_dev_ptr == nullptr)
errx(1, "driver not running");
printf("state @ %p\n", *g_dev_ptr);
(*g_dev_ptr)->print_info();
exit(0);
}
可以看到，这里是用来输出一些信息的，至于这都是一些什么信息我们暂时不必去关心，因为这不是我们今天的目的。但是我们必须记住一点，就是正常操作其返回值为 0。
/**
* Reset the driver.
*/
void reset(bool external_bus)
{
const char *path_accel = external_bus?MPU_DEVICE_PATH_ACCEL_EXT:MPU_DEVICE_PATH_ACCEL;
int fd = open(path_accel, O_RDONLY);
if (fd < 0)
err(1, "failed ");
if (ioctl(fd, SENSORIOCRESET, 0) < 0)
err(1, "driver reset failed");
if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0)
err(1, "driver poll restart failed");
close(fd);
exit(0);
}
这一段通过 ioctl调用来执行 Reset操作。但是或许我们会很奇怪，因为前面我说过这个应用其实是事实上的驱动，但是驱动居然也去调用 ioctl不免让人费解。而且我们如果去看这两个宏的定义：
#define MPU_DEVICE_PATH_ACCEL "/dev/mpu6000_accel"
#define MPU_DEVICE_PATH_GYRO "/dev/mpu6000_gyro"
#define MPU_DEVICE_PATH_ACCEL_EXT "/dev/mpu6000_accel_ext"
#define MPU_DEVICE_PATH_GYRO_EXT "/dev/mpu6000_gyro_ext"
我们看的包括 gyro在内都有类似的操作，但这里的设备明显跟我们前面看到的在 ins中打开的设备并不一样，那么这个设备又从何而来？通常来说，既然我能打开这个设备那必定就有人创建这个设备。但是很可惜，目前为止我并没有在 PX4源码中找到创建这个设备的源码。所以就只好把这个问题先放一放。
/**
* Perform some basic functional tests on the driver;
* make sure we can collect data from the sensor in polled
* and automatic modes.
*/
void
test(bool external_bus)
{
const char *path_accel = external_bus?MPU_DEVICE_PATH_ACCEL_EXT:MPU_DEVICE_PATH_ACCEL;
const char *path_gyro = external_bus?MPU_DEVICE_PATH_GYRO_EXT:MPU_DEVICE_PATH_GYRO;
accel_report a_report;
gyro_report g_report;
ssize_t sz;
/* get the driver */
int fd = open(path_accel, O_RDONLY);
if (fd < 0)
err(1, "%s open failed (try 'mpu6000 start' if the driver is not running)",
path_accel);
/* get the driver */
int fd_gyro = open(path_gyro, O_RDONLY);
if (fd_gyro < 0)
err(1, "%s open failed", path_gyro);
/* reset to manual polling */
if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_MANUAL) < 0)
err(1, "reset to manual polling");
/* do a simple demand read */
sz = read(fd, &a_report, sizeof(a_report));
if (sz != sizeof(a_report)) {
warnx("ret: %d, expected: %d", sz, sizeof(a_report));
err(1, "immediate acc read failed");
}
warnx("single read");
warnx("time: %lld", a_report.timestamp);
warnx("acc x: \t%8.4f\tm/s^2", (double)a_report.x);
warnx("acc y: \t%8.4f\tm/s^2", (double)a_report.y);
warnx("acc z: \t%8.4f\tm/s^2", (double)a_report.z);
warnx("acc x: \t%d\traw 0x%0x", (short)a_report.x_raw, (unsigned short)a_report.x_raw);
warnx("acc y: \t%d\traw 0x%0x", (short)a_report.y_raw, (unsigned short)a_report.y_raw);
warnx("acc z: \t%d\traw 0x%0x", (short)a_report.z_raw, (unsigned short)a_report.z_raw);
warnx("acc range: %8.4f m/s^2 (%8.4f g)", (double)a_report.range_m_s2,
(double)(a_report.range_m_s2 / MPU6000_ONE_G));
/* do a simple demand read */
sz = read(fd_gyro, &g_report, sizeof(g_report));
if (sz != sizeof(g_report)) {
warnx("ret: %d, expected: %d", sz, sizeof(g_report));
err(1, "immediate gyro read failed");
}
warnx("gyro x: \t% 9.5f\trad/s", (double)g_report.x);
warnx("gyro y: \t% 9.5f\trad/s", (double)g_report.y);
warnx("gyro z: \t% 9.5f\trad/s", (double)g_report.z);
warnx("gyro x: \t%d\traw", (int)g_report.x_raw);
warnx("gyro y: \t%d\traw", (int)g_report.y_raw);
warnx("gyro z: \t%d\traw", (int)g_report.z_raw);
warnx("gyro range: %8.4f rad/s (%d deg/s)", (double)g_report.range_rad_s,
(int)((g_report.range_rad_s / M_PI_F) * 180.0f + 0.5f));
warnx("temp: \t%8.4f\tdeg celsius", (double)a_report.temperature);
warnx("temp: \t%d\traw 0x%0x", (short)a_report.temperature_raw, (unsigned short)a_report.temperature_raw);
/* XXX add poll-rate tests here too */
reset(external_bus);
errx(0, "PASS");
}
test实际上就是 PX4给我们提供的一个读取 MPU6050的示例。这里基本上都是把读取上来的数据直接显示。但我关心的是这里操作的设备文件跟 ins中的并不一样，前面已经提到过了。不知道接下来的分析我们是否能够解决这个问题呢。
/**
* Start the driver.
*/
void
start(bool external_bus, enum Rotation rotation)
{
int fd;
MPU6000 **g_dev_ptr = external_bus?&g_dev_ext:&g_dev_int;
const char *path_accel = external_bus?MPU_DEVICE_PATH_ACCEL_EXT:MPU_DEVICE_PATH_ACCEL;
const char *path_gyro = external_bus?MPU_DEVICE_PATH_GYRO_EXT:MPU_DEVICE_PATH_GYRO;
if (*g_dev_ptr != nullptr)
/* if already started, the still command succeeded */
errx(0, "already started");
/* create the driver */
if (external_bus) {
#ifdef PX4_SPI_BUS_EXT
*g_dev_ptr = new MPU6000(PX4_SPI_BUS_EXT, path_accel, path_gyro, (spi_dev_e)PX4_SPIDEV_EXT_MPU, rotation);
#else
errx(0, "External SPI not available");
#endif
} else {
*g_dev_ptr = new MPU6000(PX4_SPI_BUS_SENSORS, path_accel, path_gyro, (spi_dev_e)PX4_SPIDEV_MPU, rotation);
}
if (*g_dev_ptr == nullptr)
goto fail;
if (OK != (*g_dev_ptr)->init())
goto fail;
/* set the poll rate to default, starts automatic data collection */
fd = open(path_accel, O_RDONLY);
if (fd < 0)
goto fail;
if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0)
goto fail;
close(fd);
exit(0);
fail:
if (*g_dev_ptr != nullptr) {
delete (*g_dev_ptr);
*g_dev_ptr = nullptr;
}
errx(1, "driver start failed");
}
这里就是我们启动脚本中执行的 start操作。这里的 "path_accel"跟 "path_gyro"其实就是我们前面 test跟 reset中所看到的。而它们传递给了 MPU6000的构造函数，所以我觉得会不会是在 MPU6000的构造函数中创建了这两个设备文件呢？这个我们后面在去证实。我们看到，后面执行的操作是 init、open、ioctl，最后是 close。那么我是不是可以认为前面我们用到的这个设备只是一个过渡而已呢？我个人是这么理解的。
突然发现，我们应该以另外一种视角来阅读源文件 mpu6000.cpp。我将其稍微处理了一下：
class MPU6000 : public device::SPI
{
public:
MPU6000(int bus, const char *path_accel, const char *path_gyro, spi_dev_e device, enum Rotation rotation);
virtual ~MPU6000();
virtual int init();
virtual ssize_t read(struct file *filp,char *buffer, size_t buflen);
virtual int ioctl(struct file *filp,int cmd, unsigned long arg);
};
/**
* Helper class implementing the gyro driver node.
*/
class MPU6000_gyro : public device::CDev
{
public:
MPU6000_gyro(MPU6000 *parent, const char *path);
~MPU6000_gyro();
virtual ssize_t read(struct file *filp,char *buffer, size_t buflen);
virtual int ioctl(struct file *filp,int cmd, unsigned long arg);
virtual int init();
protected:
friend class MPU6000;
void parent_poll_notify();
private:
MPU6000 *_parent;
orb_advert_t _gyro_topic;
int _gyro_class_instance;
};
/**
* Local functions in support of the shell command.
*/
namespace mpu6000
{
MPU6000 *g_dev_int; // on internal bus
MPU6000 *g_dev_ext; // on external bus
void start(bool, enum Rotation);
void test(bool);
void reset(bool);
void info(bool);
} // namespace
int mpu6000_main(int argc, char *argv[])
{
}
这样处理之后我发现整个源文件的结构要比之前清晰得多。我们看到，源文件实际上分为四个部分：两个 class、一个 namespace和一个入口函数 mpu6000_main。而从我们前面的分析已经知道 mpu6000_main调用 namespace，namespace又使用了 MPU6000，至于 MPU6000_gyro是如何被使用的我们后面就会看到。
由于这里用的是 C++，自然免不了要关心 class的继承关系。我们知道 MPU6000使用 SPI通信，所以 MPU6000的作用就不用我多说了。只是为什么 MPU6000又要分出来一个 gyro？或许是分开了上层应用更好处理吧。毕竟有些传感器加计是加计，陀螺是陀螺，并没有做到一起。
那么，CDev跟 SPI又有什么关系呢？我们找到
#include "device.h"
#include
namespace device __EXPORT
{
/**
* Abstract class for character device on SPI
*/
class __EXPORT SPI : public CDev
{
protected:
所以 SPI继承自 CDev。那么 CDev呢？ CDev在 "device.h"中定义：
/**
* Namespace encapsulating all device framework classes, functions and data.
*/
namespace device __EXPORT
{
/**
* Fundamental base class for all device drivers.
*
* This class handles the basic "being a driver" things, including
* interrupt registration and dispatch.
*/
class __EXPORT Device
{
};
/**
* Abstract class for any character device
*/
class __EXPORT CDev : public Device
{
/**
* Register a class device name, automatically adding device
* class instance suffix if need be.
*
* @param class_devname Device class name
* @return class_instamce Class instance created, or -errno on failure
*/
virtual int register_class_devname(const char *class_devname);
/**
* Register a class device name, automatically adding device
* class instance suffix if need be.
*
* @param class_devname Device class name
* @param class_instance Device class instance from register_class_devname()
* @return OK on success, -errno otherwise
*/
virtual int unregister_class_devname(const char *class_devname,unsigned class_instance);
};
/**
* Abstract class for character device accessed via PIO
*/
class __EXPORT PIO : public CDev
{
};
} // namespace device
想必你也已经看出来了这是被我处理过的代码。详细内容请阅读该头文件。
或许你会很奇怪，为什么我把其它的函数都给删除了，却偏偏留下了两个 register函数？这是因为这两个函数很特殊。我们知道在 Linux驱动中需要对设备进行注册和销毁，这里是一样的道理。
似乎已经扯太远了，下面我们把目光收回 MPU6000。
2. MPU6000
从前边的分析我们看到，这里是清一色的 C++，像我习惯了用 C来处理问题难免有点小不适应。如果你有兴趣研究 C++不妨去买基本书来好好读读。
MPU6000::MPU6000(int bus, const char *path_accel,const char *path_gyro, spi_dev_e device, enum Rotation rotation) :
SPI("MPU6000", path_accel, bus, device, SPIDEV_MODE3, MPU6000_LOW_BUS_SPEED),
_gyro(new MPU6000_gyro(this, path_gyro)),
_product(0),
_call_interval(0),
_accel_reports(nullptr),
_accel_range_scale(0.0f),
_accel_range_m_s2(0.0f),
_accel_topic(-1),
_accel_class_instance(-1),
_gyro_reports(nullptr),
_gyro_range_scale(0.0f),
_gyro_range_rad_s(0.0f),
_sample_rate(1000),
_accel_reads(perf_alloc(PC_COUNT, "mpu6000_accel_read")),
_gyro_reads(perf_alloc(PC_COUNT, "mpu6000_gyro_read")),
_sample_perf(perf_alloc(PC_ELAPSED, "mpu6000_read")),
_bad_transfers(perf_alloc(PC_COUNT, "mpu6000_bad_transfers")),
_good_transfers(perf_alloc(PC_COUNT, "mpu6000_good_transfers")),
_accel_filter_x(MPU6000_ACCEL_DEFAULT_RATE, MPU6000_ACCEL_DEFAULT_DRIVER_FILTER_FREQ),
_accel_filter_y(MPU6000_ACCEL_DEFAULT_RATE, MPU6000_ACCEL_DEFAULT_DRIVER_FILTER_FREQ),
_accel_filter_z(MPU6000_ACCEL_DEFAULT_RATE, MPU6000_ACCEL_DEFAULT_DRIVER_FILTER_FREQ),
_gyro_filter_x(MPU6000_GYRO_DEFAULT_RATE, MPU6000_GYRO_DEFAULT_DRIVER_FILTER_FREQ),
_gyro_filter_y(MPU6000_GYRO_DEFAULT_RATE, MPU6000_GYRO_DEFAULT_DRIVER_FILTER_FREQ),
_gyro_filter_z(MPU6000_GYRO_DEFAULT_RATE, MPU6000_GYRO_DEFAULT_DRIVER_FILTER_FREQ),
_rotation(rotation)
{
// disable debug() calls
_debug_enabled = true;
// default accel scale factors
_accel_scale.x_offset = 0;
_accel_scale.x_scale = 1.0f;
_accel_scale.y_offset = 0;
_accel_scale.y_scale = 1.0f;
_accel_scale.z_offset = 0;
_accel_scale.z_scale = 1.0f;
// default gyro scale factors
_gyro_scale.x_offset = 0;
_gyro_scale.x_scale = 1.0f;
_gyro_scale.y_offset = 0;
_gyro_scale.y_scale = 1.0f;
_gyro_scale.z_offset = 0;
_gyro_scale.z_scale = 1.0f;
memset(&_call, 0, sizeof(_call));
}
这就是前边我们执行 "*g_dev_ptr = new MPU6000(PX4_SPI_BUS_SENSORS, path_accel, path_gyro, (spi_dev_e)PX4_SPIDEV_MPU, rotation);"这条语句是所调用的构造函数。在这个构造函数中目前我只关心两个东西： SPI跟 _gyro。为什么？因为别的我都还不清楚是干嘛用的。也许你就要问了，那么这两个东西就就知道干嘛用的了吗？说实话，我还真不知道。但是另外两个东西我是知道的：path_accel和 path_gyro。这是设备文件的路径，你觉得我还会把它给忘了吗？也许，可能这里创建了我们前面想不出在哪里创建的设备。
SPI::SPI(const char *name,
const char *devname,
int bus,
enum spi_dev_e device,
enum spi_mode_e mode,
uint32_t frequency,
int irq) :
// base class
CDev(name, devname, irq),
// public
// protected
locking_mode(LOCK_PREEMPTION),
// private
_bus(bus),
_device(device),
_mode(mode),
_frequency(frequency),
_dev(nullptr)
{
// fill in _device_id fields for a SPI device
_device_id.devid_s.bus_type = DeviceBusType_SPI;
_device_id.devid_s.bus = bus;
_device_id.devid_s.address = (uint8_t)device;
// devtype needs to be filled in by the driver
_device_id.devid_s.devtype = 0;
}
CDev::CDev(const char *name,
const char *devname,
int irq) :
// base class
Device(name, irq),
// public
// protected
_pub_blocked(false),
// private
_devname(devname),
_registered(false),
_open_count(0)
{
for (unsigned i = 0; i < _max_pollwaiters; i++)
_pollset[i] = nullptr;
}
但是我们看到，其实也没有创建设备文件啊，只是把设备文件的路径保存在了 _devname中而已。别急，你看过 _devname是怎么使用的吗？
int CDev::init()
{
// base class init first
int ret = Device::init();
if (ret != OK)
goto out;
// now register the driver
if (_devname != nullptr) {
ret = register_driver(_devname, &fops, 0666, (void *)this);
if (ret != OK)
goto out;
_registered = true;
}
out:
return ret;
}
我们看到，这里有出现了一个 register函数，用来创建我们的设备文件。那么跟前面我们看到的 register函数有什么不一样呢？
int
CDev::register_class_devname(const char *class_devname)
{
if (class_devname == nullptr) {
return -EINVAL;
}
int class_instance = 0;
int ret = -ENOSPC;
while (class_instance < 4) {
if (class_instance == 0) {
ret = register_driver(class_devname, &fops, 0666, (void *)this);
if (ret == OK) break;
} else {
char name[32];
snprintf(name, sizeof(name), "%s%d", class_devname, class_instance);
ret = register_driver(name, &fops, 0666, (void *)this);
if (ret == OK) break;
}
class_instance++;
}
if (class_instance == 4)
return ret;
return class_instance;
}
我们看到，它们之间是存在一个封装的关系。前面我们在 ins中看到 ins打开设备有一个规律：
_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);
就是当同样功能的设备不止一个时，设备文件名会增加一个序号，从 register_class_devname函数源码中我们看到这个序号是怎么来的了。
现在我们应该知道，我们在 test跟 reset中打开的设备并不是凭空产生的。由此我们也知道，没有执行 start是不能执行这两个操作的，要不然肯定会在打开设备文件的时候出错。
当然，上面我们说道的只是打通应用程序跟驱动的一个环节，我们知道在 Linux写一个驱动，如果你要让应用程序能够通过如 open这样的接口操作设备还要做一件事：file_operations。在 Nuttx中同样需要做这个工作，就是 register函数中的 fops这个形参。
const struct file_operations CDev::fops = {
open : cdev_open,
close : cdev_close,
read : cdev_read,
write : cdev_write,
seek : cdev_seek,
ioctl : cdev_ioctl,
poll : cdev_poll,
};
在 CDev中这些接口都被定义成了 virtual函数，熟悉 C++的肯定都知道这表示什么。所以我们只要在子类中重写这些virtual函数应用程序就会调用到我们自己写的函数，如果你不写，那自然就调用父类的，以此类推。
写到这里，我想已经不用我再多做解释大家肯定都已经知道"PX4Firmware/src/drivers/mpu6000/mpu6000.cpp"的的确确就是一个驱动，而且是 mpu6000的驱动。
但是我们发现，这里的驱动是通过应用程序来注册的，我不知道是 Nuttx本身就是这么设计还是 PX4刻意这么设计。不过我觉得，这大概注意说明以前我关于 Nuttx的一个猜测，那就是我认为 Nuttx没有用户空间跟内核空间的概念，或者说不区分。
关于 register_driver函数的细节就是 Nuttx操作系统的内容了，我们今天不讨论操作系统，故对此不去深究。
那么，我们不禁要问了， init函数有在哪里调用呢？这就是我们现在要解决的问题。
由于继承关系，我们很容易想到， SPI也会有一个 init函数，而 MPU6000又是 SPI的子类，同样会有 init函数。
int SPI::init()
{
int ret = OK;
/* attach to the spi bus */
if (_dev == nullptr)
_dev = up_spiinitialize(_bus);
if (_dev == nullptr) {
debug("failed to init SPI");
ret = -ENOENT;
goto out;
}
/* deselect device to ensure high to low transition of pin select */
SPI_SELECT(_dev, _device, false);
/* call the probe function to check whether the device is present */
ret = probe();
if (ret != OK) {
debug("probe failed");
goto out;
}
/* do base class init, which will create the device node, etc. */
ret = CDev::init();
if (ret != OK) {
debug("cdev init failed");
goto out;
}
/* tell the workd where we are */
log("on SPI bus %d at %d", _bus, _device);
out:
return ret;
}
int MPU6000::init()
{
int ret;
/* do SPI init (and probe) first */
ret = SPI::init();
所以最后的问题是 MPU6000::init()是如何被调用的。那么我们回过头在去看 start我们就发现：
/* create the driver */
if (external_bus) {
#ifdef PX4_SPI_BUS_EXT
*g_dev_ptr = new MPU6000(PX4_SPI_BUS_EXT, path_accel, path_gyro, (spi_dev_e)PX4_SPIDEV_EXT_MPU, rotation);
#else
errx(0, "External SPI not available");
#endif
} else {
*g_dev_ptr = new MPU6000(PX4_SPI_BUS_SENSORS, path_accel, path_gyro, (spi_dev_e)PX4_SPIDEV_MPU, rotation);
}
if (*g_dev_ptr == nullptr)
goto fail;
if (OK != (*g_dev_ptr)->init())
goto fail;
这么明显的调用了 init函数。而且注释明明白白地写着 "create the driver"，我居然还翻来覆去倒腾了半天。所以人们都说：“纵里寻她千百度，蓦然回首，那人却在，灯火阑珊处”。
关于 start我们前面其实没有看完。
看完了 SPI，我们肯定是接着看 _gyro。
MPU6000_gyro::MPU6000_gyro(MPU6000 *parent, const char *path) :
CDev("MPU6000_gyro", path),
_parent(parent),
_gyro_topic(-1),
_gyro_class_instance(-1)
{
}
_gyro的构造函数除了初始化数据就是调用 CDev的构造函数。接下去就不用我再重复分析了吧，在 SPI中已经说得很清楚了。
所以，我们现在又要问了， gyro的 init都做了些什么，有没有调用 CDev的 init？这个还是用源码来说话吧：
int
MPU6000_gyro::init()
{
int ret;
// do base class init
ret = CDev::init();
/* if probe/setup failed, bail now */
if (ret != OK) {
debug("gyro init failed");
return ret;
}
_gyro_class_instance = register_class_devname(GYRO_DEVICE_PATH);
return ret;
}
所以答案是调用了。前面我们提到过 "register_class_devname"函数,知道这是用来创建设备文件的，为什么这里又要创建设备文件？我们会在后面给出回答。
那么当我们回到 start，接下来理所当然应该是去看 init了。
3. init
现在我们已经知道， "(*g_dev_ptr)->init()"实际上是调用了 "MPU6000::init()"。完整的源码如下：
int MPU6000::init()
{
int ret;
/* do SPI init (and probe) first */
ret = SPI::init();
/* if probe/setup failed, bail now */
if (ret != OK) {
debug("SPI setup failed");
return ret;
}
/* allocate basic report buffers */
_accel_reports = new RingBuffer(2, sizeof(accel_report));
if (_accel_reports == nullptr)
goto out;
_gyro_reports = new RingBuffer(2, sizeof(gyro_report));
if (_gyro_reports == nullptr)
goto out;
reset();
/* Initialize offsets and scales */
_accel_scale.x_offset = 0;
_accel_scale.x_scale = 1.0f;
_accel_scale.y_offset = 0;
_accel_scale.y_scale = 1.0f;
_accel_scale.z_offset = 0;
_accel_scale.z_scale = 1.0f;
_gyro_scale.x_offset = 0;
_gyro_scale.x_scale = 1.0f;
_gyro_scale.y_offset = 0;
_gyro_scale.y_scale = 1.0f;
_gyro_scale.z_offset = 0;
_gyro_scale.z_scale = 1.0f;
/* do CDev init for the gyro device node, keep it optional */
ret = _gyro->init();
/* if probe/setup failed, bail now */
if (ret != OK) {
debug("gyro init failed");
return ret;
}
_accel_class_instance = register_class_devname(ACCEL_DEVICE_PATH);
measure();
if (_accel_class_instance == CLASS_DEVICE_PRIMARY) {
/* advertise sensor topic, measure manually to initialize valid report */
struct accel_report arp;
_accel_reports->get(&arp);
/* measurement will have generated a report, publish */
_accel_topic = orb_advertise(ORB_ID(sensor_accel), &arp);
if (_accel_topic < 0)
debug("failed to create sensor_accel publication");
}
if (_gyro->_gyro_class_instance == CLASS_DEVICE_PRIMARY) {
/* advertise sensor topic, measure manually to initialize valid report */
struct gyro_report grp;
_gyro_reports->get(&grp);
_gyro->_gyro_topic = orb_advertise(ORB_ID(sensor_gyro), &grp);
if (_gyro->_gyro_topic < 0)
debug("failed to create sensor_gyro publication");
}
out:
return ret;
}
我们看到， _gyro->init()正是在这里调用的。
在这里申请的 buffer，跟初始化的那些数据，我们目前并不是很清楚是干嘛用的，也就不去关心了。可能我们要稍微了解的就是这么几个东西： reset、measure、 register_class_devname。前面我们多次说过 register_class_devname是用来创建设备文件的，那么我们都注册了那些设备呢？
_gyro_class_instance = register_class_devname(GYRO_DEVICE_PATH);
_accel_class_instance = register_class_devname(ACCEL_DEVICE_PATH);
#define ACCEL_DEVICE_PATH "/dev/accel"
#define GYRO_DEVICE_PATH "/dev/gyro"
我们如果回去看 ins初始化就会发现，其中 open操作用的也正好是这两个宏。
void MPU6000::reset()
{
// if the mpu6000 is initialised after the l3gd20 and lsm303d
// then if we don't do an irqsave/irqrestore here the mpu6000
// frequenctly comes up in a bad state where all transfers
// come as zero
irqstate_t state;
state = irqsave();
write_reg(MPUREG_PWR_MGMT_1, BIT_H_RESET);
up_udelay(10000);
// Wake up device and select GyroZ clock. Note that the
// MPU6000 starts up in sleep mode, and it can take some time
// for it to come out of sleep
write_reg(MPUREG_PWR_MGMT_1, MPU_CLK_SEL_PLLGYROZ);
up_udelay(1000);
// Disable I2C bus (recommended on datasheet)
write_reg(MPUREG_USER_CTRL, BIT_I2C_IF_DIS);
irqrestore(state);
up_udelay(1000);
// SAMPLE RATE
_set_sample_rate(_sample_rate);
usleep(1000);
// FS & DLPF FS=2000 deg/s, DLPF = 20Hz (low pass filter)
// was 90 Hz, but this ruins quality and does not improve the
// system response
_set_dlpf_filter(MPU6000_DEFAULT_ONCHIP_FILTER_FREQ);
usleep(1000);
// Gyro scale 2000 deg/s ()
write_reg(MPUREG_GYRO_CONFIG, BITS_FS_2000DPS);
usleep(1000);
// correct gyro scale factors
// scale to rad/s in SI units
// 2000 deg/s = (2000/180)*PI = 34.906585 rad/s
// scaling factor:
// 1/(2^15)*(2000/180)*PI
_gyro_range_scale = (0.0174532 / 16.4);//1.0f / (32768.0f * (2000.0f / 180.0f) * M_PI_F);
_gyro_range_rad_s = (2000.0f / 180.0f) * M_PI_F;
// product-specific scaling
switch (_product) {
case MPU6000ES_REV_C4:
case MPU6000ES_REV_C5:
case MPU6000_REV_C4:
case MPU6000_REV_C5:
// Accel scale 8g (4096 LSB/g)
// Rev C has different scaling than rev D
write_reg(MPUREG_ACCEL_CONFIG, 1 << 3);
break;
case MPU6000ES_REV_D6:
case MPU6000ES_REV_D7:
case MPU6000ES_REV_D8:
case MPU6000_REV_D6:
case MPU6000_REV_D7:
case MPU6000_REV_D8:
case MPU6000_REV_D9:
case MPU6000_REV_D10:
// default case to cope with new chip revisions, which
// presumably won't have the accel scaling bug
default:
// Accel scale 8g (4096 LSB/g)
write_reg(MPUREG_ACCEL_CONFIG, 2 << 3);
break;
}
// Correct accel scale factors of 4096 LSB/g
// scale to m/s^2 ( 1g = 9.81 m/s^2)
_accel_range_scale = (MPU6000_ONE_G / 4096.0f);
_accel_range_m_s2 = 8.0f * MPU6000_ONE_G;
usleep(1000);
// INT CFG => Interrupt on Data Ready
write_reg(MPUREG_INT_ENABLE, BIT_RAW_RDY_EN); // INT: Raw data ready
usleep(1000);
write_reg(MPUREG_INT_PIN_CFG, BIT_INT_ANYRD_2CLEAR); // INT: Clear on any read
usleep(1000);
// Oscillator set
// write_reg(MPUREG_PWR_MGMT_1,MPU_CLK_SEL_PLLGYROZ);
usleep(1000);
}
我们看到，这里基本上都是配置寄存器。这个我们就不去看其中配置的每一个参数了。还是那句话，不是我们今天的目的。
而 measure函数如果你去看你就会觉得它应该是跟读操作有关系的。我们也是后面再看。
4. read
在 ins中我们就是通过 read调用来读数据的，在底层实际上就是调用了这里的 read函数。
先说 MPU6000吧。
ssize_t MPU6000::read(struct file *filp, char *buffer, size_t buflen)
{
unsigned count = buflen / sizeof(accel_report);
/* buffer must be large enough */
if (count < 1)
return -ENOSPC;
/* if automatic measurement is not enabled, get a fresh measurement into the buffer */
if (_call_interval == 0) {
_accel_reports->flush();
measure();
}
/* if no data, error (we could block here) */
if (_accel_reports->empty())
return -EAGAIN;
perf_count(_accel_reads);
/* copy reports out of our buffer to the caller */
accel_report *arp = reinterpret_cast
int transferred = 0;
while (count--) {
if (!_accel_reports->get(arp))
break;
transferred++;
arp++;
}
/* return the number of bytes transferred */
return (transferred * sizeof(accel_report));
}
其实一开始我看到这里好像没有读数据的操作，总觉得有点失望。于是我只好查看到底哪里使用了 buffer这个指针，虽然说我不确定 "accel_report *arp =reinterpret_cast
void MPU6000::measure()
{
/*
* Fetch the full set of measurements from the MPU6000 in one pass.
*/
mpu_report.cmd = DIR_READ | MPUREG_INT_STATUS;
// sensor transfer at high clock speed
set_frequency(MPU6000_HIGH_BUS_SPEED);
if (OK != transfer((uint8_t *)&mpu_report, ((uint8_t *)&mpu_report),sizeof(mpu_report)))
return;
/*
* Convert from big to little endian
*/
report.accel_x = int16_t_from_bytes(mpu_report.accel_x);
report.accel_y = int16_t_from_bytes(mpu_report.accel_y);
report.accel_z = int16_t_from_bytes(mpu_report.accel_z);
report.temp = int16_t_from_bytes(mpu_report.temp);
report.gyro_x = int16_t_from_bytes(mpu_report.gyro_x);
report.gyro_y = int16_t_from_bytes(mpu_report.gyro_y);
report.gyro_z = int16_t_from_bytes(mpu_report.gyro_z);
if (report.accel_x == 0 &&
report.accel_y == 0 &&
report.accel_z == 0 &&
report.temp == 0 &&
report.gyro_x == 0 &&
report.gyro_y == 0 &&
report.gyro_z == 0) {
// all zero data - probably a SPI bus error
perf_count(_bad_transfers);
perf_end(_sample_perf);
//reset();
return;
}
我们看到，实际上读取传感器数据通过 transfer接口来完成，后面就是数据处理。关于 transfer又是一个比较啰嗦的话题，后面我们再专门讨论。我们先来简单看下数据读到之后是怎么处理的。
/*
* Swap axes and negate y
*/
int16_t accel_xt = report.accel_y;
int16_t accel_yt = ((report.accel_x == -32768) ? 32767 : -report.accel_x);
int16_t gyro_xt = report.gyro_y;
int16_t gyro_yt = ((report.gyro_x == -32768) ? 32767 : -report.gyro_x);
/*
* Apply the swap
*/
report.accel_x = accel_xt;
report.accel_y = accel_yt;
report.gyro_x = gyro_xt;
report.gyro_y = gyro_yt;
这里只是对数据的方向做处理。 在ins中我们看到过对方向的处理，那么我们可能会很奇怪，为什么这里还对方向做处理？个人觉得这里的处理只是让数据符合某一个坐标系，或者说是为了让数据符号左手或右手定则，具体的还得结合 PCB板上传感器的安装进行分析。
accel_report arb;
gyro_report grb;
/*
* Adjust and scale results to m/s^2.
*/
grb.timestamp = arb.timestamp = hrt_absolute_time();
grb.error_count = arb.error_count = 0; // not reported
/*
* 1) Scale raw value to SI units using scaling from datasheet.
* 2) Subtract static offset (in SI units)
* 3) Scale the statically calibrated values with a linear
* dynamically obtained factor
*
* Note: the static sensor offset is the number the sensor outputs
* at a nominally 'zero' input. Therefore the offset has to
* be subtracted.
*
* Example: A gyro outputs a value of 74 at zero angular rate
* the offset is 74 from the origin and subtracting
* 74 from all measurements centers them around zero.
*/
/* NOTE: Axes have been swapped to match the board a few lines above. */
arb.x_raw = report.accel_x;
arb.y_raw = report.accel_y;
arb.z_raw = report.accel_z;
float x_in_new = ((report.accel_x * _accel_range_scale) - _accel_scale.x_offset) * _accel_scale.x_scale;
float y_in_new = ((report.accel_y * _accel_range_scale) - _accel_scale.y_offset) * _accel_scale.y_scale;
float z_in_new = ((report.accel_z * _accel_range_scale) - _accel_scale.z_offset) * _accel_scale.z_scale;
arb.x = _accel_filter_x.apply(x_in_new);
arb.y = _accel_filter_y.apply(y_in_new);
arb.z = _accel_filter_z.apply(z_in_new);
// apply user specified rotation
rotate_3f(_rotation, arb.x, arb.y, arb.z);
arb.scaling = _accel_range_scale;
arb.range_m_s2 = _accel_range_m_s2;
arb.temperature_raw = report.temp;
arb.temperature = (report.temp) / 361.0f + 35.0f;
这段代码我第一遍看完全给看晕掉了，因为我觉得这又跟 ins重复了。但其实你如果去查看这些变量你就会发现 offset值为 0， scale值 1.0。但是 _accel_range_scale这个变量就比较有意思：
// Correct accel scale factors of 4096 LSB/g
// scale to m/s^2 ( 1g = 9.81 m/s^2)
_accel_range_scale = (MPU6000_ONE_G / 4096.0f);
_accel_range_m_s2 = 8.0f * MPU6000_ONE_G;
所以我们就知道了，它其实就只是一个转换系数，将采集来的 AD值转为加速度值。而 _accel_range_m_s2就表示当前配置下的最大可以测量的加速度值。接下去就是对数据进行滤波，滤波完了就通过 rotate_3f对数据进行旋转。所以说我怎么都觉得数据重复旋转了，这个现在是跟你解释不清楚的。
前面在 read中我们已经确定数据是通过 _accel_reports放到 buffer里边的，而现在的情况是数据存储在 arb当中，于是：
_accel_reports->force(&arb);
_accel_reports->get(arp)
所以如果我们想要了解其中细节就需要了解 _accel_reports的类。
而现在，我们要去看看 transfer又是怎么回事。
5. transfer
因为我们用的是 SPI接口，那么我们当然是到 SPI中去找这部分源码：
int SPI::transfer(uint8_t *send, uint8_t *recv, unsigned len)
{
int result;
if ((send == nullptr) && (recv == nullptr))
return -EINVAL;
LockMode
PX4 FMU [7] rgbled
1. start
我们在命令行启动 rgbled的命令如下：
if rgbled start
then
set HAVE_RGBLED 1
rgbled rgb 16 16 16
else
set HAVE_RGBLED 0
fi
入口函数如下：
int rgbled_main(int argc, char *argv[])
{
int i2cdevice = -1;
int rgbledadr = ADDR; /* 7bit */
int ch;
/* jump over start/off/etc and look at options first */
while ((ch = getopt(argc, argv, "a:b:")) != EOF) {
switch (ch) {
case 'a':
rgbledadr = strtol(optarg, NULL, 0);
break;
case 'b':
i2cdevice = strtol(optarg, NULL, 0);
break;
default:
rgbled_usage();
exit(0);
}
}
if (optind >= argc) {
rgbled_usage();
exit(1);
}
const char *verb = argv[optind];
int fd;
int ret;
if (!strcmp(verb, "start")) {
if (g_rgbled != nullptr)
errx(1, "already started");
if (i2cdevice == -1) {
// try the external bus first
i2cdevice = PX4_I2C_BUS_EXPANSION;
g_rgbled = new RGBLED(PX4_I2C_BUS_EXPANSION, rgbledadr);
if (g_rgbled != nullptr && OK != g_rgbled->init()) {
delete g_rgbled;
g_rgbled = nullptr;
}
if (g_rgbled == nullptr) {
// fall back to default bus
if (PX4_I2C_BUS_LED == PX4_I2C_BUS_EXPANSION) {
errx(1, "init failed");
}
i2cdevice = PX4_I2C_BUS_LED;
}
}
if (g_rgbled == nullptr) {
g_rgbled = new RGBLED(i2cdevice, rgbledadr);
if (g_rgbled == nullptr)
errx(1, "new failed");
if (OK != g_rgbled->init()) {
delete g_rgbled;
g_rgbled = nullptr;
errx(1, "init failed");
}
}
exit(0);
}
/* need the driver past this point */
if (g_rgbled == nullptr) {
warnx("not started");
rgbled_usage();
exit(1);
}
if (!strcmp(verb, "test")) {
fd = open(RGBLED_DEVICE_PATH, 0);
if (fd == -1) {
errx(1, "Unable to open " RGBLED_DEVICE_PATH);
}
rgbled_pattern_t pattern = { {RGBLED_COLOR_RED, RGBLED_COLOR_GREEN, RGBLED_COLOR_BLUE, RGBLED_COLOR_WHITE, RGBLED_COLOR_OFF, RGBLED_COLOR_OFF},
{500, 500, 500, 500, 1000, 0 } // "0" indicates end of pattern
};
ret = ioctl(fd, RGBLED_SET_PATTERN, (unsigned long)&pattern);
ret = ioctl(fd, RGBLED_SET_MODE, (unsigned long)RGBLED_MODE_PATTERN);
close(fd);
exit(ret);
}
if (!strcmp(verb, "info")) {
g_rgbled->info();
exit(0);
}
if (!strcmp(verb, "off") || !strcmp(verb, "stop")) {
fd = open(RGBLED_DEVICE_PATH, 0);
if (fd == -1) {
errx(1, "Unable to open " RGBLED_DEVICE_PATH);
}
ret = ioctl(fd, RGBLED_SET_MODE, (unsigned long)RGBLED_MODE_OFF);
close(fd);
exit(ret);
}
if (!strcmp(verb, "stop")) {
delete g_rgbled;
g_rgbled = nullptr;
exit(0);
}
if (!strcmp(verb, "rgb")) {
if (argc < 5) {
errx(1, "Usage: rgbled rgb
}
fd = open(RGBLED_DEVICE_PATH, 0);
if (fd == -1) {
errx(1, "Unable to open " RGBLED_DEVICE_PATH);
}
rgbled_rgbset_t v;
v.red = strtol(argv[2], NULL, 0);
v.green = strtol(argv[3], NULL, 0);
v.blue = strtol(argv[4], NULL, 0);
ret = ioctl(fd, RGBLED_SET_RGB, (unsigned long)&v);
ret = ioctl(fd, RGBLED_SET_MODE, (unsigned long)RGBLED_MODE_ON);
close(fd);
exit(ret);
}
rgbled_usage();
exit(0);
}
我们看到，在 start中跟 MPU6050的不同在于这里没有单独做一个 start接口。这也说明 rgbled的 start过程要比 MPU6050要简单得多。主要是创建了 RGBLED对象，并调用其 init函数。然后是 rgb，rgb呢主要是获取我们命令中的 RGB值，因为我们知道这是用来控制板子上的那个 RGBLED的。发送数据使用的是 ioctl接口。当然，在 Linux设备驱动中通常是用 read接口来读数据，在 MPU6050中就是这样。
RGBLED::RGBLED(int bus, int rgbled) :
I2C("rgbled", RGBLED_DEVICE_PATH, bus, rgbled, 100000),
_mode(RGBLED_MODE_OFF),
_r(0),
_g(0),
_b(0),
_brightness(1.0f),
_running(false),
_led_interval(0),
_should_run(false),
_counter(0)
{
memset(&_work, 0, sizeof(_work));
memset(&_pattern, 0, sizeof(_pattern));
}
I2C::I2C(const char *name,
const char *devname,
int bus,
uint16_t address,
uint32_t frequency,
int irq) :
// base class
CDev(name, devname, irq),
// public
// protected
_retries(0),
// private
_bus(bus),
_address(address),
_frequency(frequency),
_dev(nullptr)
{
// fill in _device_id fields for a I2C device
_device_id.devid_s.bus_type = DeviceBusType_I2C;
_device_id.devid_s.bus = bus;
_device_id.devid_s.address = address;
// devtype needs to be filled in by the driver
_device_id.devid_s.devtype = 0;
}
I2C跟 SPI一样都继承自 CDev，所以它们之间很多东西都是相通的。对 C++不太了解的可能会觉得在给 I2C传递参数的过程中有点莫名其妙，会觉得少了一个参数。其实你去看构造函数的定义你就会知道， irq有一个初值，即 0。当我们调用的时候不提供该参数的时候 irq即为 0。其它参数我们主要看两个： devname跟 address。前者是设备文件名，后者是设备地址。其值为：
/* more devices will be 1, 2, etc */
#define RGBLED_DEVICE_PATH "/dev/rgbled0"
#define ADDR PX4_I2C_OBDEV_LED /**< I2C adress of TCA62724FMG */
#define PX4_I2C_OBDEV_LED 0x55
如果你有兴趣你可以去查看 PX4所使用的驱动芯片的 I2C地址。我没去研究硬件，就不深究了。
其实构造函数中还有一个参数：bus。在这里， bus应该也指的是总线。那么它是指什么总线呢？
我们会在 init函数中看到 bus被使用了：
int I2C::init()
{
int ret = OK;
/* attach to the i2c bus */
_dev = up_i2cinitialize(_bus);
if (_dev == nullptr) {
debug("failed to init I2C");
ret = -ENOENT;
goto out;
}
// call the probe function to check whether the device is present
ret = probe();
if (ret != OK) {
debug("probe failed");
goto out;
}
// do base class init, which will create device node, etc
ret = CDev::init();
if (ret != OK) {
debug("cdev init failed");
goto out;
}
// tell the world where we are
log("on I2C bus %d at 0x%02x", _bus, _address);
out:
return ret;
}
在 SPI中我们也遇到个 up函数，返回的也是 dev，后面就是通过这个 dev跟真正的硬件打交道。
bitcraze@bitcraze-vm:~/apm$ grep -nr up_i2cinitialize ./PX4NuttX/nuttx/arch/arm/src/stm32
./PX4NuttX/nuttx/arch/arm/src/stm32/stm32_i2c.c:1888: * Name: up_i2cinitialize
./PX4NuttX/nuttx/arch/arm/src/stm32/stm32_i2c.c:1895:FAR struct i2c_dev_s *up_i2cinitialize(int port)
bitcraze@bitcraze-vm:~/apm$
FAR struct i2c_dev_s *up_i2cinitialize(int port)
{
struct stm32_i2c_priv_s * priv = NULL; /* private data of device with multiple instances */
struct stm32_i2c_inst_s * inst = NULL; /* device, single instance */
int irqs;
#if STM32_PCLK1_FREQUENCY < 4000000
# warning STM32_I2C_INIT: Peripheral clock must be at least 4 MHz to support 400 kHz operation.
#endif
#if STM32_PCLK1_FREQUENCY < 2000000
# warning STM32_I2C_INIT: Peripheral clock must be at least 2 MHz to support 100 kHz operation.
return NULL;
#endif
/* Get I2C private structure */
switch (port)
{
#ifdef CONFIG_STM32_I2C1
case 1:
priv = (struct stm32_i2c_priv_s *)&stm32_i2c1_priv;
break;
#endif
#ifdef CONFIG_STM32_I2C2
case 2:
priv = (struct stm32_i2c_priv_s *)&stm32_i2c2_priv;
break;
#endif
#ifdef CONFIG_STM32_I2C3
case 3:
priv = (struct stm32_i2c_priv_s *)&stm32_i2c3_priv;
break;
#endif
default:
return NULL;
}
/* Allocate instance */
if (!(inst = kmalloc( sizeof(struct stm32_i2c_inst_s))))
{
return NULL;
}
/* Initialize instance */
inst->ops = &stm32_i2c_ops;
inst->priv = priv;
inst->frequency = 100000;
inst->address = 0;
inst->flags = 0;
/* Init private data for the first time, increment refs count,
* power-up hardware and configure GPIOs.
*/
irqs = irqsave();
if ((volatile int)priv->refs++ == 0)
{
stm32_i2c_sem_init( (struct i2c_dev_s *)inst );
stm32_i2c_init( priv );
}
irqrestore(irqs);
return (struct i2c_dev_s *)inst;
}
跟 SPI是类似的。从代码来看，bus指的应该是所外部设备所挂载的 I2C控制器，因为 STM32存在多个 I2C控制器，而上层获取操作接口都是通过同一个函数，这样就需要区分不同的硬件接口，这就是 bus的作用。在 SPI中其实也一样。其实我们也很容易想到 priv不是用来操作硬件的， ops才是。只要稍微看下相关源码就清楚了。
下面我们来看看 init。
int RGBLED::init()
{
int ret;
ret = I2C::init();
if (ret != OK) {
return ret;
}
/* switch off LED on start */
send_led_enable(false);
send_led_rgb();
return OK;
}
通常来讲 init都是用来初始化的。这里其实也就是初始化。先是初始化 I2C，然后就把 LED给关了。在 I2C初始化过程中我们前面看到，它调用了一个 probe函数。在 Linux中写 platform驱动的时候我们经常碰到这个接口，在设备跟驱动匹配成功时会调用这个驱动，用来做一些初始化工作。在这里其实都差不多。
int RGBLED::probe()
{
int ret;
bool on, powersave;
uint8_t r, g, b;
/**
this may look strange, but is needed. There is a serial
EEPROM (Microchip-24aa01) on the PX4FMU-v1 that responds to
a bunch of I2C addresses, including the 0x55 used by this
LED device. So we need to do enough operations to be sure
we are talking to the right device. These 3 operations seem
to be enough, as the 3rd one consistently fails if no
RGBLED is on the bus.
*/
if ((ret=get(on, powersave, r, g, b)) != OK ||
(ret=send_led_enable(false) != OK) ||
(ret=send_led_enable(false) != OK)) {
return ret;
}
return ret;
}
单从代码上看，这里有点让人费解。而从注释来看是为了确保我们操作对的设备。可能要详细理解这段代码还需要专门花点时间，毕竟我们现在只是从软件方面解读 PX4。
2. 控制LED
test其实已经提供了控制 LED的示例，即通过 ioctl进行控制。那么在应用层又是如何控制 LED的呢？
bitcraze@bitcraze-vm:~/apm$ grep -nr RGBLED_DEVICE_PATH ./ardupilot/
./ardupilot/libraries/AP_Notify/ToshibaLED_PX4.cpp:38: _rgbled_fd = open(RGBLED_DEVICE_PATH, 0);
./ardupilot/libraries/AP_Notify/ToshibaLED_PX4.cpp:40: hal.console->printf("Unable to open " RGBLED_DEVICE_PATH);
bitcraze@bitcraze-vm:~/apm$
bool ToshibaLED_PX4::hw_init()
{
// open the rgb led device
_rgbled_fd = open(RGBLED_DEVICE_PATH, 0);
if (_rgbled_fd == -1) {
hal.console->printf("Unable to open " RGBLED_DEVICE_PATH);
return false;
}
ioctl(_rgbled_fd, RGBLED_SET_MODE, (unsigned long)RGBLED_MODE_ON);
last.zero();
next.zero();
hal.scheduler->register_io_process(AP_HAL_MEMBERPROC(&ToshibaLED_PX4::update_timer));
return true;
}
这里打开了我们的 LED设备，而且通过 ioctl对设备进行了一次操作。那么可以想象，后面肯定都是通过 _rgbled_fd对设备进行操作的。
// set_rgb - set color as a combination of red, green and blue values
bool ToshibaLED_PX4::hw_set_rgb(uint8_t red, uint8_t green, uint8_t blue)
{
hal.scheduler->suspend_timer_procs();
next[0] = red;
next[1] = green;
next[2] = blue;
hal.scheduler->resume_timer_procs();
return true;
}
void ToshibaLED_PX4::update_timer(void)
{
if (last == next) {
return;
}
rgbled_rgbset_t v;
v.red = next[0];
v.green = next[1];
v.blue = next[2];
ioctl(_rgbled_fd, RGBLED_SET_RGB, (unsigned long)&v);
last = next;
}
真实情况也正是这样。但是这个时候如果继续跟踪代码我相信你会跟我一样抓狂。
radiolink@ubuntu:~/px4$ grep -nr ToshibaLED_PX4 ./ardupilot/
./ardupilot/libraries/AP_Notify/AP_Notify.h:25:#include
./ardupilot/libraries/AP_Notify/AP_Notify.h:66: ToshibaLED_PX4 toshibaled;
./ardupilot/libraries/AP_Notify/ToshibaLED_PX4.h:25:class ToshibaLED_PX4 :public ToshibaLED
./ardupilot/libraries/AP_Notify/examples/ToshibaLED_test/ToshibaLED_test.pde:35:static ToshibaLED_PX4 toshiba_led;
./ardupilot/libraries/AP_Notify/ToshibaLED_PX4.cpp:22:#include "ToshibaLED_PX4.h"
./ardupilot/libraries/AP_Notify/ToshibaLED_PX4.cpp:35:bool ToshibaLED_PX4::hw_init()
./ardupilot/libraries/AP_Notify/ToshibaLED_PX4.cpp:46: hal.scheduler->register_io_process(AP_HAL_MEMBERPROC(&ToshibaLED_PX4::update_timer));
./ardupilot/libraries/AP_Notify/ToshibaLED_PX4.cpp:51:bool ToshibaLED_PX4::hw_set_rgb(uint8_t red, uint8_t green, uint8_t blue)
./ardupilot/libraries/AP_Notify/ToshibaLED_PX4.cpp:61:void ToshibaLED_PX4::update_timer(void)
./ardupilot/module.mk:3:SRCS =
radiolink@ubuntu:~/px4$
SRCS内容特别长，因为 SRCS记录了 ardupilot这个应用程序所用到的所有源文件。从这段匹配结果来看，我们很容易知道我们的 LED是由 "ToshibaLED_test.pde"进行控制的，但你可别忘了它的路径中可是有个 "examples"！你再取看它的源码：
void setup(void)
{
}
void loop(void)
{
}
AP_HAL_MAIN();
从这里看我们似乎已经找错方向了，却不知错误是从什么时候开始的。于是我开始怀疑 PX4运行的时候到底是不是通过 rgbled来进行控制的。于是我想了一招，执行 "rgbled stop"命令，结构灯真的就不亮了。然后我在执行 "rgbled rgb 16 16 16"命令，灯又开始闪烁了。那么也许 rgb就会是我们救命的稻草了。
3.rgb
在 rgbled的入口函数中 rgb的内容如下：
if (!strcmp(verb, "rgb")) {
if (argc < 5) {
errx(1, "Usage: rgbled rgb
}
fd = open(RGBLED_DEVICE_PATH, 0);
if (fd == -1) {
errx(1, "Unable to open " RGBLED_DEVICE_PATH);
}
rgbled_rgbset_t v;
v.red = strtol(argv[2], NULL, 0);
v.green = strtol(argv[3], NULL, 0);
v.blue = strtol(argv[4], NULL, 0);
ret = ioctl(fd, RGBLED_SET_RGB, (unsigned long)&v);
ret = ioctl(fd, RGBLED_SET_MODE, (unsigned long)RGBLED_MODE_ON);
close(fd);
exit(ret);
}
貌似，好像这段代码除了调用了几个接口也没有什么特别之处，但根据前面的判断秘密肯定就藏在这里，除非说这一次我又判断失误了。既然已经肯定这里有秘密，用排除法 ioctl是最有嫌疑的。
int RGBLED::ioctl(struct file *filp, int cmd, unsigned long arg)
{
int ret = ENOTTY;
switch (cmd) {
case RGBLED_SET_RGB:
/* set the specified color */
_r = ((rgbled_rgbset_t *) arg)->red;
_g = ((rgbled_rgbset_t *) arg)->green;
_b = ((rgbled_rgbset_t *) arg)->blue;
send_led_rgb();
return OK;
case RGBLED_SET_COLOR:
/* set the specified color name */
set_color((rgbled_color_t)arg);
send_led_rgb();
return OK;
case RGBLED_SET_MODE:
/* set the specified mode */
set_mode((rgbled_mode_t)arg);
return OK;
case RGBLED_SET_PATTERN:
/* set a special pattern */
set_pattern((rgbled_pattern_t *)arg);
return OK;
default:
/* see if the parent class can make any use of it */
ret = CDev::ioctl(filp, cmd, arg);
break;
}
return ret;
}
所以我们看到，这里又引出了另外两个函数： send_led_rgb和 set_mode。其源码如下：
int RGBLED::send_led_rgb()
{
/* To scale from 0..255 -> 0..15 shift right by 4 bits */
const uint8_t msg[6] = {
SUB_ADDR_PWM0, (uint8_t)((int)(_b * _brightness) >> 4),
SUB_ADDR_PWM1, (uint8_t)((int)(_g * _brightness) >> 4),
SUB_ADDR_PWM2, (uint8_t)((int)(_r * _brightness) >> 4)
};
return transfer(msg, sizeof(msg), nullptr, 0);
}
void RGBLED::set_mode(rgbled_mode_t mode)
{
if (mode != _mode) {
_mode = mode;
switch (mode) {
default:
warnx("mode unknown");
break;
}
/* if it should run now, start the workq */
if (_should_run && !_running) {
_running = true;
work_queue(LPWORK, &_work, (worker_t)&RGBLED::led_trampoline, this, 1);
}
}
}
mode部分源码我没有全部粘出来。
可能你会觉得这里除了调用了几个函数似乎也并没有什么特别，但是请记住，这里调用了一个 work_queue函数，也就是说用到了工作队列，其定义如下：
int work_queue(int qid, FAR struct work_s *work, worker_t worker,
FAR void *arg, uint32_t delay)
所以，在延时 delay之后会调用 RGBLED::led_trampoline这个函数。那么它又干了些啥事呢？
void RGBLED::led_trampoline(void *arg)
{
RGBLED *rgbl = reinterpret_cast
rgbl->led();
}
/**
* Main loop function
*/
void RGBLED::led()
{
if (!_should_run) {
_running = false;
return;
}
switch (_mode) {
case RGBLED_MODE_BLINK_SLOW:
case RGBLED_MODE_BLINK_NORMAL:
case RGBLED_MODE_BLINK_FAST:
if (_counter >= 2)
_counter = 0;
send_led_enable(_counter == 0);
break;
case RGBLED_MODE_BREATHE:
if (_counter >= 62)
_counter = 0;
int n;
if (_counter < 32) {
n = _counter;
} else {
n = 62 - _counter;
}
_brightness = n * n / (31.0f * 31.0f);
send_led_rgb();
break;
case RGBLED_MODE_PATTERN:
/* don't run out of the pattern array and stop if the next frame is 0 */
if (_counter >= RGBLED_PATTERN_LENGTH || _pattern.duration[_counter] <= 0)
_counter = 0;
set_color(_pattern.color[_counter]);
send_led_rgb();
_led_interval = _pattern.duration[_counter];
break;
default:
break;
}
_counter++;
/* re-queue ourselves to run again later */
work_queue(LPWORK, &_work, (worker_t)&RGBLED::led_trampoline, this, _led_interval);
}
由于 rgb中设置 _mode为 RGBLED_MODE_ON，所以必须有谁去修改它的值，否则 LED将一直处于亮的状态，而不是闪烁。
当然，看到这里我们知道 LED如何能够保持闪烁，但是如何改变 LED状态，如颜色呢？从上面的代码看是跟 _mode这个变量有关，但继续跟踪代码你就会发现你只能通过 set_mode函数来设置_mode，但是这个操作又只有通过 ioctl的 RGBLED_SET_MODE命令来调用。
radiolink@ubuntu:~/apm$ grep -nr RGBLED_SET_MODE ./
./PX4Firmware/src/modules/commander/commander_helper.cpp:272: ioctl(rgbleds, RGBLED_SET_MODE, (unsigned long)mode);
./PX4Firmware/src/drivers/drv_rgbled.h:76:#define RGBLED_SET_MODE _RGBLEDIOC(6)
./PX4Firmware/src/drivers/drv_rgbled.h:112:/* enum passed to RGBLED_SET_MODE ioctl()*/
./PX4Firmware/src/drivers/drv_io_expander.h:66:/* enum passed to RGBLED_SET_MODE ioctl()*/
./PX4Firmware/src/drivers/rgbled/rgbled.cpp:234: case RGBLED_SET_MODE:
./PX4Firmware/src/drivers/rgbled/rgbled.cpp:663: ret = ioctl(fd, RGBLED_SET_MODE, (unsigned long)RGBLED_MODE_PATTERN);
./PX4Firmware/src/drivers/rgbled/rgbled.cpp:681: ret = ioctl(fd, RGBLED_SET_MODE, (unsigned long)RGBLED_MODE_OFF);
./PX4Firmware/src/drivers/rgbled/rgbled.cpp:708: ret = ioctl(fd, RGBLED_SET_MODE, (unsigned long)RGBLED_MODE_ON);
./ardupilot/libraries/AP_Notify/ToshibaLED_PX4.cpp:43: ioctl(_rgbled_fd, RGBLED_SET_MODE, (unsigned long)RGBLED_MODE_ON);
radiolink@ubuntu:~/apm$
从匹配结果来判断这个操作是在 commander_helper.cpp中完成的。首先排除 rgbled本身， ToshibaLED_PX4.cpp前面我们分析的就是它，确定是死路一条，况且它设置的是固定的模式。于是剩下的就只有 commander_helper.cpp了，从匹配结果来看，它可以设置多种模式。
但是问题也随之而来，当你继续跟踪代码的时候，你会最终找到一个函数：commander_main。从函数名我们很容易想到它对应了一个命令： commander。但是很遗憾，脚本中根本就没有该命令。而且，当你尝试进行代码匹配你会发现：
radiolink@ubuntu:~/apm$ grep -nr commander_main ./
./PX4Firmware/src/modules/commander/commander.cpp:200:extern "C" __EXPORTint commander_main(int argc, char *argv[]);
./PX4Firmware/src/modules/commander/commander.cpp:246:int commander_main(int argc,char *argv[])
radiolink@ubuntu:~/apm$
于是我也只能无语了。那么是否就到此为止了呢？事情还远远没有结束！
既然这条路已经走不通了，那么只能回过头来再考虑 ToshibaLED_PX4.cpp了。那么我们前面到底忽略了什么呢？
当我回过头去看前面的笔记时，我惊讶地发现：
radiolink@ubuntu:~/px4$ grep -nr ToshibaLED_PX4 ./ardupilot/
./ardupilot/libraries/AP_Notify/AP_Notify.h:25:#include
./ardupilot/libraries/AP_Notify/AP_Notify.h:66: ToshibaLED_PX4 toshibaled;
./ardupilot/libraries/AP_Notify/ToshibaLED_PX4.h:25:class ToshibaLED_PX4 :public ToshibaLED
./ardupilot/libraries/AP_Notify/examples/ToshibaLED_test/ToshibaLED_test.pde:35:static ToshibaLED_PX4 toshiba_led;
./ardupilot/libraries/AP_Notify/ToshibaLED_PX4.cpp:22:#include "ToshibaLED_PX4.h"
./ardupilot/libraries/AP_Notify/ToshibaLED_PX4.cpp:35:bool ToshibaLED_PX4::hw_init()
./ardupilot/libraries/AP_Notify/ToshibaLED_PX4.cpp:46: hal.scheduler->register_io_process(AP_HAL_MEMBERPROC(&ToshibaLED_PX4::update_timer));
./ardupilot/libraries/AP_Notify/ToshibaLED_PX4.cpp:51:bool ToshibaLED_PX4::hw_set_rgb(uint8_t red, uint8_t green, uint8_t blue)
./ardupilot/libraries/AP_Notify/ToshibaLED_PX4.cpp:61:void ToshibaLED_PX4::update_timer(void)
./ardupilot/module.mk:3:SRCS =
radiolink@ubuntu:~/px4$
=前面我看到这段信息就很直接地以为是在 ToshibaLED_test.pde中对 LED进行控制，但是我们忽略了一个头文件： AP_Notify.h。因为我一直都是用 C的，所以很自然地以为这里只是普通的声明。所以我忘了一点：这是 PX4源码，用的是 C++！而事情也正是在这里峰回路转了。
class AP_Notify
{
public:
/// notify_type - bitmask of notification types
struct notify_type {
uint16_t initialising : 1; // 1 if initialising and copter should not be moved
uint16_t gps_status : 3; // 0 = no gps, 1 = no lock, 2 = 2d lock, 3 = 3d lock, 4 = dgps lock, 5 = rtk lock
uint16_t gps_glitching : 1; // 1 if gps position is not good
uint16_t armed : 1; // 0 = disarmed, 1 = armed
uint16_t pre_arm_check : 1; // 0 = failing checks, 1 = passed
uint16_t save_trim : 1; // 1 if gathering trim data
uint16_t esc_calibration : 1; // 1 if calibrating escs
uint16_t failsafe_radio : 1; // 1 if radio failsafe
uint16_t failsafe_battery : 1; // 1 if battery failsafe
uint16_t failsafe_gps : 1; // 1 if gps failsafe
uint16_t arming_failed : 1; // 1 if copter failed to arm after user input
uint16_t parachute_release : 1; // 1 if parachute is being released
// additional flags
uint16_t external_leds : 1; // 1 if external LEDs are enabled (normally only used for copter)
};
// the notify flags are static to allow direct class access
// without declaring the object
static struct notify_type flags;
// initialisation
void init(bool enable_external_leds);
/// update - allow updates of leds that cannot be updated during a timed interrupt
void update(void);
private:
// individual drivers
AP_BoardLED boardled;
#if CONFIG_HAL_BOARD == HAL_BOARD_PX4
ToshibaLED_PX4 toshibaled;
ToneAlarm_PX4 tonealarm;
#elif CONFIG_HAL_BOARD == HAL_BOARD_APM1 || CONFIG_HAL_BOARD == HAL_BOARD_APM2
ToshibaLED_I2C toshibaled;
ExternalLED externalled;
Buzzer buzzer;
#elif CONFIG_HAL_BOARD == HAL_BOARD_VRBRAIN
ToshibaLED_I2C toshibaled;
ExternalLED externalled;
Buzzer buzzer;
#else
ToshibaLED_I2C toshibaled;
#endif
};
所以我们看到，这里对应的是一个类： AP_Notify。如果你去查 "notify"这个单词，你会看到它是 “通告，通知；公布”的意思。这样一来就说得通了。而这里的flags请注意，它前面可有个 "static"，在 C++中表示 flags属于整个类。所以我们很容易匹配到这样的一些结果：
./ardupilot/ArduCopter/motors.pde:40: AP_Notify::flags.arming_failed =true;
./ardupilot/ArduCopter/motors.pde:45: AP_Notify::flags.arming_failed =true;
./ardupilot/ArduCopter/motors.pde:75: AP_Notify::flags.arming_failed = false;
./ardupilot/ArduCopter/motors.pde:123: AP_Notify::flags.armed = true;
./ardupilot/ArduCopter/motors.pde:151: AP_Notify::flags.armed = false;
即直接通过类名称修改该变量的值。
radiolink@ubuntu:~/apm$ grep -nr AP_Notify ./ardupilot/
./ardupilot/ArduCopter/ArduCopter.pde:145:#include
./ardupilot/ArduCopter/ArduCopter.pde:203:// AP_Notify instance
./ardupilot/ArduCopter/ArduCopter.pde:204:static AP_Notify notify;
./ardupilot/ArduCopter/ArduCopter.pde:1207: // run glitch protection and update AP_Notify if home has been initialised
./ardupilot/ArduCopter/ArduCopter.pde:1211: if (AP_Notify::flags.gps_glitching != report_gps_glitch) {
./ardupilot/ArduCopter/ArduCopter.pde:1217: AP_Notify::flags.gps_glitching = report_gps_glitch;
radiolink@ubuntu:~/apm$
那么 "ArduCopter.pde"这个源文件我们应该不陌生了，在 PX4中这是很重要的一个文件，因为如果是在 APM中我们认为我们的程序是从这里开始的。在 PX4中，如果抛开操作系统个人觉得我们也应该这样认为。那么分析到这里，我觉得这条路已经走通了，剩下的就是去看下 LED具体是如何控制的。这将顺着 notify继续讨论下去。
4. Notify
前面我们看到在 AP_Notify中只有两个函数，我们现在就先来看下这两个函数都做了哪些事情。
// initialisation
void AP_Notify::init(bool enable_external_leds)
{
AP_Notify::flags.external_leds = enable_external_leds;
boardled.init();
toshibaled.init();
#if CONFIG_HAL_BOARD == HAL_BOARD_PX4
tonealarm.init();
#endif
#if CONFIG_HAL_BOARD == HAL_BOARD_APM1 || CONFIG_HAL_BOARD == HAL_BOARD_APM2
externalled.init();
buzzer.init();
#endif
#if CONFIG_HAL_BOARD == HAL_BOARD_VRBRAIN
externalled.init();
buzzer.init();
#endif
}
// main update function, called at 50Hz
void AP_Notify::update(void)
{
boardled.update();
toshibaled.update();
#if CONFIG_HAL_BOARD == HAL_BOARD_PX4
tonealarm.update();
#endif
#if CONFIG_HAL_BOARD == HAL_BOARD_APM1 || CONFIG_HAL_BOARD == HAL_BOARD_APM2
externalled.update();
buzzer.update();
#endif
#if CONFIG_HAL_BOARD == HAL_BOARD_VRBRAIN
externalled.update();
buzzer.update();
#endif
}
其实就是调用别人的 init跟 update函数，而且注释还告诉了我们 update函数调用的频率为 50Hz。这里我们关系的只是 toshibaled，其它的我们现在就不去看了。其实你在这里把 toshibaled搞通了，其它的也一样理顺了。
radiolink@ubuntu:~/apm$ grep -nr notify.init ./ardupilot/
./ardupilot/APMrover2/APMrover2.pde:553: notify.init(false);
./ardupilot/Build.ArduCopter/ArduCopter.cpp:15657: notify.init(true);
./ardupilot/ArduPlane/ArduPlane.pde:840: notify.init(false);
./ardupilot/AntennaTracker/AntennaTracker.pde:268: notify.init(false);
./ardupilot/ArduCopter/system.pde:141: notify.init(true);
radiolink@ubuntu:~/apm$
static void init_ardupilot()
{
bool enable_external_leds = true;
// init EPM cargo gripper
#if EPM_ENABLED == ENABLED
epm.init();
enable_external_leds = !epm.enabled();
#endif
// initialise notify system
// disable external leds if epm is enabled because of pin conflict on the APM
notify.init(enable_external_leds);
}
前面我们分析 "ArduCopter"遇到过 "init_ardupilot"这个函数，当时我因为说不清楚这里的每个东西都是做什么用的，所以就略过了。而现在，我们终于又搞懂了一个。
radiolink@ubuntu:~/apm$ grep -nr notify.update ./ardupilot/
./ardupilot/APMrover2/system.pde:424: notify.update();
./ardupilot/APMrover2/GCS_Mavlink.pde:1167: notify.update();
./ardupilot/Build.ArduCopter/ArduCopter.cpp:3971: notify.update();
./ardupilot/Build.ArduCopter/ArduCopter.cpp:13066: notify.update();
./ardupilot/ArduPlane/system.pde:554: notify.update();
./ardupilot/ArduPlane/GCS_Mavlink.pde:1605: notify.update();
./ardupilot/AntennaTracker/system.pde:118: notify.update();
./ardupilot/AntennaTracker/GCS_Mavlink.pde:772: notify.update();
./ardupilot/ArduCopter/leds.pde:7: notify.update();
./ardupilot/ArduCopter/GCS_Mavlink.pde:1632: notify.update();
radiolink@ubuntu:~/apm$
// updates the status of notify
// should be called at 50hz
static void update_notify()
{
notify.update();
}
static const AP_Scheduler::Task scheduler_tasks[] PROGMEM = {
{ rc_loop, 4, 10 },
{ throttle_loop, 8, 45 },
{ update_GPS, 8, 90 },
{ update_notify, 8, 10 },
前面我们已经知道了 PX4控制速率是 400Hz，所以不必去看那个 100Hz的了。而 scheduler_tasks里面存放的是 ArduCopter中的任务，这是在线程内部实现的任务调度，在 APM中就已经存在，所以跟 Nuttx没关。
那么我们现在要做的就是沿着 toshibaled.init()跟 toshibaled.update()这两个函数往下走。
5. toshibaled
首先呢我们当然是应该看看 ToshibaLED_PX4的定义。
class ToshibaLED_PX4 : public ToshibaLED
{
public:
bool hw_init(void);
bool hw_set_rgb(uint8_t r, uint8_t g, uint8_t b);
private:
int _rgbled_fd;
void update_timer(void);
VectorN
};
从定义中我们看到， init跟 update并没有被重写，也就是继承了父类中的方法。那么：
void ToshibaLED::init()
{
_healthy = hw_init();
}
// update - updates led according to timed_updated. Should be called
// at 50Hz
void ToshibaLED::update()
{
// return immediately if not enabled
if (!_healthy) {
return;
}
update_colours();
set_rgb(_red_des, _green_des, _blue_des);
}
如果你手上的是最新版源码，你会看到一个新的类： "class ToshibaLED: public RGBLed"。多少个类都无所谓，都差不多。 hw_init这个函数我们前面也看了，就是做一些初始化工作，包括打开设备。
bool ToshibaLED_PX4::hw_init()
{
// open the rgb led device
_rgbled_fd = open(RGBLED_DEVICE_PATH, 0);
if (_rgbled_fd == -1) {
hal.console->printf("Unable to open " RGBLED_DEVICE_PATH);
return false;
}
ioctl(_rgbled_fd, RGBLED_SET_MODE, (unsigned long)RGBLED_MODE_ON);
last.zero();
next.zero();
hal.scheduler->register_io_process(AP_HAL_MEMBERPROC(&ToshibaLED_PX4::update_timer));
return true;
}
其中稍微要注意的就是 hal这条语句。这里又注册了一个调度函数。因为这里是通过 hal注册的，所以我们知道这里又是由 ArduCopter提供任务调度。如果忽略了这一点你看 update又会觉得糊涂，因为你去看源码就会发现 set_rgb最终调用了 hw_set_rgb函数，但是你去看该函数：
// set_rgb - set color as a combination of red, green and blue values
bool ToshibaLED_PX4::hw_set_rgb(uint8_t red, uint8_t green, uint8_t blue)
{
hal.scheduler->suspend_timer_procs();
next[0] = red;
next[1] = green;
next[2] = blue;
hal.scheduler->resume_timer_procs();
return true;
}
你会觉得它好像并没有实际操作硬件。但其实我们刚看到的只是更新了数据，真正把数据写入硬件的正是 ToshibaLED_PX4::update_timer这个函数的工作。具体颜色是如何更新的去看 update_colours这个函数最清楚。
void ToshibaLED_PX4::update_timer(void)
{
if (last == next) {
return;
}
rgbled_rgbset_t v;
v.red = next[0];
v.green = next[1];
v.blue = next[2];
ioctl(_rgbled_fd, RGBLED_SET_RGB, (unsigned long)&v);
last = next;
}
知道这里我才知道前面我认为调用 ioctl应该使用 "RGBLED_SET_MODE"错得有多么离谱。但我们如果去看 rgbled的代码你会发现：
int
RGBLED::ioctl(struct file *filp, int cmd, unsigned long arg)
{
int ret = ENOTTY;
switch (cmd) {
case RGBLED_SET_RGB:
/* set the specified color */
_r = ((rgbled_rgbset_t *) arg)->red;
_g = ((rgbled_rgbset_t *) arg)->green;
_b = ((rgbled_rgbset_t *) arg)->blue;
send_led_rgb();
return OK;
/**
* Send RGB PWM settings to LED driver according to current color and brightness
*/
int
RGBLED::send_led_rgb()
{
/* To scale from 0..255 -> 0..15 shift right by 4 bits */
const uint8_t msg[6] = {
SUB_ADDR_PWM0, (uint8_t)((int)(_b * _brightness) >> 4),
SUB_ADDR_PWM1
PX4 FMU [8] BoardLED
如果你到 "/dev"目录下去找，你会发现似乎没有一个设备文件与 BoardLED相关，也就是说 BoardLED可能不是通过设备文件来操作的。那究竟这里又是怎么个情况呢？我们继续往下看。
从前面对 rgbled的分析我们知道这个时候我们应该直接以 "void AP_Notify::update(void)"作为切入口。
// main update function, called at 50Hz
void AP_Notify::update(void)
{
boardled.update();
toshibaled.update();
private:
// individual drivers
AP_BoardLED boardled;
所以对应的类为： "AP_BoardLED"。
class AP_BoardLED
{
public:
// initialise the LED driver
void init(void);
// should be called at 50Hz
void update(void);
private:
// counter incremented at 50Hz
uint8_t _counter;
};
我们看到，它其实就只有两个接口函数，我们很容易想到它们由 AP_Notify在 init跟 update中调用。
extern const AP_HAL::HAL& hal;
void AP_BoardLED::init(void)
{
// setup the main LEDs as outputs
hal.gpio->pinMode(HAL_GPIO_A_LED_PIN, HAL_GPIO_OUTPUT);
hal.gpio->pinMode(HAL_GPIO_B_LED_PIN, HAL_GPIO_OUTPUT);
hal.gpio->pinMode(HAL_GPIO_C_LED_PIN, HAL_GPIO_OUTPUT);
// turn all lights off
hal.gpio->write(HAL_GPIO_A_LED_PIN, HAL_GPIO_LED_OFF);
hal.gpio->write(HAL_GPIO_B_LED_PIN, HAL_GPIO_LED_OFF);
hal.gpio->write(HAL_GPIO_C_LED_PIN, HAL_GPIO_LED_OFF);
}
#elif CONFIG_HAL_BOARD == HAL_BOARD_PX4
# define HAL_GPIO_A_LED_PIN 27
# define HAL_GPIO_B_LED_PIN 26
# define HAL_GPIO_C_LED_PIN 25
# define HAL_GPIO_LED_ON LOW
# define HAL_GPIO_LED_OFF HIGH
#elif CONFIG_HAL_BOARD == HAL_BOARD_FLYMAPLE
其实看到这里我都觉得很奇怪，似乎它还有 LED，而且是直接通过 IO进行控制的。要搞清楚这些，我们就需要先搞清楚 hal.gpio是什么。这又涉及到一个类： "HAL_PX4"。
AP_HAL_MAIN();
#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__
// AP_HAL instance
const AP_HAL::HAL& hal = AP_HAL_BOARD_DRIVER;
#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
#elif CONFIG_HAL_BOARD == HAL_BOARD_LINUX
const HAL_PX4 AP_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 */
{}
这样我们就知道， gpio实际上是调用的 gpioDriver。那么很自然地，我们会继续往下跟踪代码。
static PX4GPIO gpioDriver;
class PX4::PX4GPIO : public AP_HAL::GPIO {
public:
PX4GPIO();
void init();
void pinMode(uint8_t pin, uint8_t output);
int8_t analogPinToDigitalPin(uint8_t pin);
uint8_t read(uint8_t pin);
void write(uint8_t pin, uint8_t value);
void toggle(uint8_t pin);
/* Alternative interface: */
AP_HAL::DigitalSource* channel(uint16_t n);
/* Interrupt interface: */
bool attach_interrupt(uint8_t interrupt_num, AP_HAL::Proc p, uint8_t mode);
/* return true if USB cable is connected */
bool usb_connected(void);
private:
int _led_fd;
int _tone_alarm_fd;
int _gpio_fmu_fd;
int _gpio_io_fd;
};
在这里最让人印象深刻的我想就是这几个 fd了。因为在 Linux中我们通常将 open的返回值保存在 fd中，所以这里四个 fd应该是对应了四个文件，而且是设备文件。因为如果是普通文件意义不是很大。而它的 init函数前面我们分析的时候看到了，是在 main_loop函数中调用的。
void PX4GPIO::init()
{
#ifdef CONFIG_ARCH_BOARD_PX4FMU_V1
_led_fd = open(LED_DEVICE_PATH, O_RDWR);
if (_led_fd == -1) {
hal.scheduler->panic("Unable to open " LED_DEVICE_PATH);
}
if (ioctl(_led_fd, LED_OFF, LED_BLUE) != 0) {
hal.console->printf("GPIO: Unable to setup GPIO LED BLUE\n");
}
if (ioctl(_led_fd, LED_OFF, LED_RED) != 0) {
hal.console->printf("GPIO: Unable to setup GPIO LED RED\n");
}
#endif
_tone_alarm_fd = open("/dev/tone_alarm", O_WRONLY);
if (_tone_alarm_fd == -1) {
hal.scheduler->panic("Unable to open /dev/tone_alarm");
}
_gpio_fmu_fd = open(PX4FMU_DEVICE_PATH, 0);
if (_gpio_fmu_fd == -1) {
hal.scheduler->panic("Unable to open GPIO");
}
#ifdef CONFIG_ARCH_BOARD_PX4FMU_V1
if (ioctl(_gpio_fmu_fd, GPIO_CLEAR, GPIO_EXT_1) != 0) {
hal.console->printf("GPIO: Unable to setup GPIO_1\n");
}
#endif
// also try to setup for the relay pins on the IO board
_gpio_io_fd = open(PX4IO_DEVICE_PATH, O_RDWR);
if (_gpio_io_fd == -1) {
hal.console->printf("GPIO: Unable to open px4io\n");
}
}
/**
* Device paths for things that support the GPIO ioctl protocol.
*/
# define PX4FMU_DEVICE_PATH "/dev/px4fmu"
# define PX4IO_DEVICE_PATH "/dev/px4io"
所以这里又出现了几个新的设备文件，待会我们又有的忙活了。这可是一颗大地雷啊。
但是当我们去查看 pinMode函数源码时，我们却不免大跌眼镜：
void PX4GPIO::pinMode(uint8_t pin, uint8_t output)
{
switch (pin) {
case PX4_GPIO_FMU_SERVO_PIN(0) ... PX4_GPIO_FMU_SERVO_PIN(5):
ioctl(_gpio_fmu_fd, output?GPIO_SET_OUTPUT:GPIO_SET_INPUT, 1U<<(pin-PX4_GPIO_FMU_SERVO_PIN(0)));
break;
}
}
/*
start servo channels used as GPIO at 50. Pin 50 is
the first FMU servo pin
*/
#define PX4_GPIO_FMU_SERVO_PIN(n) (n+50)
而我们前面在 init中传递下来的值也就二十几，而在 write函数中也是这个情况，
void PX4GPIO::write(uint8_t pin, uint8_t value)
{
case PX4_GPIO_FMU_SERVO_PIN(0) ... PX4_GPIO_FMU_SERVO_PIN(5):
ioctl(_gpio_fmu_fd, value==LOW?GPIO_CLEAR:GPIO_SET, 1U<<(pin-PX4_GPIO_FMU_SERVO_PIN(0)));
break;
}
}
除非说我对代码的理解有误，要不然我觉得这里又让人匪夷所思。我的理解是它等价于下面的代码：
case PX4_GPIO_FMU_SERVO_PIN(0):
case PX4_GPIO_FMU_SERVO_PIN(1):
case PX4_GPIO_FMU_SERVO_PIN(2):
case PX4_GPIO_FMU_SERVO_PIN(3):
case PX4_GPIO_FMU_SERVO_PIN(4):
case PX4_GPIO_FMU_SERVO_PIN(5):
ioctl(_gpio_fmu_fd, value==LOW?GPIO_CLEAR:GPIO_SET, 1U<<(pin-PX4_GPIO_FMU_SERVO_PIN(0)));
break;
但这样一来我们的 "HAL_GPIO_A_LED_PIN"传下来根本就找不到对应的 case，那又如何达到操作硬件的目的呢？而且从 ioctl这条语句来看， pin显然不应该比 50小。
但是，反复查了几遍，却查不出我可曾漏掉了任何东西。当然，现在的情况跟 rgbled又不一样， rgbled是由于自己的疏忽卡在一些点上怎么都走不通。而现在是可以走通，只是让人觉得不合理。于是我觉得比较合理的解释是 AP_BoardLED主要是为了对 APM的兼容。既然如此，那么 AP_BoardLED::update我们就不必继续往下看了，就直接去把那两颗地雷给挖了吧。
PX4 FMU [9] fmu
这是我们前面踩到的两颗地雷中的一颗，其设备文件为： "PX4FMU_DEVICE_PATH"即 "/dev/px4fmu"。现在我们要做的第一件事就是确定源码在哪里。
radiolink@ubuntu:~/apm$ grep -nr PX4FMU_DEVICE_PATH ./
./PX4Firmware/src/modules/gpio_led/gpio_led.c:211: gpio_dev = PX4FMU_DEVICE_PATH;
./PX4Firmware/src/drivers/drv_gpio.h:64:# define PX4FMU_DEVICE_PATH "/dev/px4fmu"
./PX4Firmware/src/drivers/drv_gpio.h:92:# define PX4FMU_DEVICE_PATH "/dev/px4fmu"
./PX4Firmware/src/drivers/drv_gpio.h:102:# define PX4FMU_DEVICE_PATH "/dev/px4fmu"
./PX4Firmware/src/drivers/ardrone_interface/ardrone_motor_control.c:112: fd = open(PX4FMU_DEVICE_PATH, 0);
./PX4Firmware/src/drivers/px4fmu/fmu.cpp:239: CDev("fmuservo", PX4FMU_DEVICE_PATH),
./PX4Firmware/src/drivers/px4fmu/fmu.cpp:1636: fd = open(PX4FMU_DEVICE_PATH, O_RDWR);
./PX4Firmware/src/drivers/px4fmu/fmu.cpp:1655: fd = open(PX4FMU_DEVICE_PATH, O_RDWR);
radiolink@ubuntu:~/apm$
其实我们在前面分析的时候，你要是稍微细心一点，你就会发现， PX4的底层驱动基本上都放在 "PX4Firmware/src/drivers"这个路径下面。
但我们如果去看 "g_builtins"数组，里边就有这么一条语句：
{"fmu", SCHED_PRIORITY_DEFAULT, CONFIG_PTHREAD_STACK_DEFAULT, fmu_main},
这表示 fmu跟我们的 mpu6050一样都做成了一个命令。那么在脚本中关于它的命令又是什么呢？
if [ $BOARD == FMUv1 -a $deviceD == /dev/ttyS1 ]
then
echo "Setting FMU mode_serial"
fmu mode_serial
else
echo "Setting FMU mode_pwm"
fmu mode_pwm
fi
由于我们是 V2版本，所以是后一条命令。而现在，我们将看到它的入口函数： fmu_main。
int fmu_main(int argc, char *argv[])
{
PortMode new_mode = PORT_MODE_UNSET;
const char *verb = argv[1];
if (!strcmp(verb, "stop")) {
fmu_stop();
errx(0, "FMU driver stopped");
}
if (!strcmp(verb, "id")) {
uint8_t id[12];
(void)get_board_serial(id);
errx(0, "Board serial:\n %02X%02X%02X%02X %02X%02X%02X%02X %02X%02X%02X%02X",
(unsigned)id[0], (unsigned)id[1], (unsigned)id[2], (unsigned)id[3], (unsigned)id[4], (unsigned)id[5],
(unsigned)id[6], (unsigned)id[7], (unsigned)id[8], (unsigned)id[9], (unsigned)id[10], (unsigned)id[11]);
}
if (fmu_start() != OK)
errx(1, "failed to start the FMU driver");
/*
* Mode switches.
*/
if (!strcmp(verb, "mode_gpio")) {
new_mode = PORT_FULL_GPIO;
} else if (!strcmp(verb, "mode_pwm")) {
new_mode = PORT_FULL_PWM;
#if defined(CONFIG_ARCH_BOARD_PX4FMU_V1)
} else if (!strcmp(verb, "mode_serial")) {
new_mode = PORT_FULL_SERIAL;
} else if (!strcmp(verb, "mode_gpio_serial")) {
new_mode = PORT_GPIO_AND_SERIAL;
} else if (!strcmp(verb, "mode_pwm_serial")) {
new_mode = PORT_PWM_AND_SERIAL;
} else if (!strcmp(verb, "mode_pwm_gpio")) {
new_mode = PORT_PWM_AND_GPIO;
#endif
}
/* was a new mode set? */
if (new_mode != PORT_MODE_UNSET) {
/* yes but it's the same mode */
if (new_mode == g_port_mode)
return OK;
/* switch modes */
int ret = fmu_new_mode(new_mode);
exit(ret == OK ? 0 : 1);
}
if (!strcmp(verb, "test"))
test();
if (!strcmp(verb, "fake"))
fake(argc - 1, argv + 1);
if (!strcmp(verb, "sensor_reset")) {
if (argc > 2) {
int reset_time = strtol(argv[2], 0, 0);
sensor_reset(reset_time);
} else {
sensor_reset(0);
warnx("resettet default time");
}
exit(0);
}
fprintf(stderr, "FMU: unrecognised command %s, try:\n", verb);
#if defined(CONFIG_ARCH_BOARD_PX4FMU_V1)
fprintf(stderr, " mode_gpio, mode_serial, mode_pwm, mode_gpio_serial, mode_pwm_serial, mode_pwm_gpio, test\n");
#elif defined(CONFIG_ARCH_BOARD_PX4FMU_V2)
fprintf(stderr, " mode_gpio, mode_pwm, test, sensor_reset [milliseconds]\n");
#endif
exit(1);
}
在这里如果你不去过多地计较细节，你需要关注的其实就两条语句：
if (fmu_start() != OK)
new_mode = PORT_FULL_PWM;
int ret = fmu_new_mode(new_mode);
所以跟 mpu6050命令不一样，这里不需要在命令中使用 start。
int fmu_start(void)
{
int ret = OK;
if (g_fmu == nullptr) {
g_fmu = new PX4FMU;
if (g_fmu == nullptr) {
ret = -ENOMEM;
} else {
ret = g_fmu->init();
if (ret != OK) {
delete g_fmu;
g_fmu = nullptr;
}
}
}
return ret;
}
除了 new没有参数，跟 mpu6050过程是一样的。都是先 new一个对象，然后调用 init函数。
new一个对象，那就必然牵扯到构造函数了。
PX4FMU::PX4FMU() :
CDev("fmuservo", PX4FMU_DEVICE_PATH),
_mode(MODE_NONE),
_pwm_default_rate(50),
_pwm_alt_rate(50),
_pwm_alt_rate_channels(0),
_current_update_rate(0),
_task(-1),
_t_actuators(-1),
_t_actuator_armed(-1),
_t_outputs(0),
_num_outputs(0),
_primary_pwm_device(false),
_task_should_exit(false),
_armed(false),
_pwm_on(false),
_mixers(nullptr),
_failsafe_pwm({0}),
_disarmed_pwm({0}),
_num_failsafe_set(0),
_num_disarmed_set(0)
{
for (unsigned i = 0; i < _max_actuators; i++) {
_min_pwm[i] = PWM_DEFAULT_MIN;
_max_pwm[i] = PWM_DEFAULT_MAX;
}
_debug_enabled = true;
}
构造函数其实都是做一些初始化工作， CDev就是刚才我们匹配到的，其参数 "PX4FMU_DEVICE_PATH"就是后面我们要要创建的设备文件。其它的比较能引起我们注意的可能也就是 pwm了。如果你到pixhawk( https://pixhawk.org/modules/pixhawk)网站上你就会看到：
你会看到 pixhawk有八路主输出通道和六路辅助输出通道。根据目前掌握的资料，主输出通道控制电机，辅助输出通道控制云台等设备。
int PX4FMU::init()
{
int ret;
ASSERT(_task == -1);
/* do regular cdev init */
ret = CDev::init();
if (ret != OK)
return ret;
/* try to claim the generic PWM output device node as well - it's OK if we fail at this */
ret = register_driver(PWM_OUTPUT_DEVICE_PATH, &fops, 0666, (void *)this);
if (ret == OK) {
log("default PWM output device");
_primary_pwm_device = true;
}
/* reset GPIOs */
gpio_reset();
/* start the IO interface task */
_task = task_spawn_cmd("fmuservo",
SCHED_DEFAULT,
SCHED_PRIORITY_DEFAULT,
2048,
(main_t)&PX4FMU::task_main_trampoline,
nullptr);
if (_task < 0) {
debug("task start failed: %d", errno);
return -errno;
}
return OK;
}
=我们知道，在 CDev::init中会去创建我们的设备文件，但这里其实还创建了另外一个设备文件： " PWM_OUTPUT_DEVICE_PATH"即 "/dev/pwm_output"。二者的操作接口都是：fops。这里其实又牵扯出了另外一个东西： RCOutput.cpp，这个我们后面在讲。
而 gpio_reset其实就是对 IO初始化，这个不复杂：
void PX4FMU::gpio_reset(void)
{
/*
* Setup default GPIO config - all pins as GPIOs, input if
* possible otherwise output if possible.
*/
for (unsigned i = 0; i < _ngpio; i++) {
if (_gpio_tab[i].input != 0) {
stm32_configgpio(_gpio_tab[i].input);
} else if (_gpio_tab[i].output != 0) {
stm32_configgpio(_gpio_tab[i].output);
}
}
#if defined(CONFIG_ARCH_BOARD_PX4FMU_V1)
/* if we have a GPIO direction control, set it to zero (input) */
stm32_gpiowrite(GPIO_GPIO_DIR, 0);
stm32_configgpio(GPIO_GPIO_DIR);
#endif
}
const PX4FMU::GPIOConfig PX4FMU::_gpio_tab[] = {
#if defined(CONFIG_ARCH_BOARD_PX4FMU_V2)
{GPIO_GPIO0_INPUT, GPIO_GPIO0_OUTPUT, 0},
{GPIO_GPIO1_INPUT, GPIO_GPIO1_OUTPUT, 0},
{GPIO_GPIO2_INPUT, GPIO_GPIO2_OUTPUT, 0},
{GPIO_GPIO3_INPUT, GPIO_GPIO3_OUTPUT, 0},
{GPIO_GPIO4_INPUT, GPIO_GPIO4_OUTPUT, 0},
{GPIO_GPIO5_INPUT, GPIO_GPIO5_OUTPUT, 0},
{0, GPIO_VDD_5V_PERIPH_EN, 0},
{0, GPIO_VDD_3V3_SENSORS_EN, 0},
{GPIO_VDD_BRICK_VALID, 0, 0},
{GPIO_VDD_SERVO_VALID, 0, 0},
{GPIO_VDD_5V_HIPOWER_OC, 0, 0},
{GPIO_VDD_5V_PERIPH_OC, 0, 0},
#endif
};
/* User GPIOs
*
* GPIO0-5 are the PWM servo outputs.
*/
#define GPIO_GPIO0_INPUT (GPIO_INPUT|GPIO_PULLUP|GPIO_PORTE|GPIO_PIN14)
#define GPIO_GPIO1_INPUT (GPIO_INPUT|GPIO_PULLUP|GPIO_PORTE|GPIO_PIN13)
#define GPIO_GPIO2_INPUT (GPIO_INPUT|GPIO_PULLUP|GPIO_PORTE|GPIO_PIN11)
#define GPIO_GPIO3_INPUT (GPIO_INPUT|GPIO_PULLUP|GPIO_PORTE|GPIO_PIN9)
#define GPIO_GPIO4_INPUT (GPIO_INPUT|GPIO_PULLUP|GPIO_PORTD|GPIO_PIN13)
#define GPIO_GPIO5_INPUT (GPIO_INPUT|GPIO_PULLUP|GPIO_PORTD|GPIO_PIN14)
#define GPIO_GPIO0_OUTPUT (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTE|GPIO_PIN14)
#define GPIO_GPIO1_OUTPUT (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTE|GPIO_PIN13)
#define GPIO_GPIO2_OUTPUT (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTE|GPIO_PIN11)
#define GPIO_GPIO3_OUTPUT (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTE|GPIO_PIN9)
#define GPIO_GPIO4_OUTPUT (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTD|GPIO_PIN13)
#define GPIO_GPIO5_OUTPUT (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTD|GPIO_PIN14)
可以看到，这里的 IO配置跟我们平时写单片机程序不太一样，是通过 stm32_configgpio函数来完成的，这是由 Nuttx提供的接口，在PX4NuttX/nuttx/arch/arm/src/stm32/stm32_gpio.c源文件中。从代码来看，这些 PWM通道即可以作为输入，又可以作为输出，具体原因还不清楚。这个就后面在详细分析了。
在 gpio_reset之后我们看到有创建了一个任务，入口函数为： PX4FMU::task_main_trampoline。该函数只有一条语句： "g_fmu->task_main();"。严格地讲，这里是创建了一个线程。那么，这个线程是用来干嘛的呢？如果你去看这部分源码，你会发现里边用到了一个叫做 "ORB"的这么一个东西，目前没有从相关资料中了解到 "ORB"是什么。而且它操作的是 "/obj"目录下的设备文件：
nsh> ls -l /obj
/obj:
crw-rw-rw- 0 _obj_
crw-rw-rw- 0 actuator_armed
crw-rw-rw- 0 actuator_controls_0
crw-rw-rw- 0 actuator_controls_1
crw-rw-rw- 0 actuator_controls_2
crw-rw-rw- 0 actuator_controls_3
crw-rw-rw- 0 actuator_direct
crw-rw-rw- 0 actuator_outputs_0
crw-rw-rw- 0 battery_status
crw-rw-rw- 0 input_rc
crw-rw-rw- 0 parameter_update
crw-rw-rw- 0 safety
crw-rw-rw- 0 sensor_accel0
crw-rw-rw- 0 sensor_accel1
crw-rw-rw- 0 sensor_baro0
crw-rw-rw- 0 sensor_gyro0
crw-rw-rw- 0 sensor_gyro1
crw-rw-rw- 0 sensor_mag0
crw-rw-rw- 0 servorail_status
crw-rw-rw- 0 system_power
crw-rw-rw- 0 vehicle_command
crw-rw-rw- 0 vehicle_control_mode
nsh>
目前我还没有搞清楚这些设备文件跟 "/dev"目录下的设备文件有什么区别，但是读传感器数据显然用的不是这些设备，那么它们存在的意义又是什么？你觉得 APM团队会搞一个没有用的东西放在这里吗？我觉得不会。
看来选择分析 BoardLED真的是策略上的失败。那么接下来分析 px4io估计也不会好受，这个至少先做好心里准备。当然我相信，随着对 PX4理解的深入，这些所谓的问题肯定都将不再是问题。
PX4 FMU [10] px4io
1. 概述
我们当然还是要先来看下关于 px4io的脚本：
set HAVE_PX4IO false
if px4io start norc
then
set HAVE_PX4IO true
else
echo Loading /etc/px4io/px4io.bin
tone_alarm MBABGP
if px4io update /etc/px4io/px4io.bin
then
echo "upgraded PX4IO firmware OK"
tone_alarm MSPAA
else
echo "Failed to upgrade PX4IO firmware"
tone_alarm MNGGG
fi
sleep 1
if px4io start norc
then
set HAVE_PX4IO true
tone_alarm start
fi
fi
if [ $HAVE_PX4IO == true ]
then
echo "PX4IO board OK"
if px4io checkcrc /etc/px4io/px4io.bin
then
echo "PX4IO CRC OK"
else
echo "PX4IO CRC failure"
echo "PX4IO CRC failure" >> $logfile
tone_alarm MBABGP
if px4io forceupdate 14662 /etc/px4io/px4io.bin
then
sleep 1
if px4io start norc
then
echo "PX4IO restart OK"
echo "PX4IO restart OK" >> $logfile
tone_alarm MSPAA
else
echo "PX4IO restart failed"
echo "PX4IO restart failed" >> $logfile
tone_alarm MNGGG
sh /etc/init.d/rc.error
fi
else
echo "PX4IO update failed"
echo "PX4IO update failed" >> $logfile
tone_alarm MNGGG
fi
fi
else
echo "No PX4IO board found"
echo "No PX4IO board found" >> $logfile
if [ $BOARD == FMUv2 ]
then
sh /etc/init.d/rc.error
fi
fi
if [ $BOARD == FMUv1 -a $deviceD == /dev/ttyS1 ]
then
echo "Setting FMU mode_serial"
fmu mode_serial
else
echo "Setting FMU mode_pwm"
fmu mode_pwm
fi
从脚本中我们看到第一个命令行参数是 start，而 update只有在第一次的时候才会调用，因为前面我们已经看到更新 IO板的固件是需要匹配 CRC32的，而在后面的 if中才有 CRC校验。所以后面的 forceupdate也是用来更新 IO板固件的。
但是我们从脚本中看到，先调用的是 px4io，然后才是 fmu命令，不知到在功能上他们是否存在着什么联系。
2. start
因为这是一个 nsh命令，所以我们第一个想到的必然是 px4io_main函数。
radiolink@ubuntu:~/apm$ grep -nr px4io_main ./PX4Firmware/src/drivers/
./PX4Firmware/src/drivers/px4io/px4io.cpp:2685:extern "C" __EXPORTint px4io_main(int argc, char *argv[]);
./PX4Firmware/src/drivers/px4io/px4io.cpp:3108:px4io_main(int argc,char *argv[])
radiolink@ubuntu:~/apm$ ls ./PX4Firmware/src/drivers/px4io/
module.mk px4io.cpp px4io_i2c.cpp px4io_serial.cpp px4io_uploader.cpp uploader.h
radiolink@ubuntu:~/apm$
当时选择第一个分析 fmu也就是看到 fmu源文件少，感觉上应该会相对简单点吧，但是谁知这亿出头砸下去才发现下边是块大石头。从匹配结果来看，光是源文件 px4io.cpp就三千多行代码，所以也不会简单。
int px4io_main(int argc, char *argv[])
{
/* check for sufficient number of arguments */
if (argc < 2)
goto out;
if (!strcmp(argv[1], "start"))
start(argc - 1, argv + 1);
void start(int argc, char *argv[])
{
if (g_dev != nullptr)
errx(0, "already loaded");
/* allocate the interface */
device::Device *interface = get_interface();
/* create the driver - it will set g_dev */
(void)new PX4IO(interface);
if (g_dev == nullptr) {
delete interface;
errx(1, "driver alloc failed");
}
if (OK != g_dev->init()) {
delete g_dev;
g_dev = nullptr;
errx(1, "driver init failed");
}
/* disable RC handling on request */
if (argc > 1) {
if (!strcmp(argv[1], "norc")) {
if (g_dev->disable_rc_handling())
warnx("Failed disabling RC handling");
} else {
warnx("unknown argument: %s", argv[1]);
}
}
#ifdef CONFIG_ARCH_BOARD_PX4FMU_V1
int dsm_vcc_ctl;
if (param_get(param_find("RC_RL1_DSM_VCC"), &dsm_vcc_ctl) == OK) {
if (dsm_vcc_ctl) {
g_dev->set_dsm_vcc_ctl(true);
g_dev->ioctl(nullptr, DSM_BIND_POWER_UP, 0);
}
}
#endif
exit(0);
}
这段代码还是比较好理解的，先是获取了一个 " interface"，然后是 new了一个 " driver"，最后就是调用 init函数了。这里其实涉及到了 fmu跟 io的通讯，我们可以在 get_interface中看到。
device::Device *get_interface()
{
device::Device *interface = nullptr;
#ifndef CONFIG_ARCH_BOARD_PX4FMU_V1
/* try for a serial interface */
if (PX4IO_serial_interface != nullptr)
interface = PX4IO_serial_interface();
if (interface != nullptr)
goto got;
#endif
/* try for an I2C interface if we haven't got a serial one */
if (PX4IO_i2c_interface != nullptr)
interface = PX4IO_i2c_interface();
if (interface != nullptr)
goto got;
errx(1, "cannot alloc interface");
got:
if (interface->init() != OK) {
delete interface;
errx(1, "interface init failed");
}
return interface;
}
阅读这段代码，我们很容易一不小心就以为 fmu跟 io通讯用的是 I2C接口。但是请注意，这里用的是 "# ifndef"。所以下面就是 px4io_serial.cpp源文件中的内容了。
device::Device *PX4IO_serial_interface()
{
return new PX4IO_serial();
}
class PX4IO_serial : public device::Device
{
public:
PX4IO_serial();
virtual ~PX4IO_serial();
PX4IO_serial::PX4IO_serial() :
Device("PX4IO_serial"),
_tx_dma(nullptr),
_rx_dma(nullptr),
_rx_dma_status(_dma_status_inactive),
_pc_txns(perf_alloc(PC_ELAPSED, "io_txns ")),
_pc_dmasetup(perf_alloc(PC_ELAPSED, "io_dmasetup ")),
_pc_retries(perf_alloc(PC_COUNT, "io_retries ")),
_pc_timeouts(perf_alloc(PC_COUNT, "io_timeouts ")),
_pc_crcerrs(perf_alloc(PC_COUNT, "io_crcerrs ")),
_pc_dmaerrs(perf_alloc(PC_COUNT, "io_dmaerrs ")),
_pc_protoerrs(perf_alloc(PC_COUNT, "io_protoerrs")),
_pc_uerrs(perf_alloc(PC_COUNT, "io_uarterrs ")),
_pc_idle(perf_alloc(PC_COUNT, "io_idle ")),
_pc_badidle(perf_alloc(PC_COUNT, "io_badidle "))
{
g_interface = this;
}
所以 PX4IO_serial_interface只是一个中间函数，它只是用来 new一个 PX4IO_serial对象。而 PX4IO_serial是直接继承自 device::Device，为什么？因为串口跟 SPI一样都是最底层的硬件接口，除非使用 SPI虚拟一个串口，否则不可能出现串口继承自 SPI的情况。
在 get_interface函数中我们看到 PX4IO_serial也有自己的 init函数，那么它又做了哪些事情呢？我们现在就来看下。
int PX4IO_serial::init()
{
/* allocate DMA */
_tx_dma = stm32_dmachannel(PX4IO_SERIAL_TX_DMAMAP);
_rx_dma = stm32_dmachannel(PX4IO_SERIAL_RX_DMAMAP);
if ((_tx_dma == nullptr) || (_rx_dma == nullptr))
return -1;
/* configure pins for serial use */
stm32_configgpio(PX4IO_SERIAL_TX_GPIO);
stm32_configgpio(PX4IO_SERIAL_RX_GPIO);
/* reset & configure the UART */
rCR1 = 0;
rCR2 = 0;
rCR3 = 0;
/* eat any existing interrupt status */
(void)rSR;
(void)rDR;
/* configure line speed */
uint32_t usartdiv32 = PX4IO_SERIAL_CLOCK / (PX4IO_SERIAL_BITRATE / 2);
uint32_t mantissa = usartdiv32 >> 5;
uint32_t fraction = (usartdiv32 - (mantissa << 5) + 1) >> 1;
rBRR = (mantissa << USART_BRR_MANT_SHIFT) | (fraction << USART_BRR_FRAC_SHIFT);
/* attach serial interrupt handler */
irq_attach(PX4IO_SERIAL_VECTOR, _interrupt);
up_enable_irq(PX4IO_SERIAL_VECTOR);
/* enable UART in DMA mode, enable error and line idle interrupts */
rCR3 = USART_CR3_EIE;
rCR1 = USART_CR1_RE | USART_CR1_TE | USART_CR1_UE | USART_CR1_IDLEIE;
/* create semaphores */
sem_init(&_completion_semaphore, 0, 0);
sem_init(&_bus_semaphore, 0, 1);
/* XXX this could try talking to IO */
return 0;
}
可能这个时候我们会觉得很奇怪，怎么直接就操作硬件了呢？都没有创建设备文件。在 Linux中虽说也可以在用户空间直接写一个驱动，但像这么干的我毕竟还没有见到。我只能说 Nuttx确实是不分内核空间跟用户空间。从这里我们很清楚地知道 fmu跟 io通讯使用的是串口。
radiolink@ubuntu:~/apm$ grep -nr PX4IO_SERIAL_TX_GPIO ./
./PX4Firmware/src/drivers/boards/px4fmu-v2/board_config.h:65:#define PX4IO_SERIAL_TX_GPIO GPIO_USART6_TX
./PX4Firmware/src/drivers/px4io/px4io_serial.cpp:217: stm32_unconfiggpio(PX4IO_SERIAL_TX_GPIO);
./PX4Firmware/src/drivers/px4io/px4io_serial.cpp:249: stm32_configgpio(PX4IO_SERIAL_TX_GPIO);
radiolink@ubuntu:~/apm$
从匹配结果我们知道使用的是 USAER6。知道这个信息有什么用呢？用处可大了，有了这个信息我们就可以到 PCB上去查看 fmu跟 io的硬件连接，从而熟悉硬件。毕竟，软件终究是在硬件上运行的。
既然要查看硬件连接，那么我们要做的第一件事就是找出 USART6到底是多少号管脚这个就需要到 ST官网去下载相应的数据手册。
这是从 STM32F427手册中找到的，我们用的是 LQFP100的封装，所以管脚号为 63、64。知道了这个信息就方便我们去查看原理图了。
所以我们看到 64pin连接了 STM32F103的 12pin。而 63pin连接的是 13pin。关于这些寄存器配置，那就需要去对照数据手册。
在 interface之后我们看到是 new过程。那我们当然很自然地会去想 PX4IO又是什么，干嘛的呢？我们继续往下看：
/**
* The PX4IO class.
*
* Encapsulates PX4FMU to PX4IO communications modeled as file operations.
*/
class PX4IO : public device::CDev
{
public:
/**
* Constructor.
*
* Initialize all class variables.
*/
PX4IO(device::Device *interface);
namespace
{
PX4IO *g_dev = nullptr;
}
PX4IO::PX4IO(device::Device *interface) :
CDev("px4io", PX4IO_DEVICE_PATH),
_interface(interface),
_hardware(0),
_max_actuators(0),
_max_controls(0),
_max_rc_input(0),
_max_relays(0),
_max_transfer(16), /* sensible default */
_update_interval(0),
_rc_handling_disabled(RC_HANDLING_DEFAULT),
_rc_chan_count(0),
_rc_last_valid(0),
_task(-1),
_task_should_exit(false),
_mavlink_fd(-1),
_perf_update(perf_alloc(PC_ELAPSED, "io update")),
_perf_write(perf_alloc(PC_ELAPSED, "io write")),
_perf_chan_count(perf_alloc(PC_COUNT, "io rc #")),
_status(0),
_alarms(0),
_t_actuator_controls_0(-1),
_t_actuator_controls_1(-1),
_t_actuator_controls_2(-1),
_t_actuator_controls_3(-1),
_t_actuator_armed(-1),
_t_vehicle_control_mode(-1),
_t_param(-1),
_t_vehicle_command(-1),
_to_input_rc(0),
_to_outputs(0),
_to_battery(0),
_to_servorail(0),
_to_safety(0),
_primary_pwm_device(false),
_lockdown_override(false),
_battery_amp_per_volt(90.0f / 5.0f), // this matches the 3DR current sensor
_battery_amp_bias(0),
_battery_mamphour_total(0),
_battery_last_timestamp(0)
#ifdef CONFIG_ARCH_BOARD_PX4FMU_V1
, _dsm_vcc_ctl(false)
#endif
{
/* we need this potentially before it could be set in task_main */
g_dev = this;
_debug_enabled = false;
_servorail_status.rssi_v = 0;
}
可以看到，它继承自 device::CDev，也就是说它会去创建设备文件： PX4IO_DEVICE_PATH即 "/dev/px4io"。当然，我们现在不去讨论究竟谁在使用该设备。该构造函数同时也告诉我们 start后面通过 g_dev调用的 init函数其实是 PX4IO::init函数。但是 init函数内容比较多，所以我们还是先看 g_dev->disable_rc_handling。
int PX4IO::disable_rc_handling()
{
return io_disable_rc_handling();
}
int PX4IO::io_disable_rc_handling()
{
uint16_t set = PX4IO_P_SETUP_ARMING_RC_HANDLING_DISABLED;
uint16_t clear = 0;
return io_reg_modify(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_ARMING, clear, set);
}
int PX4IO::io_reg_modify(uint8_t page, uint8_t offset, uint16_t clearbits, uint16_t setbits)
{
int ret;
uint16_t value;
ret = io_reg_get(page, offset, &value, 1);
if (ret != OK)
return ret;
value &= ~clearbits;
value |= setbits;
return io_reg_set(page, offset, value);
}
从代码理解我们会认为这里修改了寄存器的值，但请注意，这里实际上修改的是 io板的值，根据这些参数你找不到对应的寄存器。所以我觉得更确切地理解是这里修改的是 io的参数。而在io_reg函数中就是通过 interface跟 io进行通讯。目前也只能通过注释知道这里配置的是 "arming controls"，或者根据 start中的注释 "/* disable RC handling on request */"认为这里是给飞控加锁。详细了解还得去看 io源码。其实你如果去看 io源码，你会发现这里操作的其实是一些全局变量的。
int
PX4IO::init()
{
int ret;
ASSERT(_task == -1);
/* do regular cdev init */
ret = CDev::init();
if (ret != OK)
return ret;
/* get some parameters */
unsigned protocol = io_reg_get(PX4IO_PAGE_CONFIG, PX4IO_P_CONFIG_PROTOCOL_VERSION);
if (protocol == _io_reg_get_error) {
log("failed to communicate with IO");
mavlink_log_emergency(_mavlink_fd, "[IO] failed to communicate with IO, abort.");
return -1;
}
if (protocol != PX4IO_PROTOCOL_VERSION) {
log("protocol/firmware mismatch");
mavlink_log_emergency(_mavlink_fd, "[IO] protocol/firmware mismatch, abort.");
return -1;
}
_hardware = io_reg_get(PX4IO_PAGE_CONFIG, PX4IO_P_CONFIG_HARDWARE_VERSION);
_max_actuators = io_reg_get(PX4IO_PAGE_CONFIG, PX4IO_P_CONFIG_ACTUATOR_COUNT);
_max_controls = io_reg_get(PX4IO_PAGE_CONFIG, PX4IO_P_CONFIG_CONTROL_COUNT);
_max_relays = io_reg_get(PX4IO_PAGE_CONFIG, PX4IO_P_CONFIG_RELAY_COUNT);
_max_transfer = io_reg_get(PX4IO_PAGE_CONFIG, PX4IO_P_CONFIG_MAX_TRANSFER) - 2;
_max_rc_input = io_reg_get(PX4IO_PAGE_CONFIG, PX4IO_P_CONFIG_RC_INPUT_COUNT);
if ((_max_actuators < 1) || (_max_actuators > 255) ||
(_max_relays > 32) ||
(_max_transfer < 16) || (_max_transfer > 255) ||
(_max_rc_input < 1) || (_max_rc_input > 255)) {
log("config read error");
mavlink_log_emergency(_mavlink_fd, "[IO] config read fail, abort.");
return -1;
}
if (_max_rc_input > RC_INPUT_MAX_CHANNELS)
_max_rc_input = RC_INPUT_MAX_CHANNELS;
/*
* Check for IO flight state - if FMU was flagged to be in
* armed state, FMU is recovering from an in-air reset.
* Read back status and request the commander to arm
* in this case.
*/
uint16_t reg;
/* get IO's last seen FMU state */
ret = io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_ARMING, ®, sizeof(reg));
if (ret != OK)
return ret;
/*
* in-air restart is only tried if the IO board reports it is
* already armed, and has been configured for in-air restart
*/
if ((reg & PX4IO_P_SETUP_ARMING_INAIR_RESTART_OK) &&
(reg & PX4IO_P_SETUP_ARMING_FMU_ARMED)) {
} else {
/* dis-arm IO before touching anything */
io_reg_modify(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_ARMING,
PX4IO_P_SETUP_ARMING_FMU_ARMED |
PX4IO_P_SETUP_ARMING_INAIR_RESTART_OK |
PX4IO_P_SETUP_ARMING_MANUAL_OVERRIDE_OK |
PX4IO_P_SETUP_ARMING_ALWAYS_PWM_ENABLE, 0);
if (_rc_handling_disabled) {
ret = io_disable_rc_handling();
} else {
/* publish RC config to IO */
ret = io_set_rc_config();
if (ret != OK) {
log("failed to update RC input config");
mavlink_log_info(_mavlink_fd, "[IO] RC config upload fail");
return ret;
}
}
}
这是 init中的代码，其实都是在跟 io进行交互。但由于我们目前还不清楚两者交互的内容，所以很容易看晕掉。但实际上，前半部分是读，后半部分是写。当然，上面并不是 init源码的全部，还有下面部分。
/* try to claim the generic PWM output device node as well - it's OK if we fail at this */
ret = register_driver(PWM_OUTPUT_DEVICE_PATH, &fops, 0666, (void *)this);
if (ret == OK) {
log("default PWM output device");
_primary_pwm_device = true;
}
/* start the IO interface task */
_task = task_create("px4io", SCHED_PRIORITY_ACTUATOR_OUTPUTS, 2048, (main_t)&PX4IO::task_main_trampoline, nullptr);
if (_task < 0) {
debug("task start failed: %d", errno);
return -errno;
}
mavlink_log_info(_mavlink_fd, "[IO] init ok");
return OK;
}
其实你要是去看 fmu部分，你会发现 fmu也注册了一个 PWM_OUTPUT_DEVICE_PATH设备。但是前面我们在脚本中看到了， px4io的启动在 fmu之前，所以 fmu中奖创建失败。也就是说应用程序操作该设备调用的是 px4io中的接口。然后就是它创建了一个任务，其主函数是： "PX4IO::task_main_trampoline"。在这个任务中，除了跟 io进行通讯之外同样用到了 ORB，这个我们暂时就不看了。
3. update
根据推测， update只会被调用一次，而且是在 io没有固件的情况下才会被调用。其实这也说得过去，在 io没有固件的时候， start中的所有交互都将失败，命令行检测到对应的返回值就使用 update更新固件。其命令行为： "px4io update /etc/px4io/px4io.bin"。源码如下：
if (!strcmp(argv[1], "update")) {
if (g_dev != nullptr) {
printf("[px4io] loaded, detaching first\n");
/* stop the driver */
delete g_dev;
g_dev = nullptr;
}
PX4IO_Uploader *up;
const char *fn[4];
/* work out what we're uploading... */
if (argc > 2) {
fn[0] = argv[2];
fn[1] = nullptr;
} else {
#if defined(CONFIG_ARCH_BOARD_PX4FMU_V1)
fn[0] = "/etc/extras/px4io-v1_default.bin";
fn[1] = "/fs/microsd/px4io1.bin";
fn[2] = "/fs/microsd/px4io.bin";
fn[3] = nullptr;
#elif defined(CONFIG_ARCH_BOARD_PX4FMU_V2)
fn[0] = "/etc/extras/px4io-v2_default.bin";
fn[1] = "/fs/microsd/px4io2.bin";
fn[2] = "/fs/microsd/px4io.bin";
fn[3] = nullptr;
#else
#error "unknown board"
#endif
}
up = new PX4IO_Uploader;
int ret = up->upload(&fn[0]);
delete up;
switch (ret) {
case OK:
break;
case -ENOENT:
errx(1, "PX4IO firmware file not found");
case -EEXIST:
case -EIO:
errx(1, "error updating PX4IO - check that bootloader mode is enabled");
case -EINVAL:
errx(1, "verify failed - retry the update");
case -ETIMEDOUT:
errx(1, "timed out waiting for bootloader - power-cycle and try again");
default:
errx(1, "unexpected error %d", ret);
}
return ret;
}
从源码中我们看到，当我们不带 bin文件路径参数时，会有三个候补文件，在跟文件系统中，我们也是可以看到这些文件的。但这些都不是最重要的，重要的是 PX4IO_Uploader这个结构。一看就知道它是用来更新固件的。
class PX4IO_Uploader
{
public:
PX4IO_Uploader();
virtual ~PX4IO_Uploader();
int upload(const char *filenames[]);
PX4IO_Uploader::PX4IO_Uploader() :
_io_fd(-1),
_fw_fd(-1)
{
}
PX4IO_Uploader::~PX4IO_Uploader()
{
}
int
PX4IO_Uploader::upload(const char *filenames[])
{
int ret;
const char *filename = NULL;
size_t fw_size;
#ifndef PX4IO_SERIAL_DEVICE
#error Must define PX4IO_SERIAL_DEVICE in board configuration to support firmware upload
#endif
/* allow an early abort and look for file first */
for (unsigned i = 0; filenames[i] != nullptr; i++) {
_fw_fd = open(filenames[i], O_RDONLY);
if (_fw_fd < 0) {
log("failed to open %s", filenames[i]);
continue;
}
log("using firmware from %s", filenames[i]);
filename = filenames[i];
break;
}
if (filename == NULL) {
log("no firmware found");
close(_io_fd);
_io_fd = -1;
return -ENOENT;
}
从这里其实我们也看到，它回去遍历整个数组，我们在使用的时候当然不必关心这些细节。
_io_fd = open(PX4IO_SERIAL_DEVICE, O_RDWR);
if (_io_fd < 0) {
log("could not open interface");
return -errno;
}
/* save initial uart configuration to reset after the update */
struct termios t_original;
tcgetattr(_io_fd, &t_original);
/* adjust line speed to match bootloader */
struct termios t;
tcgetattr(_io_fd, &t);
cfsetspeed(&t, 115200);
tcsetattr(_io_fd, TCSANOW, &t);
/* look for the bootloader for 150 ms */
for (int i = 0; i < 15; i++) {
ret = sync();
if (ret == OK) {
break;
} else {
usleep(10000);
}
}
if (ret != OK) {
/* this is immediately fatal */
log("bootloader not responding");
tcsetattr(_io_fd, TCSANOW, &t_original);
close(_io_fd);
_io_fd = -1;
return -EIO;
}
struct stat st;
if (stat(filename, &st) != 0) {
log("Failed to stat %s - %d\n", filename, (int)errno);
tcsetattr(_io_fd, TCSANOW, &t_original);
close(_io_fd);
_io_fd = -1;
return -errno;
}
fw_size = st.st_size;
if (_fw_fd == -1) {
tcsetattr(_io_fd, TCSANOW, &t_original);
close(_io_fd);
_io_fd = -1;
return -ENOENT;
}
#define PX4IO_SERIAL_DEVICE "/dev/ttyS4"
这里我们很容易想到， PX4IO_SERIAL_DEVICE其实就是与 io通信的串口，只是，我们似乎没有看到在哪个地方创建了这个设备。
radiolink@ubuntu:~/apm$ grep -nr PX4IO_SERIAL_DEVICE ./
./PX4Firmware/src/drivers/boards/px4fmu-v2/board_config.h:64:#define PX4IO_SERIAL_DEVICE "/dev/ttyS4"
./PX4Firmware/src/drivers/boards/px4fmu-v1/board_config.h:62:#define PX4IO_SERIAL_DEVICE "/dev/ttyS2"
./PX4Firmware/src/drivers/px4io/px4io_uploader.cpp:84:#ifndef PX4IO_SERIAL_DEVICE
./PX4Firmware/src/drivers/px4io/px4io_uploader.cpp:85:#error Must define PX4IO_SERIAL_DEVICE in board configuration to support firmware upload
./PX4Firmware/src/drivers/px4io/px4io_uploader.cpp:109: _io_fd = open(PX4IO_SERIAL_DEVICE, O_RDWR);
radiolink@ubuntu:~/apm$
这个结果或许会让我们觉得很沮丧，毕竟我们不相信会凭空产生一个设备文件。但其实如果我们换一种方式去思考，你就会发现：
radiolink@ubuntu:~/apm$ grep -nr /dev/ttyS4 ./PX4NuttX/
./PX4NuttX/nuttx/arch/arm/src/kl/kl_serial.c:893: (void)uart_register("/dev/ttyS4", &TTYS4_DEV);
./PX4NuttX/nuttx/arch/arm/src/lm/lm_serial.c:1283: (void)uart_register("/dev/ttyS4", &TTYS4_DEV);
./PX4NuttX/nuttx/arch/arm/src/kinetis/kinetis_serial.c:1284: (void)uart_register("/dev/ttyS4", &TTYS4_DEV);
./PX4NuttX/nuttx/arch/arm/src/sam34/sam_serial.c:1217: (void)uart_register("/dev/ttyS4", &TTYS4_DEV);
radiolink@ubuntu:~/apm$
其实还是有人去创建了它的。只不过问题的我们用的不是 STM232吗，这里似乎哪个都不是我们要的结果。但我要说的是，这里至少给我吗提供了一个方向，即 uart_register函数。于是：
radiolink@ubuntu:~/apm$ grep -nr uart_register ./PX4NuttX/ | grep stm32
./PX4NuttX/nuttx/arch/arm/src/stm32/stm32_serial.c:2464: (void)uart_register("/dev/console", &uart_devs[CONSOLE_UART - 1]->dev);
./PX4NuttX/nuttx/arch/arm/src/stm32/stm32_serial.c:2469: (void)uart_register("/dev/ttyS0", &uart_devs[CONSOLE_UART - 1]->dev);
./PX4NuttX/nuttx/arch/arm/src/stm32/stm32_serial.c:2505: (void)uart_register(devname, &uart_devs[i]->dev);
Binary file ./PX4NuttX/nuttx/arch/arm/src/stm32_serial.o matches
radiolink@ubuntu:~/apm$
这不结果就出来了吗。由于我们的设备文件是 "/dev/ttyS4"，所以肯定用的 "( void)uart_register(devname, &uart_devs[i]->dev);"进行注册的。玉树我们找到对应的代码：
/****************************************************************************
* Name: up_serialinit
*
* Description:
* Register serial console and serial ports. This assumes
* that up_earlyserialinit was called previously.
*
****************************************************************************/
void up_serialinit(void)
{
#ifdef HAVE_UART
char devname[16];
unsigned i;
unsigned minor = 0;
#ifdef CONFIG_PM
int ret;
#endif
/* Register to receive power management callbacks */
#ifdef CONFIG_PM
ret = pm_register(&g_serialcb);
DEBUGASSERT(ret == OK);
#endif
/* Register the console */
#if CONSOLE_UART > 0
(void)uart_register("/dev/console", &uart_devs[CONSOLE_UART - 1]->dev);
#ifndef CONFIG_SERIAL_DISABLE_REORDERING
/* If not disabled, register the console UART to ttyS0 and exclude
* it from initializing it further down
*/
(void)uart_register("/dev/ttyS0", &uart_devs[CONSOLE_UART - 1]->dev);
minor = 1;
#endif /* CONFIG_SERIAL_DISABLE_REORDERING not defined */
/* If we need to re-initialise the console to enable DMA do that here. */
# ifdef SERIAL_HAVE_CONSOLE_DMA
up_dma_setup(&uart_devs[CONSOLE_UART - 1]->dev);
# endif
#endif /* CONSOLE_UART > 0 */
/* Register all remaining USARTs */
strcpy(devname, "/dev/ttySx");
for (i = 0; i < STM32_NUSART; i++)
{
/* Don't create a device for non configured ports */
if (uart_devs[i] == 0)
{
continue;
}
#ifndef CONFIG_SERIAL_DISABLE_REORDERING
/* Don't create a device for the console - we did that above */
if (uart_devs[i]->dev.isconsole)
{
continue;
}
#endif
/* Register USARTs as devices in increasing order */
devname[9] = '0' + minor++;
(void)uart_register(devname, &uart_devs[i]->dev);
}
#endif /* HAVE UART */
}
这是完整的 up_serialinit函数。从最后一段代码我们知道，只要 STM32_NUSART的值不小于 4就必然创建 "/dev/ttyS4"设备。我当然相信肯定不会比 4更小，验证过程比较繁琐，这里就略过了。
而接下来在是我们的重点：更新固件。
/* do the usual program thing - allow for failure */
for (unsigned retries = 0; retries < 1; retries++) {
if (retries > 0) {
log("retrying update...");
ret = sync();
if (ret != OK) {
/* this is immediately fatal */
log("bootloader not responding");
tcsetattr(_io_fd, TCSANOW, &t_original);
close(_io_fd);
_io_fd = -1;
return -EIO;
}
}
ret = get_info(INFO_BL_REV, bl_rev);
if (ret == OK) {
if (bl_rev <= BL_REV) {
log("found bootloader revision: %d", bl_rev);
} else {
log("found unsupported bootloader revision %d, exiting", bl_rev);
tcsetattr(_io_fd, TCSANOW, &t_original);
close(_io_fd);
_io_fd = -1;
return OK;
}
}
ret = erase();
if (ret != OK) {
log("erase failed");
continue;
}
ret = program(fw_size);
if (ret != OK) {
log("program failed");
continue;
}
if (bl_rev <= 2)
ret = verify_rev2(fw_size);
else if(bl_rev == 3) {
ret = verify_rev3(fw_size);
}
if (ret != OK) {
log("verify failed");
continue;
}
ret = reboot();
if (ret != OK) {
log("reboot failed");
tcsetattr(_io_fd, TCSANOW, &t_original);
close(_io_fd);
_io_fd = -1;
return ret;
}
log("update complete");
ret = OK;
break;
}
/* reset uart to previous/default baudrate */
tcsetattr(_io_fd, TCSANOW, &t_original);
close(_fw_fd);
close(_io_fd);
_io_fd = -1;
// sleep for enough time for the IO chip to boot. This makes
// forceupdate more reliably startup IO again after update
up_udelay(100*1000);
return ret;
}
这段代码核心就两个函数： erase跟 program。前者用来擦除 Flash，后者用来更新固件。
int PX4IO_Uploader::erase()
{
log("erase...");
send(PROTO_CHIP_ERASE);
send(PROTO_EOC);
return get_sync(10000); /* allow 10s timeout */
}
int PX4IO_Uploader::program(size_t fw_size)
{
uint8_t file_buf[PROG_MULTI_MAX];
ssize_t count;
int ret;
size_t sent = 0;
log("programming %u bytes...", (unsigned)fw_size);
ret = lseek(_fw_fd, 0, SEEK_SET);
while (sent < fw_size) {
/* get more bytes to program */
size_t n = fw_size - sent;
if (n > sizeof(file_buf)) {
n = sizeof(file_buf);
}
count = read_with_retry(_fw_fd, file_buf, n);
if (count != (ssize_t)n) {
log("firmware read of %u bytes at %u failed -> %d errno %d",
(unsigned)n,
(unsigned)sent,
(int)count,
(int)errno);
}
if (count == 0)
return OK;
sent += count;
if (count < 0)
return -errno;
ASSERT((count % 4) == 0);
send(PROTO_PROG_MULTI);
send(count);
send(&file_buf[0], count);
send(PROTO_EOC);
ret = get_sync(1000);
if (ret != OK)
return ret;
}
return OK;
}
擦除其实就是发送一些擦除命令。更新固件也就是把固件的内容读取出来，然后通过串口发送给 io。而剩下的就是 io的任务了。
最后我们还看到有个 reboot，这是用来重启 io板的。重启之后由于已经有固件了，所以就会去启动 IO的固件。
关于 update基本上就这么多。
4. forceupdate
这是也是用来更新固件的，命令为： "px4io forceupdate 14662 /etc/px4io/px4io.bin"。其实在它前边还有一个命令，想必你已经想到了，对的，就是 CRC校验。命令为： "px4io checkcrc /etc/px4io/px4io.bin"，这个我们稍后再讨论。
if (!strcmp(argv[1], "forceupdate")) {
/*
PX4 FMU [11] RCOutput
1. init
我们现在要做的第一件事当然是确定设备 "PWM_OUTPUT_DEVICE_PATH" 是被谁使用的。
radiolink@ubuntu:~/apm$ grep -nr PWM_OUTPUT_DEVICE_PATH ./
./PX4Firmware/src/systemcmds/pwm/pwm.c:104: "\t-d
./PX4Firmware/src/systemcmds/pwm/pwm.c:112: const char *dev = PWM_OUTPUT_DEVICE_PATH;
./PX4Firmware/src/systemcmds/tests/test_servo.c:66: fd = open(PWM_OUTPUT_DEVICE_PATH, O_RDWR);
./PX4Firmware/src/systemcmds/tests/test_ppm_loopback.c:70: servo_fd = open(PWM_OUTPUT_DEVICE_PATH, O_RDWR);
./PX4Firmware/src/systemcmds/esc_calib/esc_calib.c:79: " [-d
./PX4Firmware/src/systemcmds/esc_calib/esc_calib.c:91: char *dev = PWM_OUTPUT_DEVICE_PATH;
./PX4Firmware/src/modules/uavcan/uavcan_main.cpp:108: // unregister_driver(PWM_OUTPUT_DEVICE_PATH);
./PX4Firmware/src/drivers/drv_pwm_output.h:60:#define PWM_OUTPUT_DEVICE_PATH "/dev/pwm_output"
./PX4Firmware/src/drivers/hil/hil.cpp:159: CDev("hilservo", PWM_OUTPUT_DEVICE_PATH/*"/dev/hil" XXXL*/),
./PX4Firmware/src/drivers/hil/hil.cpp:199: // unregister_driver(PWM_OUTPUT_DEVICE_PATH);
./PX4Firmware/src/drivers/hil/hil.cpp:219: //ret = register_driver(PWM_OUTPUT_DEVICE_PATH, &fops, 0666, (void *)this);
./PX4Firmware/src/drivers/hil/hil.cpp:751: fd = open(PWM_OUTPUT_DEVICE_PATH, 0);
./PX4Firmware/src/drivers/px4io/px4io.cpp:826: ret = register_driver(PWM_OUTPUT_DEVICE_PATH, &fops, 0666, (void *)this);
./PX4Firmware/src/drivers/px4io/px4io.cpp:1078: unregister_driver(PWM_OUTPUT_DEVICE_PATH);
./PX4Firmware/src/drivers/px4fmu/fmu.cpp:304: unregister_driver(PWM_OUTPUT_DEVICE_PATH);
./PX4Firmware/src/drivers/px4fmu/fmu.cpp:323: ret = register_driver(PWM_OUTPUT_DEVICE_PATH, &fops, 0666, (void *)this);
./ardupilot/libraries/AP_HAL_VRBRAIN/RCOutput.cpp:22: _pwm_fd = open(PWM_OUTPUT_DEVICE_PATH, O_RDWR);
./ardupilot/libraries/AP_HAL_VRBRAIN/RCOutput.cpp:24: hal.scheduler->panic("Unable to open " PWM_OUTPUT_DEVICE_PATH);
./ardupilot/libraries/AP_HAL_PX4/RCOutput.cpp:24: _pwm_fd = open(PWM_OUTPUT_DEVICE_PATH, O_RDWR);
./ardupilot/libraries/AP_HAL_PX4/RCOutput.cpp:26: hal.scheduler->panic("Unable to open " PWM_OUTPUT_DEVICE_PATH);
radiolink@ubuntu:~/apm$
看到这个结果可能会让人不知所措，不知如何下手。但其实我们并不需要都去分析。如果你挨个剔除，最后也就只剩下 ardupilot中的两个结果。但你仔细看你就会发现， AP_HAL_PX4才是我们要分析的。为什么？原因在于它们做的都是同样的事(猜就是)而我们用的就是 PX4。
不知你是否还记得 hal这个家伙： " const HAL_PX4 AP_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 */
{}
static PX4RCOutput rcoutDriver;
class PX4::PX4RCOutput : public AP_HAL::RCOutput
于是我们就知道， hal中的 rcout就是 RCOutput。
#define PX4_NUM_OUTPUT_CHANNELS 16
class PX4::PX4RCOutput : public AP_HAL::RCOutput
{
public:
void init(void* machtnichts);
void set_freq(uint32_t chmask, uint16_t freq_hz);
uint16_t get_freq(uint8_t ch);
void enable_ch(uint8_t ch);
void disable_ch(uint8_t ch);
void write(uint8_t ch, uint16_t period_us);
void write(uint8_t ch, uint16_t* period_us, uint8_t len);
uint16_t read(uint8_t ch);
void read(uint16_t* period_us, uint8_t len);
void set_safety_pwm(uint32_t chmask, uint16_t period_us);
void set_failsafe_pwm(uint32_t chmask, uint16_t period_us);
void force_safety_off(void);
void _timer_tick(void);
private:
int _pwm_fd;
int _alt_fd;
uint16_t _freq_hz;
uint16_t _period[PX4_NUM_OUTPUT_CHANNELS];
volatile uint8_t _max_channel;
volatile bool _need_update;
perf_counter_t _perf_rcout;
uint32_t _last_output;
unsigned _servo_count;
unsigned _alt_servo_count;
uint32_t _rate_mask;
uint16_t _enabled_channels;
void _init_alt_channels(void);
};
看到这个定义可能你的第一感觉也是"构造函数在哪？"其实这里用的是默认的构造函数。所以我们更应该关心的是 init函数是在哪里调用的。
radiolink@ubuntu:~/apm$ grep -nr "rcout->init" ./
./ardupilot/libraries/AP_HAL_FLYMAPLE/HAL_FLYMAPLE_Class.cpp:78: rcout->init(NULL);
./ardupilot/libraries/AP_HAL_AVR_SITL/HAL_AVR_SITL_Class.cpp:74: rcout->init(NULL);
./ardupilot/libraries/AP_HAL_Linux/HAL_Linux_Class.cpp:140: rcout->init(NULL);
./ardupilot/libraries/AP_HAL_VRBRAIN/HAL_VRBRAIN_Class.cpp:160: hal.rcout->init(NULL);
./ardupilot/libraries/AP_HAL_PX4/HAL_PX4_Class.cpp:128: hal.rcout->init(NULL);
./ardupilot/libraries/AP_HAL_AVR/HAL_AVR_APM1_Class.cpp:67: rcout->init(NULL);
./ardupilot/libraries/AP_HAL_AVR/HAL_AVR_APM2_Class.cpp:66: rcout->init(NULL);
radiolink@ubuntu:~/apm$
从这段结果中我们很容易帅选出我们的初始化。
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();
这就是我们前面看到的那个主循环，我们看到 " rcout->init"形参为空，所以我们不必考虑形参。
void PX4RCOutput::init(void* unused)
{
_perf_rcout = perf_alloc(PC_ELAPSED, "APM_rcout");
_pwm_fd = open(PWM_OUTPUT_DEVICE_PATH, O_RDWR);
if (_pwm_fd == -1) {
hal.scheduler->panic("Unable to open " PWM_OUTPUT_DEVICE_PATH);
}
if (ioctl(_pwm_fd, PWM_SERVO_ARM, 0) != 0) {
hal.console->printf("RCOutput: Unable to setup IO arming\n");
}
if (ioctl(_pwm_fd, PWM_SERVO_SET_ARM_OK, 0) != 0) {
hal.console->printf("RCOutput: Unable to setup IO arming OK\n");
}
_rate_mask = 0;
_alt_fd = -1;
_servo_count = 0;
_alt_servo_count = 0;
if (ioctl(_pwm_fd, PWM_SERVO_GET_COUNT, (unsigned long)&_servo_count) != 0) {
hal.console->printf("RCOutput: Unable to get servo count\n");
return;
}
_alt_fd = open("/dev/px4fmu", O_RDWR);
if (_alt_fd == -1) {
hal.console->printf("RCOutput: failed to open /dev/px4fmu");
return;
}
}
init函数代码不是特别多，我们看到它还打开了另外一个设备： "/dev/px4fmu"。这就是我们前面看到的 fmu，所以后面凡是看到_alt_fd我们就应该知道它是对 fmu的操作。关于 PWM这个设备，这里也就是还有几个 ioctl操作。结合注释，前两个操作是用来给飞控加锁的。如果你去查词典你会知道 "arming"是武装的意思，武装实际上意味着解锁。但这个时候显然是不应该解锁的，因为这个时候解锁显然是不合理的。但是在词典的网络释义中你会看到一个 "待发状态"的解释，我觉得这个解释在这里是比较合理的。而第三个 ioctl操作结合注释应该是用来获取有多少路输出的。最简单的就是你去查看 _servo_count是怎么使用的。
可能这个时候你会想继续跟踪 ioctl操作，看下它是怎么实现的，但这样必然涉及到 io固件，到时候肯定又是一箩筐的东西出来了，关键是我们还不知道那一箩筐东西都是干什么用的。所以相比之下，我更希望搞清楚上面是怎么使用hal.rcout的。因为根据目前掌握的信息， hal.rcout是用来跟 io固件交互的，但是我们知道 io控制了八路主输出通道即电机输出，而这些输出的计算都是在 fmu中完成的，所以我认为搞明白上层是怎么使用hal.rcout的我们就知道了电机控制信号是怎么输出的。但这又是一颗大地雷啊，其繁琐自不必说。
2. AP_MotorsQuad
如果你使用命令 "grep -nr "hal.rcout" ./"进行匹配，其匹配结果肯定会让你抓狂，有差不多两百个匹配结果，我们很容易不知道该去看哪一个。这样我们就需要对结果稍加分析，简单说就是归类。
./ardupilot/AntennaTracker/GCS_Mavlink.pde
./ardupilot/ArduCopter/radio.pde
./ardupilot/ArduCopter/GCS_Mavlink.pde
./ardupilot/libraries/AP_ServoRelayEvents/AP_ServoRelayEvents.cpp
./ardupilot/libraries/AP_HAL_FLYMAPLE/examples/RCPassthroughTest/RCPassthroughTest.pde
./ardupilot/libraries/AP_BoardConfig/AP_BoardConfig.cpp
./ardupilot/libraries/AP_HAL_VRBRAIN/HAL_VRBRAIN_Class.cpp
./ardupilot/libraries/AP_HAL_VRBRAIN/Scheduler.cpp
./ardupilot/libraries/RC_Channel/RC_Channel.cpp
./ardupilot/libraries/RC_Channel/RC_Channel_aux.cpp
./ardupilot/libraries/AP_HAL_PX4/HAL_PX4_Class.cpp
./ardupilot/libraries/AP_HAL_PX4/Scheduler.cpp
./ardupilot/libraries/AP_HAL_AVR/examples/RCPassthroughTest/RCPassthroughTest.pde
./ardupilot/libraries/APM_OBC/APM_OBC.cpp
./ardupilot/libraries/AP_Motors/examples/AP_Motors_test/AP_Motors_test.pde
./ardupilot/libraries/AP_Motors/AP_MotorsCoax.cpp
./ardupilot/libraries/AP_Motors/AP_MotorsHeli.cpp
./ardupilot/libraries/AP_Motors/AP_Motors_Class.cpp
./ardupilot/libraries/AP_Motors/AP_MotorsMatrix.cpp
./ardupilot/libraries/AP_Motors/AP_MotorsTri.cpp
./ardupilot/libraries/AP_Motors/AP_MotorsSingle.cpp
./ardupilot/libraries/DataFlash/LogFile.cpp
./ardupilot/libraries/GCS_MAVLink/GCS_Common.cpp
./ardupilot/libraries/AP_HAL/examples/RCOutput/RCOutput.pde
这是我按所在的源文件进行分类的，我们看到匹配结果主要出自 "ardupilot/libraries"目录。而其中有两个目录比较能够引起我们的注意： RC_Channel和 AP_Motors。前者我们很容易把它跟遥控器联系起来，而后者我们知道肯定跟电机相关。既然你跟电机有关，那不正合我意了吗？但问题是你搞这么多个 AP_Motors源文件干嘛呢？这个我们稍后会看到。而在 AP_Motors目录下有一个文件却特别值得我们注意： "AP_HAL_AVR/examples/RCPassthroughTest/RCPassthroughTest.pde"。因为它的路径中有一个关键字： "examples"。实际上这是教你如何使用 AP_Motors的一个示例。那么我们如何说它是一个示例程序呢？就凭这么一个关键字吗？非也，关键字只是给我们提供了一个线索而已。
/*
* Example of AP_Motors library.
* Code by Randy Mackay. DIYDrones.com
*/
// Libraries
const AP_HAL::HAL& hal = AP_HAL_BOARD_DRIVER;
RC_Channel rc1(0), rc2(1), rc3(2), rc4(3);
// uncomment the row below depending upon what frame you are using
//AP_MotorsTri motors(rc1, rc2, rc3, rc4);
AP_MotorsQuad motors(rc1, rc2, rc3, rc4);
//AP_MotorsHexa motors(rc1, rc2, rc3, rc4);
//AP_MotorsY6 motors(rc1, rc2, rc3, rc4);
//AP_MotorsOcta motors(rc1, rc2, rc3, rc4);
//AP_MotorsOctaQuad motors(rc1, rc2, rc3, rc4);
//AP_MotorsHeli motors(rc1, rc2, rc3, rc4);
// setup
void setup()
{
}
// loop
void loop()
{
}
// stability_test
void motor_order_test()
{
}
// stability_test
void stability_test()
{
}
AP_HAL_MAIN();
如果你还记得 ArduCopter，你就会发现这个源文件的结构跟 ArduCopter是一样的。也就是说它编译出来应该跟 ArduCopter同样对待。那么下面我们就来看看它是如何使用 AP_Motors的。
首先这里定义了一个变量： motors。这个变量代表了一组电机，实际上就是机架上所有的电机，而不是一个电机。所以从 motors的定义可以看出，同 AP_MotorsQuad一样 AP_MotorsTri、AP_MotorsHexa 、AP_MotorsY6都是用来控制电机的类。那么为什么要这么多个类呢？原因在于不可能只有一种机型！这个我们在调参的时候就可以看到。但是在这里，我们同样看到了RC_Channel这个类，而且 AP_Motors使用了 RC_Channel。所以我们就不妨去看看这个类。
/// @class AP_MotorsQuad
class AP_MotorsQuad : public AP_MotorsMatrix {
public:
/// Constructor
AP_MotorsQuad( RC_Channel& rc_roll, RC_Channel& rc_pitch, RC_Channel& rc_throttle, RC_Channel& rc_yaw, uint16_t speed_hz = AP_MOTORS_SPEED_DEFAULT) : AP_MotorsMatrix(rc_roll, rc_pitch, rc_throttle, rc_yaw, speed_hz) {
};
// setup_motors - configures the motors for a quad
virtual void setup_motors();
protected:
};
/// @class AP_MotorsMatrix
class AP_MotorsMatrix : public AP_Motors {
public:
/// Constructor
AP_MotorsMatrix( RC_Channel& rc_roll, RC_Channel& rc_pitch, RC_Channel& rc_throttle, RC_Channel& rc_yaw, uint16_t speed_hz = AP_MOTORS_SPEED_DEFAULT) :
/// @class AP_Motors
class AP_Motors {
public:
// Constructor
AP_Motors( RC_Channel& rc_roll, RC_Channel& rc_pitch, RC_Channel& rc_throttle, RC_Channel& rc_yaw, uint16_t speed_hz = AP_MOTORS_SPEED_DEFAULT);
这是 AP_MotorsQuad类的继承关系，而其构造函数都需要 RC_Channel作为参数。从构造函数中我们发现他们都只需要四个 RC_Channel参数，而且我们可以大致猜测其作用，即油门跟各个方向上的控制量。这个控制量未必就全部都来自遥控器，更有可能融合了姿态在里边。或者你这样想，如果里边没有融合姿态，那么 AP_MotorsQuad必定要讲姿态参与计算，在 AP_MotorsQuad类(或者父类)中必定有姿态相关的数据，但这个我并没有发现。其实我们发现 AP_MotorsQuad只重写了一个接口： setup_motors。也就是说这个接口体现了各种机型之间的区别。
在 APM中文网站上有一片教程：《 连接你的遥控输入和电机（Pixhawk）》，告诉你 PX4电机应该怎么接。
这张图片介绍了四旋翼电机是怎么接的。为什么要了解这个？这是因为在分析 setup_motors的时候我们将要用到。
// setup_motors - configures the motors for a quad
void AP_MotorsQuad::setup_motors()
{
// call parent
AP_MotorsMatrix::setup_motors();
// hard coded config for supported frames
if( _flags.frame_orientation == AP_MOTORS_PLUS_FRAME ) {
// plus frame set-up
add_motor(AP_MOTORS_MOT_1, 90, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 2);
add_motor(AP_MOTORS_MOT_2, -90, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 4);
add_motor(AP_MOTORS_MOT_3, 0, AP_MOTORS_MATRIX_YAW_FACTOR_CW, 1);
add_motor(AP_MOTORS_MOT_4, 180, AP_MOTORS_MATRIX_YAW_FACTOR_CW, 3);
}else if( _flags.frame_orientation == AP_MOTORS_V_FRAME ) {
// V frame set-up
}else if( _flags.frame_orientation == AP_MOTORS_H_FRAME ) {
// H frame set-up - same as X but motors spin in opposite directiSons
} else if (_flags.frame_orientation == AP_MOTORS_VTAIL_FRAME) {
}else{
// X frame set-up
add_motor(AP_MOTORS_MOT_1, 45, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 1);
add_motor(AP_MOTORS_MOT_2, -135, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 3);
add_motor(AP_MOTORS_MOT_3, -45, AP_MOTORS_MATRIX_YAW_FACTOR_CW, 4);
add_motor(AP_MOTORS_MOT_4, 135, AP_MOTORS_MATRIX_YAW_FACTOR_CW, 2);
}
}
// setup_motors - configures the motors for a given frame type - should be overwritten by child classes
virtual void setup_motors() {
remove_all_motors();
};
// remove_all_motors - removes all motor definitions
void AP_MotorsMatrix::remove_all_motors()
{
for( int8_t i=0; i
}
}
// add_motor using just position and yaw_factor (or prop direction)
void add_motor(int8_t motor_num, float angle_degrees,float yaw_factor, uint8_t testing_order);
这里我只保留了我接触比较多的两种模式，即 "+"模式和 "X"模式。这段代码实际上也就是修改了电机的配置。由 add_motor跟 remove_motor函数来完成。
首先我们需要把 add_motor函数的几个参数解释一下。 angle_degrees应该是一眼就看出来了，是一个角度。跟谁的角度呢？当然是机头方向。这里比较让人迷糊的就是 motor_num跟 testing_order这两个参数。两者都很像是电机的序号，但问题是同一个电机不应该有两个序号。所以结合 angle_degrees参数确认了一下， motor_num跟上面的图是能够对上的，也就是说这个才是电机序号。那么 testing_order是什么？目前我们看到使用它的代码是：
// output_test - spin a motor at the pwm value specified
// motor_seq is the motor's sequence number from 1 to the number of motors on the frame
// pwm value is an actual pwm value that will be output, normally in the range of 1000 ~ 2000
void AP_MotorsMatrix::output_test(uint8_t motor_seq, int16_t pwm)
{
// exit immediately if not armed
if (!_flags.armed) {
return;
}
// loop through all the possible orders spinning any motors that match that description
for (uint8_t i=0; i
// turn on this motor
hal.rcout->write(pgm_read_byte(&_motor_to_channel_map[i]), pwm);
}
}
}
我们的第一感觉当然是这个是测试用的，但是如果你继续跟踪：
radiolink@ubuntu:~/apm$ grep -nr output_test ./ardupilot/libraries/AP_Motors/
./ardupilot/libraries/AP_Motors/AP_Motors_Class.h:121: // output_test - spin a motor at the pwm value specified
./ardupilot/libraries/AP_Motors/AP_Motors_Class.h:124: virtual void output_test(uint8_t motor_seq, int16_t pwm) = 0;
./ardupilot/libraries/AP_Motors/AP_MotorsCoax.cpp:221:// output_test - spin a motor at the pwm value specified
./ardupilot/libraries/AP_Motors/AP_MotorsCoax.cpp:224:void AP_MotorsCoax::output_test(uint8_t motor_seq, int16_t pwm)
./ardupilot/libraries/AP_Motors/AP_MotorsHeli.cpp:277:// output_test - spin a motor at the pwm value specified
./ardupilot/libraries/AP_Motors/AP_MotorsHeli.cpp:280:void AP_MotorsHeli::output_test(uint8_t motor_seq, int16_t pwm)
你会有一种越看越乱的感觉，但其实你应该换一种方式去思考。如果你按照我们以前的顺时针或逆时针去编号，你就会发现一切都很好地符合。当然，我们这也只是知道了 testing_order是怎么来的，关于它是使用暂时就不讲了。
那么，还有最后一个形参： "yaw_factor"，从字面理解应该是 yaw的一个系数。怎么去理解呢？我们不妨先看看它们的值：
#define AP_MOTORS_MATRIX_YAW_FACTOR_CW -1
#define AP_MOTORS_MATRIX_YAW_FACTOR_CCW 1
我们看到实参只有两个值：+/-1。为什么？因为它表示的是方向！即顺时针跟逆时针。指的是谁呢？这个在网站上有说明，是 顺时针浆和 逆时针浆。其实对找图自己稍微分析一下也就出来了。那么这个参数的作用是什么呢？这个就涉及到四旋翼是怎么让机头转向的。四旋翼改变机头方向是通过让对角电机同增同减来实现的。所以我就需要知道机头转向的时候哪个电机应该增，哪个电机又应该减。
所以 PX4配置电机跟我们之前配置电机不一样，我们之前配置电机不计算角度。所以看 PX4对电机的处理我们多少会有点不习惯。
// add_motor using just position and prop direction
void AP_MotorsMatrix::add_motor(int8_t motor_num, float angle_degrees,float yaw_factor, uint8_t testing_order)
{
// call raw motor set-up method
add_motor_raw(
motor_num,
cosf(radians(angle_degrees + 90)), // roll factor
cosf(radians(angle_degrees)), // pitch factor
yaw_factor, // yaw factor
testing_order);
}
// add_motor
void AP_MotorsMatrix::add_motor_raw(int8_t motor_num, float roll_fac,float pitch_fac, float yaw_fac, uint8_t testing_order)
{
// ensure valid motor number is provided
if( motor_num >= 0 && motor_num < AP_MOTORS_MAX_NUM_MOTORS ) {
// increment number of motors if this motor is being newly motor_enabled
if( !motor_enabled[motor_num] ) {
motor_enabled[motor_num] = true;
}
// set roll, pitch, thottle factors and opposite motor (for stability patch)
_roll_factor[motor_num] = roll_fac;
_pitch_factor[motor_num] = pitch_fac;
_yaw_factor[motor_num] = yaw_fac;
// set order that motor appears in test
_test_order[motor_num] = testing_order;
// disable this channel from being used by RC_Channel_aux
RC_Channel_aux::disable_aux_channel(_motor_to_channel_map[motor_num]);
}
}
所以我们看到，最终它配置几个电机也就是配置了几个数组，所以删除电机的时候也就是删除这几个数组中相关的数据。所以现在的问题是谁在用这些数据？我们当然知道是它自己，关键是哪个函数在用，怎么用的。可能我们会很奇怪，为什么 roll跟 pitch的系数是一个余弦值？从解析几何中我们知道，一个向量在直角坐标系中的分量实际上就是该向量与各轴夹角的余弦值，所以这里计算的实际上就是分量。你可以这样去考虑，我们现在知道 roll的控制量是 R，但我们实际上要控制的是 4个电机，如果说四个电机都输出 R，那肯定就不能在 roll方向运动。所以需要有一种方式把 R分配到每一个电机上去。而这个 PX4采用的就是 R在 各电机方向上的分解。实际上如果我们把余弦值的大小(不包括符号)当成单位 1你会发现实际上跟我们以前做的小四轴是一样的。 pitch方向道理是一样的，但是他们之间相差有一个夹角，这个你可以打开 PX4的外壳看上面的丝印，方向符合右手定则。如果你把 PX4水平放正了你会看到 x顺时针旋转 90°即是 y轴。所以我们在上面看到的是 +90°而不是 -90°。
radiolink@ubuntu:~/apm$ grep -nr _roll_factor ./ardupilot/libraries/AP_Motors/
./ardupilot/libraries/AP_Motors/AP_MotorsMatrix.cpp:194: rpy_out[i] = _rc_roll.pwm_out * _roll_factor[i] +
./ardupilot/libraries/AP_Motors/AP_MotorsMatrix.cpp:375: _roll_factor[motor_num] = roll_fac;
./ardupilot/libraries/AP_Motors/AP_MotorsMatrix.cpp:411: _roll_factor[motor_num] = 0;
./ardupilot/libraries/AP_Motors/AP_MotorsMatrix.h:78: float _roll_factor[AP_MOTORS_MAX_NUM_MOTORS]; // each motors contribution to roll
radiolink@ubuntu:~/apm$
这里我想大家都看得出来第一条结果是使用。后面一次是 设置、删除、定义。既然找到了出处，那么接下来自然就是阅读源码了。
// output_armed - sends commands to the motors
// includes new scaling stability patch
void AP_MotorsMatrix::output_armed()
{
int8_t i;
int16_t out_min_pwm = _rc_throttle.radio_min + _min_throttle; // minimum pwm value we can send to the motors
int16_t out_max_pwm = _rc_throttle.radio_max; // maximum pwm value we can send to the motors
int16_t out_mid_pwm = (out_min_pwm+out_max_pwm)/2; // mid pwm value we can send to the motors
int16_t out_best_thr_pwm; // the is the best throttle we can come up which provides good control without climbing
float rpy_scale = 1.0; // this is used to scale the roll, pitch and yaw to fit within the motor limits
int16_t rpy_out[AP_MOTORS_MAX_NUM_MOTORS]; // buffer so we don't have to multiply coefficients multiple times.
int16_t motor_out[AP_MOTORS_MAX_NUM_MOTORS]; // final outputs sent to the motors
int16_t rpy_low = 0; // lowest motor value
int16_t rpy_high = 0; // highest motor value
int16_t yaw_allowed; // amount of yaw we can fit in
int16_t thr_adj; // the difference between the pilot's desired throttle and out_best_thr_pwm (the throttle that is actually provided)
// initialize limits flag
limit.roll_pitch = false;
limit.yaw = false;
limit.throttle_lower = false;
limit.throttle_upper = false;
// Throttle is 0 to 1000 only
// To-Do: we should not really be limiting this here because we don't "own" this _rc_throttle object
if (_rc_throttle.servo_out < 0) {
_rc_throttle.servo_out = 0;
limit.throttle_lower = true;
}
if (_rc_throttle.servo_out > _max_throttle) {
_rc_throttle.servo_out = _max_throttle;
limit.throttle_upper = true;
}
// capture desired roll, pitch, yaw and throttle from receiver
_rc_roll.calc_pwm();
_rc_pitch.calc_pwm();
_rc_throttle.calc_pwm();
_rc_yaw.calc_pwm();
// if we are not sending a throttle output, we cut the motors
if (_rc_throttle.servo_out == 0) {
} else {
// check if throttle is below limit
if (_rc_throttle.radio_out <= out_min_pwm) { // perhaps being at min throttle itself is not a problem, only being under is
limit.throttle_lower = true;
}
// calculate roll and pitch for each motor
// set rpy_low and rpy_high to the lowest and highest values of the motors
for (i=0; i
rpy_out[i] = _rc_roll.pwm_out * _roll_factor[i] +
_rc_pitch.pwm_out * _pitch_factor[i];
// record lowest roll pitch command
if (rpy_out[i] < rpy_low) {
rpy_low = rpy_out[i];
}
// record highest roll pich command
if (rpy_out[i] > rpy_high) {
rpy_high = rpy_out[i];
}
}
}
int16_t motor_mid = (rpy_low+rpy_high)/2;
out_best_thr_pwm = min(out_mid_pwm - motor_mid, max(_rc_throttle.radio_out, (_rc_throttle.radio_out+_hover_out)/2));
// calculate amount of yaw we can fit into the throttle range
// this is always equal to or less than the requested yaw from the pilot or rate controller
yaw_allowed = min(out_max_pwm - out_best_thr_pwm, out_best_thr_pwm - out_min_pwm) - (rpy_high-rpy_low)/2;
yaw_allowed = max(yaw_allowed, AP_MOTORS_MATRIX_YAW_LOWER_LIMIT_PWM);
if (_rc_yaw.pwm_out >= 0) {
// if yawing right
if (yaw_allowed > _rc_yaw.pwm_out) {
yaw_allowed = _rc_yaw.pwm_out; // to-do: this is bad form for yaw_allows to change meaning to become the amount that we are going to output
}else{
limit.yaw = true;
}
}else{
// if yawing left
yaw_allowed = -yaw_allowed;
if( yaw_allowed < _rc_yaw.pwm_out ) {
yaw_allowed = _rc_yaw.pwm_out; // to-do: this is bad form for yaw_allows to change meaning to become the amount that we are going to output
}else{
limit.yaw = true;
}
}
// add yaw to intermediate numbers for each motor
rpy_low = 0;
rpy_high = 0;
for (i=0; i
rpy_out[i] = rpy_out[i] +
yaw_allowed * _yaw_factor[i];
// record lowest roll+pitch+yaw command
if( rpy_out[i] < rpy_low ) {
rpy_low = rpy_out[i];
}
// record highest roll+pitch+yaw command
if( rpy_out[i] > rpy_high) {
rpy_high = rpy_out[i];
}
}
}
经过方才的分析，对 rpy_out的计算我想已经不难理。我们把公式稍微整理一下就得到：
rpy_out[i] = _rc_roll.pwm_out * _roll_factor[i] + _rc_pitch.pwm_out * _pitch_factor[i] + yaw_allowed * _yaw_factor[i];
这时候我们用 +/-1进行替换其实跟我们之前的计算是一样的，但可以看到，我们之前的只能算是简化版。这段代码的复杂主要在于 yaw_allowed的计算。其实我也一直都很疑惑，为什么值直接使用 _rc_yaw.pwm_out呢？使用 yaw_allowed之后 yaw的输出实际上比我们给的输出要小。关于 yaw_allowed的计算源码中有一段注释，但由于英文不好，所以就去掉了。机器翻译其实也晦涩难懂。
要搞懂 yaw_allowed我们需要先弄明白几个变量：out_max_pwm、out_best_thr_pwm、out_min_pwm、rpy_high、rpy_low。这是它计算时用到的五个变量，最后两个变量注释解释得比较清楚，是电机的最高和最低值。结合其计算，我们知道它是混合了 roll+pitch的控制量。源码中称做 "command"，这个应该是从用户的角度来描述的。理想情况下它们的和为 0。 out_max_pwm跟 out_min_pwm是油门输出的最大最小值。它们又涉及到另外三个变量： _rc_throttle.radio_min、 _rc_throttle.radio_max、 _min_throttle。其中前两个根据推测应该是校准时候获取的油门通道的最大与最小值。第三个是油门输出的最小值，我个人的理解是“怠速”。怠速油门确保只要飞控解锁了任何情况下电机都有控制信号，要让电机停止工作那就得给飞控上锁。
// set_min_throttle - sets the minimum throttle that will be sent to the engines when they're not off (i.e. to prevents issues with some motors spinning and some not at very low throttle)
void AP_Motors::set_min_throttle(uint16_t min_throttle)
{
_min_throttle = (float)min_throttle * (_rc_throttle.radio_max - _rc_throttle.radio_min) / 1000.0f;
}
// set_mid_throttle - sets the mid throttle which is close to the hover throttle of the copter
// this is used to limit the amount that the stability patch will increase the throttle to give more room for roll, pitch and yaw control
void AP_Motors::set_mid_throttle(uint16_t mid_throttle)
{
_hover_out = _rc_throttle.radio_min + (float)(_rc_throttle.radio_max - _rc_throttle.radio_min) * mid_throttle / 1000.0f;
}
基本上我就是从这段代码确定它们是油门校准时候的最大最小值。如果你写过飞控代码你就知道，这两个函数做的工作实际上就是映射，由等比公式得到。所以这里的形参都是按 1000作为最大行程量的。这样我们就知道 _min_throttle是怠速油门。那么 _hover_out又是什么？根据判断，这应该是悬停油门。但是我们要注意， _min_throttle跟 _hover_out的行程是不一样的，从计算中我们就可以看到，后者多了 _rc_throttle.radio_min。也就是说前者不能直接输出，但是后者可以。由于不能直接输出，在计算 out_min_pwm的时候才有 _rc_throttle.radio_min在里边，也就是说它实际上就是怠速。而怠速显然是解锁后的最低速度，所以这里又把它称做最小输出。
那么 out_best_thr_pwm又是什么呢？我对注释反正是一知半解的。如果按照注释去理解，它应该是最好的油门输出，但最好指的是什么我并不清楚。于是我也只好去分析它的计算了。
我们不如先理想化令 motor_mid的值为 0，这样计算就变成了：
out_best_thr_pwm = min(out_mid_pwm - 0, max(_rc_throttle.radio_out, (_rc_throttle.radio_out+_hover_out)/2));
out_best_thr_pwm = max(_rc_throttle.radio_out, (_rc_throttle.radio_out+_hover_out)/2);
if(out_best_thr_pwm>out_mid_pwm) out_best_thr_pwm=out_mid_pwm;
其中下面是我处理的等效代码。其实我们还可以进一步等效处理：
out_best_thr_pwm = (_rc_throttle.radio_out+_hover_out)/2;
if(out_best_thr_pwm<_rc_throttle.radio_out) out_best_thr_pwm=_rc_throttle.radio_out;
if(out_best_thr_pwm>out_mid_pwm) out_best_thr_pwm=out_mid_pwm;
这样一来我们终于清楚了，原来 out_best_thr_pwm就是油门输出跟悬停油门的均值啊，代码写那么复杂原来就是为了限位！
但是，当我试图用相同的方法处理 yaw_allowed的时候，我发现并不那么凑效。
yaw_allowed = min(out_max_pwm - out_best_thr_pwm, out_best_thr_pwm - out_min_pwm) - (rpy_high-rpy_low)/2; yaw_allowed = out_max_pwm - out_best_thr_pwm; for( i=0; i 3.AP_Motors_test // setup // loop // stability_test // set_roll, set_pitch, set_yaw, set_throttle // returns just the PWM without the offset from radio_min // set rc channel ranges void RC_Channel::load_eeprom(void) int16_t RC_Channel::range_to_pwm() radiolink@ubuntu:~/apm$ grep -nr save_eeprom ./ static void trim_radio() GCS_MAVLINK::handleMessage(mavlink_message_t* msg) void GCS_MAVLINK::update(void (*run_cli)(AP_HAL::UARTDriver *)) mavlink是一种通信协议，如果你到网上去搜索你就会看到这种通信协议已经应用到了 PX4，那么我觉得显然就应该是这里的 GCS_MAVLINK了。而在通信的另一头我们很容易想到是我们的调参软件。关于更多调参细节，这里当然无法完全说清楚，那就不如先留着吧。 // radiolink@ubuntu:~/apm$ grep -nr "motors.output" ./ardupilot/ArduCopter/ /***************************************** radiolink@ubuntu:~/apm$ grep -nr "motors.set_pitch" ./ radiolink@ubuntu:~/apm$ grep -nr "motors" ./ardupilot/ArduCopter/ArduCopter.pde radiolink@ubuntu:~/apm$ grep -nr "set_pitch" ./ardupilot/libraries/AC_AttitudeControl/ radiolink@ubuntu:~/apm$ grep -nr "_motors.set_throttle" ./ardupilot/libraries/AC_AttitudeControl/ PX4 FMU [12] RC_Channel 1. setup radiolink@ubuntu:~/apm$ ls ardupilot/libraries/RC_Channel/ #define CH_1 0 void setup() void RC_Channel::set_default_dead_zone(int16_t dzone) /* /* void print_radio_values() void setup_radio(void) void read_radio() // read input from APM_RC - create a control_in value /* HAL_PX4::HAL_PX4() : #include uint16_t PX4RCInput::read(uint8_t ch) bool PX4RCInput::set_overrides(int16_t *overrides, uint8_t len) PX4RCInput::set_override(uint8_t channel, int16_t override) { radiolink@ubuntu:~/apm$ grep -nr set_overrides ./ |grep rcin void PX4RCInput::_timer_tick(void) int orb_copy(const struct orb_metadata *meta, int handle, void *buffer) void PX4RCInput::init(void* unused) orb_subscribe(const struct orb_metadata *meta) /** /** void loop() void debug_rcin() { print_pwm() 1.input_rc /** orb_subscribe( const struct orb_metadata *meta) radiolink@ubuntu:~/apm$ grep -nr input_rc ./PX4Firmware/src/drivers/ /** radiolink@ubuntu:~/apm$ grep -nr ORB_DECLARE ./PX4Firmware/src/ /** int node_mkpath(char *buf, Flavor f, const struct orb_metadata *meta) radiolink@ubuntu:~/apm$ grep -nr o_name ./PX4Firmware/src/ radiolink@ubuntu:~/apm$ grep -nr input_rc ./PX4Firmware/src/ radiolink@ubuntu:~/apm$ grep -nr TOPIC_MASTER_DEVICE_PATH ./ ORBDevMaster::ORBDevMaster(Flavor f) : radiolink@ubuntu:~/apm$ grep -nr ORBDevMaster ./PX4Firmware/src/ {"uorb", SCHED_PRIORITY_DEFAULT, 4096, uorb_main}, ioctl(fd, ORBIOCADVERTISE, (unsigned long)(uintptr_t)meta); void PX4RCInput::_timer_tick(void) orb_copy(const struct orb_metadata *meta, int handle,void *buffer) ssize_t ORBDevNode::read(struct file *filp, char *buffer, size_t buflen) radiolink@ubuntu:~/apm$ grep -nr "ORB_ID(input_rc)" ./PX4Firmware/src/ ./PX4Firmware/src/drivers/px4io/px4io.cpp:1655: _to_input_rc = orb_advertise(ORB_ID(input_rc), &rc_val); int orb_publish(const struct orb_metadata *meta, orb_advert_t handle,const void *data) orb_advert_t orb_advertise(const struct orb_metadata *meta, const void *data) int PX4IO::io_get_raw_rc_input(rc_input_values &input_rc) void PX4IO::task_main() PX4IO::task_main_trampoline(int argc, char *argv[]) int PX4IO::init() 1.hrt_ppm_decode # 这是 IO固件所用到的一些文件，当然仅仅是 PX4Firmware目录下的。而现在我们要阅读的源码在 "PX4Firmware/src/modules/px4iofirmware/"目录，其内容如下： radiolink@ubuntu:~/apm$ ls PX4Firmware/src/modules/px4iofirmware/ 因为前面我们知道 fmu跟 IO是通过串口进行通讯的，所以我们现在要阅读的源码主要在 serial.c中。 if (channel_count > 9) { 因此我有理由相信在 IO中也使用了宏 PX4IO_PAGE_RAW_RC_INPUT。所以： radiolink@ubuntu:~/apm$ grep -nr PX4IO_PAGE_RAW_RC_INPUT ./PX4Firmware/src/modules/px4iofirmware/ 在这段代码中我们并没有看到数据拷贝，所以数据拷贝应该是由调用 registers_get的函数来完成。 static void rx_handle_packet(void) void rx_dma_callback(DMA_HANDLE handle, uint8_t status, void *arg) 在 rx_handle_packet函数中我们看到 IO提供了 registers_get跟registers_set函数分别用来获取和设置 IO数据。而 rx_dma_callback函数的调用我们很容易想到是在串口收到数据的时候。这个我们回头再来折腾，现在我们要去看另外一个东西：r_page_raw_rc_input。在registers_get函数中我们要读取的数据是来自 r_page_raw_rc_input，它的数据肯定也是有人放进去的，而不是凭空产生的。所以： radiolink@ubuntu:~/apm$ grep -nr r_page_raw_rc_input ./PX4Firmware/src/modules/px4iofirmware/ 经分析， controls.c是用来获取遥控器数据的。其实，从其命名上也是相当明显的。 /* 从源码中我们看到，数组 r_page_raw_rc_input中所存放的并不仅仅是接收到的遥控器数据。遥控器数据仅仅是其中一部分而已。如果我们去看其定义将会更加清楚： /** 当然我现在也不去研究这个数组中到底都放了些什么数据。我关心的是 ppm_buffer中的数据是怎么来的，直至数据的最源头。但是我们要知道ppm_buffer跟 ppm_decoded_channels是分不开的。 radiolink@ubuntu:~/apm$ grep -nr ppm_buffer ./PX4Firmware/src/ 在这里，我们可能会觉得是源文件 ppm_decode.c是我们要找的文件，但是： void ppm_input_decode(bool reset, unsigned count) 所以函数 ppm_input_decode根本就没有被调用。而且我们前面的 mk文件中根本就没有添加systemlib目录。那么我们要找的源码就只能在源文件 drv_hrt.c中了。 /** 这个时候我们已经跟踪到中断服务函数了，在往下那就是中断初始化了。从这里我们也看到，真正的 PPM数据最后是来自 ppm_temp_buffer数组，这是由 hrt_ppm_decode函数剩下的部分代码来完成的，负责 PPM解码工作。 switch (ppm.phase) { 实际上就是对脉宽进行测量。 void 我以为 PX4仅仅支持 ppm跟 sbus，但从这里我们知道，其实 PX4还支持另外一种dsm输入。当然，采用那种输入方式并不是我们现在所关心的。我们现在关系的是谁调用了 controls_tick函数。 radiolink@ubuntu:~/apm$ grep -nr controls_tick ./PX4Firmware/src/ user_start函数是 IO的 init进程，这个前面我们已经分析过了。所以，最后是由 IO的主循环对 controls_tick函数进行调用。 看过了 IO的输入，我们现在来看看 IO中是怎么把信号输出的。 // output_test - spin a motor at the pwm value specified ssize_t PX4IO::write(file * /*filp*/, const char *buffer, size_t len) radiolink@ubuntu:~/apm$ grep -nr PX4IO_PAGE_DIRECT_PWM ./PX4Firmware/src/ 因为我们现在关注的是 io固件，所以我们现在要找的源码在 "registers.c"源文件中。可能我们觉得这里值应该出现一个 "case"语句，但这里却出现了两个，为什么？如果你去看源码你会看到实际上有一个 "registers_set"和一个 "registers_get"函数。当然 get函数我们没有使用，就不去关心。下面我们就来看看 set函数。 int registers_set(uint8_t page, uint8_t offset, const uint16_t *values,unsigned num_values) 所以我们看到，在这里 set主要是数据拷贝动作。而我们现在要解决的是在把数据放到 "r_page_servos"数组中之后数据是怎样到大寄存器的。 radiolink@ubuntu:~/apm$ grep -nr r_page_servos ./PX4Firmware/src/modules/px4iofirmware/ 去阅读源码我们就会发现， mixer.cpp中的结果均来自同一个函数： mixer_tick。该函数同样是在 Px4io.c中由 user_start调用。代码如下： int user_start(int argc, char *argv[]) 这样我们就只需要关心 mixer_tick函数即可。而该函数从头到尾超过 150行代码，我们在看源码的时候要将它拆成几段。 void mixer_tick(void) 这段代码并不难理解，是用来检测超时的。超时时间为 200ms。可能我们都会奇怪， user_start中的主循环执行一次会消耗 200ms吗？关于这点，我只能说目前我还没有去研究它是怎么控制循环的，暂时不予讨论。 /* default to failsafe mixing */ r_status_flags就是前面我们在 set函数中设置的标志，如果set函数被正常调用，那么到这里 source的值为 MIX_NONE。关于失控保护，正常情况下是可以忽略的。 /* 这段代码注释说的比较清楚，是进行解锁检查的。说到解锁，我们都还没看 fmu是怎么解锁的，待会还真得看看。飞控通常都是要解锁之后才能飞的，这是出于安全考虑。我想 fmu解锁了应该也会通过串口发送一个信息给 io并最终调用 set函。 /* 从前面的代码中我们知道 source的值为 MIX_NONE，所以这一段不会执行。这段代码只是给 r_page_servos一个特定的值，前面是失控保护，而后面应该是关闭电机。其实如果 fmu发过来的数据是 1ms的脉宽，电机也是关闭的。 /* set arming */ 所以最终是通过 up_pwm_servo_set函数最终将 PWM信号输出的。其中 r_page_servos这组数据是 fmu发送过来的，而 r_page_servo_disarmed这组数据是前面 pwm_limit_calc函数计算得到的。 PX4 FMU [16] arm 需要先说明一下，这里的 arm不是指 ARM芯片或 ARM公司，而是指的解锁。当然我们去查词典 arm并没有解锁的解释，最为接近的翻译个人认为是"武装"。至于为什么取 "arm"这么一个名字，我觉得应该还是从安全的角度考虑的。即解锁之后表示飞控具有一定的危险性，加锁之后则是安全的。 static const AP_Scheduler::Task scheduler_tasks[] PROGMEM = { // rc_loops - reads user input from transmitter/receiver 所以，最后遥控器数据是在 control_in中放着。关于这段代码的细节我们这里就不去深究了，在这里我们只要清楚我们是通过 "hal.rcin"来获取遥控器数据的就可以了。而数组中的另一个函数：arm_motors_check才是我们的中点。这个函数干了啥事呢？它是用来解锁跟加锁的。这个函数我们要把它拆成几段， // arm_motors_check - checks for pilot input to arm or disarm the copter 这一段代码是在解锁前检查油门跟模式。油门最低不需要解释，这是必须的。模式检查则提醒我们不是所有模式下都允许解锁。为什么呢？这么说吧，当四轴正在空中飞行的时候解锁操作显然是不被允许的，因为解锁伴随着一定量的初始化，容易导致失控。但问题是在检测模式之前不是已经检查过油门了吗？难道油门都已经最低了四轴还能在空中继续飞行？然而这样的情况确实存在，就是当油门通道控制的不再是平均转速而是升降速率的时候这种情况将会发生，也就是带气压计的像定高，定点这样的一些模式。在这样一些模式只有在四轴着陆之后才允许解锁。这就是变量 "ap.land_complete"的作用。该变量表示四轴是否已经着陆。为什么说它代表了飞行器着陆呢？我们倒不妨去跟踪下代码： void set_land_complete(bool b) 结果中的第一个 "true"是落地检测，后面两个是初始化跟加锁解锁的时候调用的。 // update_land_detector - checks if we have landed and updates the ap.land_complete flag 可能我们看不太懂 "motors.limit.throttle_lower"这个变量，但结合注释，这里确实是落地检测，说的更直接点就是着陆。而因为在油门控制平均转速的时候，油门为 0即意味着已经着陆，所以在这些模式不需要考虑着陆的问题。 int16_t tmp = g.rc_4.control_in; 这段代码不难理解，其实就是通过 4通摇杆打到最边上来进行加锁和解锁操作，核心函数分别是： init_disarm_motors和 init_arm_motors。但是在调用 "init_arm_motors"函数之前， pre_arm_checks跟 arm_checks函数负责对飞控进行检查，其中未必每一项我们都能够理解，但是在 arm_checks函数中有一项我觉得我们是需要知道的： // check if safety switch has been pushed 用过 PX4的都知道， PX4在使用遥控器解锁之前需要先解锁安全开关，而这段代码按照注释的说法就是检查安全开关的状态。开关其实只有两种状态，其实这里是检查安全开关有没有解锁。所以最后我们应该清楚，我们使用遥控器解锁这只是一个解锁动作，也就是众多解锁条件中的一个。真正要解锁，传感器还必须正常工作，这也是解锁必备的条件，否则是不能解锁的。 // init_arm_motors - performs arming process including initialisation of barometer and gyros 这里的几个初始化可能我们会接触比较多，第一个是初始化机头，然后是 home坐标和气压计。这几个初始化告诉我们每一次解锁都会以当前的位置来重置这些数据，也就是初始方向，初始位置和初始高度都是由解锁时刻决定的，这点很重要，要不要我们就可能会搞不清楚我们的机头、home跟初始高度。 PX4 FMU [17] stabilize bitcraze@bitcraze-vm:~/apm$ ls ardupilot/ArduCopter/control_* // stabilize_init - initialise stabilize controller bitcraze@bitcraze-vm:~/apm$ grep -nr stabilize_init ardupilot/ArduCopter/ static void rc_loop() // stabilize_run - runs the main stabilize controller // relax_bf_rate_controller - ensure body-frame rate controller has zero errors to relax rate controller output // get_pilot_desired_angle - transform pilot's roll or pitch input into a desired lean angle // get_pilot_desired_throttle - transform pilot's throttle input to make cruise throttle mid stick // set_throttle_out - to be called by upper throttle controllers when they wish to provide throttle output directly to motors // get_angle_boost - returns a throttle including compensation for roll/pitch angle throttle_out = constrain_float((float)(throttle_pwm-_motors.throttle_min()) * temp + _motors.throttle_min(), _motors.throttle_min(), 1000); float temp = _ahrs.cos_roll(); bitcraze@bitcraze-vm:~/apm$ grep -nr set_throttle_out ardupilot/ArduCopter/ // get_smoothing_gain - returns smoothing gain to be passed into attitude_control.angle_ef_roll_pitch_rate_ef_yaw_smooth // methods to be called by upper controllers to request and implement a desired attitude // methods to be called by upper controllers to request and implement a desired attitude ATC_ACCEL_RP_MAX,126000 Vector3f angle_ef_error; // earth frame angle errors // update_ef_roll_angle_and_error - update _angle_ef_target.x using an earth frame roll rate request rate_ef_desired = safe_sqrt(2.0f*126000*(fabs(angle_to_target)-437.5)); // Main loop - 100hz // rate_controller_run - run lowest level body-frame rate controller and send outputs to the motors // rate_bf_to_motor_roll - ask the rate controller to calculate the motor outputs to achieve the target rate in centi-degrees / second PX4 FMU [18] althold 1.althold_init // althold_init - initialise althold controller /// set_speed_z - sets maximum climb and descent rates bitcraze@bitcraze-vm:~/apm$ grep -nr _speed_down_cms ./ardupilot/libraries/AC_AttitudeControl/ bitcraze@bitcraze-vm:~/apm$ grep -nr _accel_z_cms ./ardupilot/libraries/AC_AttitudeControl/ /// set_target_to_stopping_point_z - returns reasonable stopping altitude in cm above home // althold_run - runs the althold controller // get_pilot_desired_climb_rate - transform pilot's throttle input to // set_throttle_takeoff - allows parents to tell throttle controller we are taking off so I terms can be cleared bitcraze@bitcraze-vm:~/apm$ grep -nr _max_throttle ./ardupilot/libraries/ // update_max_throttle - updates the limits on _max_throttle if necessary taking into account slow_start_throttle flag // call throttle controller /// set_alt_target_from_climb_rate - adjusts target up or down using a climb rate in cm/s // pos_to_rate_z - position to rate controller for Z axis _vel_target.z = safe_sqrt(2.0f*250*(_pos_error.z-125)); // rate_to_accel_z - calculates desired accel required to achieve the velocity target // accel_to_throttle - alt hold's acceleration controller _accel_error.z = _accel_error.z + 0.11164f * (constrain_float(accel_target_z - z_accel_meas, -32000, 32000) - _accel_error.z); bitcraze@bitcraze-vm:~/apm$ grep -nr _throttle_hover ardupilot/libraries/ // update_thr_cruise - update throttle cruise if necessary // Default constructor. /** // return true if inertial navigation is active bitcraze@bitcraze-vm:~/apm$ grep -nr _ekf_use ardupilot/libraries/ /** /** bitcraze@bitcraze-vm:~/apm$ grep -nr "inertial_nav.update" ./ardupilot/ArduCopter/ // update - updates velocities and positions using latest info from ahrs and barometer if new data is available; float tmp = _k3_xy * dt; bitcraze@bitcraze-vm:~/apm$ grep -nr _position_error ./ardupilot/libraries/ // check_baro - check if new baro readings have arrived and use them to correct vertical accelerometer offsets // check_gps - check if new gps readings have arrived and use them to correct position estimates bitcraze@bitcraze-vm:~/apm$ grep -nr "_flags.gps_glitching" ./ardupilot/libraries/
yaw_allowed = max(yaw_allowed, AP_MOTORS_MATRIX_YAW_LOWER_LIMIT_PWM);
yaw_allowed = out_best_thr_pwm - out_min_pwm;
if(yaw_allowed>(out_max_pwm - out_best_thr_pwm)) yaw_allowed=(out_max_pwm - out_best_thr_pwm);
yaw_allowed = yaw_allowed - (rpy_high-rpy_low)/2;
if(yaw_allowed
处理之后发现在最大最小值之间插了一个计算进去，使得代码又多了一层神秘感。其实这里还有下面这种等效：
if(yaw_allowed>(out_best_thr_pwm - out_min_pwm)) yaw_allowed=(out_best_thr_pwm - out_min_pwm);
yaw_allowed = yaw_allowed - (rpy_high-rpy_low)/2;
if(yaw_allowed
但似乎不论那种形式的等效，都不能很好地帮助我们理解作者的意图。在我看来这里 "-(rpy_high-rpy_low)/2"是搅局的，它搅乱了我的判断。所以暂时还是没有想明白为什么要这样计算，也只好先放一边了。而在 output_armed函数的最后，我们会看到：
hal.rcout->write(pgm_read_byte(&_motor_to_channel_map[i]), motor_out[i]);
}
}
}
我们看到，最后是通过 write函数将数据发给 io的。而且数据后面还是经过处理的，处理之后把数据放到了 motor_out数组中。
PX4 FMU [11] RCOutput
刚刚我们话了不少篇幅去讲述 AP_MotorsQuad。在分析 output_armed函数的时候，我们遇到了点困难，最后还剩下一点代码我们也跳过了。因为在写下去的话篇幅太长了，自己到时候回过头来看的时候也肯定会觉得繁琐，于是索性就到此为止了，毕竟这个地方已经耗了几天了。
所以，现在我们还是先来看看这个示例程序。
void setup()
{
hal.console->println("AP_Motors library test ver 1.0");
// motor initialisation
motors.set_update_rate(490);
// motors.set_frame_orientation(AP_MOTORS_X_FRAME);
motors.set_frame_orientation(AP_MOTORS_PLUS_FRAME);
motors.set_min_throttle(130);
motors.set_mid_throttle(500);
motors.Init(); // initialise motors
// setup radio
if (rc3.radio_min == 0) {
// cope with AP_Param not being loaded
rc3.radio_min = 1000;
}
if (rc3.radio_max == 0) {
// cope with AP_Param not being loaded
rc3.radio_max = 2000;
}
// set rc channel ranges
rc1.set_angle(4500);
rc2.set_angle(4500);
rc3.set_range(130, 1000);
rc4.set_angle(4500);
motors.enable();
motors.output_min();
hal.scheduler->delay(1000);
}
这里我就不再去跟踪 motors在底下到底都干了些啥了，因为那样的话你就需要从 AP_MotorsMatrix一直跟踪到 px4io，甚至是 io固件。那么话说回来， setup到底又干了些啥呢？先是设置了更新速率，然后设置机型，再是设置怠速跟悬停油门，这个时候才到初始化。初始化我们就不去看了。后面就是 rc初始化，然后使能输出。关于 rc，后面我们需要专门分析 RC_Channel，这里也略过。
void loop()
{
int16_t value;
// display help
hal.console->println("Press 't' to run motor orders test, 's' to run stability patch test. Be careful the motors will spin!");
// wait for user to enter something
while( !hal.console->available() ) {
hal.scheduler->delay(20);
}
// get character from user
value = hal.console->read();
// test motors
if (value == 't' || value == 'T') {
motor_order_test();
}
if (value == 's' || value == 'S') {
stability_test();
}
}
我们看到，主循环是从终端获取输入，根据用户输入的命令调用不同的测试函数。其中 motor_order_test函数调用了 motors.output_test接口进行测试，我们就不讨论了。
void stability_test()
{
int16_t value, roll_in, pitch_in, yaw_in, throttle_in, throttle_radio_in, avg_out;
int16_t testing_array[][4] = {
// roll, pitch, yaw, throttle
{ 0, 0, 0, 0},
{ 0, 0, 0, 100},
{ 0, 0, 0, 200},
{ 0, 0, 0, 300},
{ 4500, 0, 0, 300},
{ -4500, 0, 0, 300},
{ 0, 4500, 0, 300},
{ 0, -4500, 0, 300},
{ 0, 0, 4500, 300},
{ 0, 0, -4500, 300},
{ 0, 0, 0, 400},
{ 0, 0, 0, 500},
{ 0, 0, 0, 600},
{ 0, 0, 0, 700},
{ 0, 0, 0, 800},
{ 0, 0, 0, 900},
{ 0, 0, 0, 1000},
{ 4500, 0, 0, 1000},
{ -4500, 0, 0, 1000},
{ 0, 4500, 0, 1000},
{ 0, -4500, 0, 1000},
{ 0, 0, 4500, 1000},
{ 0, 0, -4500, 1000},
{5000, 1000, 0, 1000},
{5000, 2000, 0, 1000},
{5000, 3000, 0, 1000},
{5000, 4000, 0, 1000},
{5000, 5000, 0, 1000},
{5000, 0, 1000, 1000},
{5000, 0, 2000, 1000},
{5000, 0, 3000, 1000},
{5000, 0, 4500, 1000}
};
uint32_t testing_array_rows = 32;
hal.console->printf_P(PSTR("\nTesting stability patch\nThrottle Min:%d Max:%d\n"),(int)rc3.radio_min,(int)rc3.radio_max);
// arm motors
motors.armed(true);
// run stability test
for (int16_t i=0; i < testing_array_rows; i++) {
roll_in = testing_array[i][0];
pitch_in = testing_array[i][1];
yaw_in = testing_array[i][2];
throttle_in = testing_array[i][3];
motors.set_pitch(roll_in);
motors.set_roll(pitch_in);
motors.set_yaw(yaw_in);
motors.set_throttle(throttle_in);
motors.output();
// calc average output
throttle_radio_in = rc3.radio_out;
avg_out = ((hal.rcout->read(0) + hal.rcout->read(1) + hal.rcout->read(2) + hal.rcout->read(3))/4);
// display input and output
hal.console->printf_P(PSTR("R:%5d \tP:%5d \tY:%5d \tT:%5d\tMOT1:%5d \tMOT2:%5d \tMOT3:%5d \tMOT4:%5d \t ThrIn/AvgOut:%5d/%5d\n"),
(int)roll_in,
(int)pitch_in,
(int)yaw_in,
(int)throttle_in,
(int)hal.rcout->read(0),
(int)hal.rcout->read(1),
(int)hal.rcout->read(2),
(int)hal.rcout->read(3),
(int)throttle_radio_in,
(int)avg_out);
}
// set all inputs to motor library to zero and disarm motors
motors.set_pitch(0);
motors.set_roll(0);
motors.set_yaw(0);
motors.set_throttle(0);
motors.armed(false);
hal.console->println("finished test.");
}
这里并没有使用遥控器，而是设计了一个数组作为输入。而真正去控制电机，我们看到也只需使用 5个接口， 4个 set和一个 output。
void set_roll(int16_t roll_in) { _rc_roll.servo_out = roll_in; }; // range -4500 ~ 4500
void set_pitch(int16_t pitch_in) { _rc_pitch.servo_out = pitch_in; }; // range -4500 ~ 4500
void set_yaw(int16_t yaw_in) { _rc_yaw.servo_out = yaw_in; }; // range -4500 ~ 4500
void set_throttle(int16_t throttle_in) { _rc_throttle.servo_out = throttle_in; }; // range 0 ~ 1000
// output - sends commands to the motors
void AP_Motors::output()
{
// update max throttle
update_max_throttle();
// output to motors
if (_flags.armed ) {
output_armed();
}else{
output_disarmed();
}
};
这里的这几个 "_rc"也正好是我们前面 output_armed函数所使用的。而 output函数也就只是根据是否解锁调用不同的函数完成下一步操作。不难理解，在 output_disarmed函数中所做的事情就是关闭电机，这个是显而易见的。
关于 RC_Channel我们就先看这么多。
4. RC_Channel
在 test中我们看到 AP_Motors使用 set设置 rc的值，而在 AP_MotorsMatrix::output_armed中我们也看到，我们真正在用的时候使用的是 pwm_out而不是 servo_out。那么他们之间肯定有一个计算，这个过程就是由 calc_pwm函数来完成的。
void RC_Channel::calc_pwm(void)
{
if(_type == RC_CHANNEL_TYPE_RANGE) {
pwm_out = range_to_pwm();
radio_out = (_reverse >= 0) ? (radio_min + pwm_out) : (radio_max - pwm_out);
}else if(_type == RC_CHANNEL_TYPE_ANGLE_RAW) {
pwm_out = (float)servo_out * 0.1f;
radio_out = (pwm_out * _reverse) + radio_trim;
}else{ // RC_CHANNEL_TYPE_ANGLE
pwm_out = angle_to_pwm();
radio_out = pwm_out + radio_trim;
}
radio_out = constrain_int16(radio_out, radio_min.get(), radio_max.get());
}
那么在这里呢我们先要解决一个问题，就是 "_type"，我们需要知道它的值是多少。那么我们回到 test中的 "setup"函数，我们会看到其中有这样几条语句：
rc1.set_angle(4500);
rc2.set_angle(4500);
rc3.set_range(130, 1000);
rc4.set_angle(4500);
// setup the control preferences
void RC_Channel::set_range(int16_t low, int16_t high)
{
_type = RC_CHANNEL_TYPE_RANGE;
_high = high;
_low = low;
_high_out = high;
_low_out = low;
}
void RC_Channel::set_angle(int16_t angle)
{
_type = RC_CHANNEL_TYPE_ANGLE;
_high = angle;
}
这样我们就知道油门通道使用 range_to_pwm函数计算，其余三个通道使用 angle_to_pwm函数计算。但其实我一直都有个疑问， " radio_out"是什么？为什么会需要两个输出？其实你如果仔细观察 calc_pwm函数你就会发现，两者之间其实就是相差了一个量。 radio_min跟 radio_max我前面说是校准是后获取的最小最大值，那么 radio_trim又是什么？根据经验它应该是一个修正量。我们可以找到这样一段代码：
{
radio_min.load();
radio_trim.load();
radio_max.load();
_reverse.load();
_dead_zone.load();
}
void RC_Channel::save_eeprom(void)
{
radio_min.save();
radio_trim.save();
radio_max.save();
_reverse.save();
_dead_zone.save();
}
这两个函数前者是加载数据，后者是保存数据。加载的数据肯定源自保存的数据，问题在于谁在保存数据？这个肯定是校准程序干的！所以，综合以上信息，其实 pwm_out跟 radio_out就是范围上的一个差别。pwm_out对应的是 PWM的输出范围，即 [1-1000]，而 radio_out对应的是遥控器的输出范围。我们知道遥控器的输出是 [1-2] ms，在这里实际上就是 [1000 - 2000]，这是一个标准信号，可以直接用来控制舵机电调。当然，这个只是油门 3通的情况是这个范围。对于 1、2、4通由于 servo_out是有符号数，所以 pwm_out是可正可负的。
{
if (_high_out == _low_out) {
return radio_trim;
}
return ((long)(servo_out - _low_out) * (long)(radio_max - radio_min)) / (long)(_high_out - _low_out);
}
int16_t RC_Channel::angle_to_pwm()
{
if((servo_out * _reverse) > 0)
return _reverse * ((long)servo_out * (long)(radio_max - radio_trim)) / (long)_high;
else
return _reverse * ((long)servo_out * (long)(radio_trim - radio_min)) / (long)_high;
}
angle_to_pwm函数还是比较容易看出来这是行程映射，但 range_to_pwm由于使用了 _low_out多少会让人有点糊涂。其实你把 _low_out理解成我们所说的怠速就通了。由于油门有怠速，那自然是高于怠速部分的才需要做映射。
但是刚刚我们说 save_eeprom是校准的时候用到的，那么现在我们就来看看到底是不是。
./ardupilot/ArduCopter/radio.pde:200: g.rc_1.save_eeprom();
./ardupilot/ArduCopter/radio.pde:201: g.rc_2.save_eeprom();
./ardupilot/ArduCopter/radio.pde:202: g.rc_4.save_eeprom();
./ardupilot/libraries/RC_Channel/RC_Channel.cpp:262:RC_Channel::save_eeprom(void)
./ardupilot/libraries/RC_Channel/RC_Channel.h:49: void save_eeprom(void);
radiolink@ubuntu:~/apm$
但这个结果却让人觉得很奇怪，少了一个通道：三通。包括其它的辅助通道也统统没有。这说明系统中还有其它的方式来保存这些数据。
{
for (uint8_t i = 0; i < 30; i++) {
read_radio();
}
g.rc_1.trim(); // roll
g.rc_2.trim(); // pitch
g.rc_4.trim(); // yaw
g.rc_1.save_eeprom();
g.rc_2.save_eeprom();
g.rc_4.save_eeprom();
}
void
{
uint8_t result = MAV_RESULT_FAILED; // assume failure. Each messages id is responsible for return ACK or NAK if required
switch (msg->msgid) {
// Pre-Flight calibration requests
case MAVLINK_MSG_ID_COMMAND_LONG: // MAV ID: 76
{
// decode packet
mavlink_command_long_t packet;
mavlink_msg_command_long_decode(msg, &packet);
// exit immediately if this command is not meant for this vehicle
if (mavlink_check_target(packet.target_system, packet.target_component)) {
break;
}
switch(packet.command) {
case MAV_CMD_PREFLIGHT_CALIBRATION:
if (packet.param1 == 1 ||
packet.param2 == 1) {
ins.init_accel();
ahrs.set_trim(Vector3f(0,0,0)); // clear out saved trim
result = MAV_RESULT_ACCEPTED;
}
if (packet.param3 == 1) {
init_barometer(false); // fast barometer calibration
result = MAV_RESULT_ACCEPTED;
}
if (packet.param4 == 1) {
trim_radio();
result = MAV_RESULT_ACCEPTED;
}
{
// receive new packets
mavlink_message_t msg;
mavlink_status_t status;
status.packet_rx_drop_count = 0;
// process received bytes
uint16_t nbytes = comm_get_available(chan);
for (uint16_t i=0; i
uint8_t c = comm_receive_ch(chan);
if (run_cli != NULL) {
/* allow CLI to be started by hitting enter 3 times, if no
* heartbeat packets have been received */
if (!mavlink_active && (hal.scheduler->millis() - _cli_timeout) < 20000 &&
comm_is_idle(chan)) {
if (c == '\n' || c == '\r') {
crlf_count++;
} else {
crlf_count = 0;
}
if (crlf_count == 3) {
run_cli(_port);
}
}
}
// Try to get a new message
if (mavlink_parse_char(chan, c, &msg, &status)) {
// we exclude radio packets to make it possible to use the
// CLI over the radio
if (msg.msgid != MAVLINK_MSG_ID_RADIO && msg.msgid != MAVLINK_MSG_ID_RADIO_STATUS) {
mavlink_active = true;
}
handleMessage(&msg);
}
}
5.ArduCopter
简单看了下测试程序，现在我们来看下在 ArduCopter中又是怎么使用 AP_Motors的。
// Motor Output
#if FRAME_CONFIG == QUAD_FRAME
#define MOTOR_CLASS AP_MotorsQuad
#elif FRAME_CONFIG == TRI_FRAME
#define MOTOR_CLASS AP_MotorsTri
#elif FRAME_CONFIG == HEXA_FRAME
#define MOTOR_CLASS AP_MotorsHexa
#elif FRAME_CONFIG == Y6_FRAME
#define MOTOR_CLASS AP_MotorsY6
#elif FRAME_CONFIG == OCTA_FRAME
#define MOTOR_CLASS AP_MotorsOcta
#elif FRAME_CONFIG == OCTA_QUAD_FRAME
#define MOTOR_CLASS AP_MotorsOctaQuad
#elif FRAME_CONFIG == HELI_FRAME
#define MOTOR_CLASS AP_MotorsHeli
#elif FRAME_CONFIG == SINGLE_FRAME
#define MOTOR_CLASS AP_MotorsSingle
#elif FRAME_CONFIG == COAX_FRAME
#define MOTOR_CLASS AP_MotorsCoax
#else
#error Unrecognised frame type
#endif
#if FRAME_CONFIG == HELI_FRAME // helicopter constructor requires more arguments
static MOTOR_CLASS motors(g.rc_1, g.rc_2, g.rc_3, g.rc_4, g.rc_7, g.rc_8, g.heli_servo_1, g.heli_servo_2, g.heli_servo_3, g.heli_servo_4);
#elif FRAME_CONFIG == TRI_FRAME // tri constructor requires additional rc_7 argument to allow tail servo reversing
static MOTOR_CLASS motors(g.rc_1, g.rc_2, g.rc_3, g.rc_4, g.rc_7);
#elif FRAME_CONFIG == SINGLE_FRAME // single constructor requires extra servos for flaps
static MOTOR_CLASS motors(g.rc_1, g.rc_2, g.rc_3, g.rc_4, g.single_servo_1, g.single_servo_2, g.single_servo_3, g.single_servo_4);
#elif FRAME_CONFIG == COAX_FRAME // single constructor requires extra servos for flaps
static MOTOR_CLASS motors(g.rc_1, g.rc_2, g.rc_3, g.rc_4, g.single_servo_1, g.single_servo_2);
#else
static MOTOR_CLASS motors(g.rc_1, g.rc_2, g.rc_3, g.rc_4);
#endif
从这里看，我们通过宏 "FRAME_CONFIG"确定我们到底使用哪一种机型。我们可以在配置文件 "ardupilot/ArduCopter/config.h"中找到其定义：
// FRAME_CONFIG
//
#ifndef FRAME_CONFIG
# define FRAME_CONFIG QUAD_FRAME
#endif
但这会让我们觉得很奇怪，我们不是可以通过上位机配置机型的吗，怎么是编译的时候就确定了机型呢？这个我想需要后面去分析 PX4的调参才能有答案。
从源码中我们看到，这里有出现了一个新东西： "g"。这可不是什么重力加速度。
// Parameters
//
// Global parameters are all contained within the 'g' class.
//
static Parameters g;
我们不去查看 Parameters这个类型，内容太多，足足有五百多行。所以我们还是去查看它是怎么控制电机的。根据我们前面看到的示例程序，输出使用的是 motors.output接口，于是：
./ardupilot/ArduCopter/setup.pde:260: motors.output_test(i, pwm_high);
./ardupilot/ArduCopter/setup.pde:287: motors.output_test(i, pwm_low);
./ardupilot/ArduCopter/radio.pde:82: motors.output_min();
./ardupilot/ArduCopter/motor_test.pde:61: motors.output_test(motor_test_seq, pwm);
./ardupilot/ArduCopter/failsafe.pde:56: motors.output_min();
./ardupilot/ArduCopter/failsafe.pde:67: motors.output();
./ardupilot/ArduCopter/compassmot.pde:232: motors.output_min();
./ardupilot/ArduCopter/system.pde:64: motors.output();
./ardupilot/ArduCopter/GCS_Mavlink.pde:187: // set motors outputs as enabled if safety switch is not disarmed (i.e. either NONE or ARMED)
./ardupilot/ArduCopter/motors.pde:667: motors.output();
radiolink@ubuntu:~/apm$
这里我们匹配到两个结果， " ardupilot /ArduCopter/failsafe.pde"是失控保护用的，我们暂时不讨论失控保护。所以我们这里应该去找 "ardupilot/ArduCopter/motors.pde"。
* Set the flight control servos based on the current calculated values
*****************************************/
static void
set_servos_4()
{
// check if we are performing the motor test
if (ap.motor_test) {
motor_test_output();
} else {
#if FRAME_CONFIG == TRI_FRAME
// To-Do: implement improved stability patch for tri so that we do not need to limit throttle input to motors
g.rc_3.servo_out = min(g.rc_3.servo_out, 800);
#endif
motors.output();
}
}
void loop()
{
// Execute the fast loop
// ---------------------
fast_loop();
// 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
}
这样我们就看到了 motors.output函数是怎么被调用的。但是，这里似乎还漏掉了什么东西。是的，电机输出需要用到五个接口函数，那么，另外还有四个呢？
./ardupilot/libraries/AC_AttitudeControl/AC_AttitudeControl_Heli.cpp:137: _motors.set_pitch(_passthrough_pitch);
./ardupilot/libraries/AC_AttitudeControl/AC_AttitudeControl_Heli.cpp:228: _motors.set_pitch(pitch_out);
./ardupilot/libraries/AC_AttitudeControl/AC_AttitudeControl.cpp:403: _motors.set_pitch(rate_bf_to_motor_pitch(_rate_bf_target.y));
./ardupilot/libraries/AP_Motors/examples/AP_Motors_test/AP_Motors_test.pde:181: motors.set_pitch(roll_in);
./ardupilot/libraries/AP_Motors/examples/AP_Motors_test/AP_Motors_test.pde:204: motors.set_pitch(0);
radiolink@ubuntu:~/apm$
看到这个结果你肯定会觉得奇怪，不应该是这样的。但是别忘了，这里用的可是 C++！C++是可以使用指针的。
380: uint8_t motor_test : 1; // 16 // true if we are currently performing the motors test
451:static MOTOR_CLASS motors(g.rc_1, g.rc_2, g.rc_3, g.rc_4, g.rc_7, g.rc_8, g.heli_servo_1, g.heli_servo_2, g.heli_servo_3, g.heli_servo_4);
453:static MOTOR_CLASS motors(g.rc_1, g.rc_2, g.rc_3, g.rc_4, g.rc_7);
455:static MOTOR_CLASS motors(g.rc_1, g.rc_2, g.rc_3, g.rc_4, g.single_servo_1, g.single_servo_2, g.single_servo_3, g.single_servo_4);
457:static MOTOR_CLASS motors(g.rc_1, g.rc_2, g.rc_3, g.rc_4, g.single_servo_1, g.single_servo_2);
459:static MOTOR_CLASS motors(g.rc_1, g.rc_2, g.rc_3, g.rc_4);
635:AC_AttitudeControl_Heli attitude_control(ahrs, aparm, motors, g.p_stabilize_roll, g.p_stabilize_pitch, g.p_stabilize_yaw,
638:AC_AttitudeControl attitude_control(ahrs, aparm, motors, g.p_stabilize_roll, g.p_stabilize_pitch, g.p_stabilize_yaw,
641:AC_PosControl pos_control(ahrs, inertial_nav, motors, attitude_control,
773: { arm_motors_check, 40, 1 },
847: { arm_motors_check, 10, 10 },
1156: if (!motors.armed()) {
1161: motors.set_frame_orientation(g.frame_orientation);
1253: if (!motors.armed() && gps.can_calculate_base_pos()) {
1464: motors.ext_gyro_gain(g.rc_6.control_in);
radiolink@ubuntu:~/apm$
也就是它在两个结构中被使用， attitude_control和 pos_control。
// Attitude, Position and Waypoint navigation objects
// To-Do: move inertial nav up or other navigation variables down here
#if FRAME_CONFIG == HELI_FRAME
AC_AttitudeControl_Heli attitude_control(ahrs, aparm, motors, g.p_stabilize_roll, g.p_stabilize_pitch, g.p_stabilize_yaw,
g.pid_rate_roll, g.pid_rate_pitch, g.pid_rate_yaw);
#else
AC_AttitudeControl attitude_control(ahrs, aparm, motors, g.p_stabilize_roll, g.p_stabilize_pitch, g.p_stabilize_yaw,
g.pid_rate_roll, g.pid_rate_pitch, g.pid_rate_yaw);
#endif
AC_PosControl pos_control(ahrs, inertial_nav, motors, attitude_control,
g.p_alt_hold, g.p_throttle_rate, g.pid_throttle_accel,
g.p_loiter_pos, g.pid_loiter_rate_lat, g.pid_loiter_rate_lon);
宏 FRAME_CONFIG的定义我们前面已经看到了，是 QUAD_FRAME，所以 attitude_control的类型是 AC_AttitudeControl。应该来说， attitude_control是姿态控制的，那么 pos_control又是什么呢？而且我们看到 pos_control使用了 attitude_control这又是怎么怎么回事？这可是条大鱼，后面我们要专门花时间讨论。而现在，我们关心的是 motors。
./ardupilot/libraries/AC_AttitudeControl/AC_AttitudeControl_Heli.cpp:137: _motors.set_pitch(_passthrough_pitch);
./ardupilot/libraries/AC_AttitudeControl/AC_AttitudeControl_Heli.cpp:228: _motors.set_pitch(pitch_out);
./ardupilot/libraries/AC_AttitudeControl/AC_AttitudeControl.cpp:403: _motors.set_pitch(rate_bf_to_motor_pitch(_rate_bf_target.y));
radiolink@ubuntu:~/apm$
//
// rate_controller_run - run lowest level body-frame rate controller and send outputs to the motors
// should be called at 100hz or more
//
void AC_AttitudeControl::rate_controller_run()
{
// call rate controllers and send output to motors object
// To-Do: should the outputs from get_rate_roll, pitch, yaw be int16_t which is the input to the motors library?
// To-Do: skip this step if the throttle out is zero?
_motors.set_roll(rate_bf_to_motor_roll(_rate_bf_target.x));
_motors.set_pitch(rate_bf_to_motor_pitch(_rate_bf_target.y));
_motors.set_yaw(rate_bf_to_motor_yaw(_rate_bf_target.z));
}
// 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
所以最后我们还是跟踪到了 fast_loop。这是快速循环，前面我们已经分析过了，其周期是 2.5 ms。到这里可能我们又要问了， rate_bf_to_motor是什么函数？根据经验，这是用速率计算电机输出的函数。其中涉及到 PID算法，也只能后面在讲。
五个接口出来了四个，那应该还有一个。
./ardupilot/libraries/AC_AttitudeControl/AC_AttitudeControl.cpp:672: _motors.set_throttle(get_angle_boost(throttle_out));
./ardupilot/libraries/AC_AttitudeControl/AC_AttitudeControl.cpp:674: _motors.set_throttle(throttle_out);
radiolink@ubuntu:~/apm$
// set_throttle_out - to be called by upper throttle controllers when they wish to provide throttle output directly to motors
// provide 0 to cut motors
void AC_AttitudeControl::set_throttle_out(int16_t throttle_out, bool apply_angle_boost)
{
//设置油门...
if (apply_angle_boost) {
_motors.set_throttle(get_angle_boost(throttle_out));
}else{
_motors.set_throttle(throttle_out);
// clear angle_boost for logging purposes
_angle_boost = 0;
}
// update compass with throttle value
// To-Do: find another method to grab the throttle out and feed to the compass. Could be done completely outside this class
//compass.set_throttle((float)g.rc_3.servo_out/1000.0f);
}
radiolink@ubuntu:~/apm$ grep -nr "set_throttle_out" ./ardupilot/libraries/AC_AttitudeControl/
./ardupilot/libraries/AC_AttitudeControl/AC_PosControl.cpp:385: _attitude_control.set_throttle_out((int16_t)p+i+d+_throttle_hover, true);
./ardupilot/libraries/AC_AttitudeControl/AC_AttitudeControl.cpp:667: // set_throttle_out - to be called by upper throttle controllers when they wish to provide throttle output directly to motors
./ardupilot/libraries/AC_AttitudeControl/AC_AttitudeControl.cpp:669:void AC_AttitudeControl::set_throttle_out(int16_t throttle_out, bool apply_angle_boost)
./ardupilot/libraries/AC_AttitudeControl/AC_AttitudeControl.h:151: // set_throttle_out - to be called by upper throttle controllers when they wish to provide throttle output directly to motors
./ardupilot/libraries/AC_AttitudeControl/AC_AttitudeControl.h:153: void set_throttle_out(int16_t throttle_pwm, boolapply_angle_boost);
radiolink@ubuntu:~/apm$
这样我们就知道了，原来 AC_PosControl.cpp通过调用 _attitude_control来控制油门。实际上这里是控高。在 PX4中有各种需要定高的模式，其中就需要用高控高。如果你用 "grep -nr "set_throttle_out" ./ardupilot/ArduCopter/"命令去匹配你就会看到，有 control_stabilize.pde、 control_althold.pde等源文件，每一个这种源文件都实现了一种控制模式。比如这里列举的两种，前者是自稳模式，后者是定高模式。
油门控制会因为控制模式的不同而已不同的方式调用 AC_AttitudeControl::set_throttle_out函数，这个我们到控制模式中去专门分析。那么下面我们就去看下 RC_Channel又是怎么一回事呢，前面在 AP_Motors中我就很好奇，它跟遥控器有关系吗？不如一起去瞧瞧。
在前一篇笔记中我们把 RC_Channel一笔带过了。但其实如果你到 " ardupilot/libraries/RC_Channel/"目录下去看你会看到：
examples RC_Channel_aux.cpp RC_Channel_aux.h RC_Channel.cpp RC_Channel.h
radiolink@ubuntu:~/apm$
所以这里也有一个示例教你怎么使用 RC_Channel。完整路径为： " ardupilot/libraries/RC_Channel/examples/RC_Channel/RC_Channel.pde"。
#define CH_2 1
#define CH_3 2
#define CH_4 3
#define CH_5 4
#define CH_6 5
#define CH_7 6
#define CH_8 7
const AP_HAL::HAL& hal = AP_HAL_BOARD_DRIVER;
RC_Channel rc_1(CH_1);
RC_Channel rc_2(CH_2);
RC_Channel rc_3(CH_3);
RC_Channel rc_4(CH_4);
RC_Channel rc_5(CH_5);
RC_Channel rc_6(CH_6);
RC_Channel rc_7(CH_7);
RC_Channel rc_8(CH_8);
RC_Channel *rc = &rc_1;
实际上 RC_Channel很容易让人误以为它是遥控器的输入，但从前一篇笔记中我们看到其实不是。这一段就是定义了八个 RC_Channel变量，也就是可以处理八个通道数据。
{
hal.console->println("ArduPilot RC Channel test");
hal.scheduler->delay(500);
setup_radio();
print_radio_values();
// set type of output, symmetrical angles or a number range;
// XXX BROKEN
rc_1.set_angle(4500);
rc_1.set_default_dead_zone(80);
rc_1.set_type(RC_CHANNEL_TYPE_ANGLE_RAW);
// XXX BROKEN
rc_2.set_angle(4500);
rc_2.set_default_dead_zone(80);
rc_2.set_type(RC_CHANNEL_TYPE_ANGLE_RAW);
rc_3.set_range(0,1000);
rc_3.set_default_dead_zone(20);
rc_4.set_angle(6000);
rc_4.set_default_dead_zone(500);
rc_4.set_type(RC_CHANNEL_TYPE_ANGLE_RAW);
rc_5.set_range(0,1000);
rc_6.set_range(200,800);
rc_7.set_range(0,1000);
rc_8.set_range(0,1000);
for (int i = 0; i < 30; i++) {
read_radio();
}
rc_1.trim();
rc_2.trim();
rc_4.trim();
}
set_range函数跟 set_range函数前面我们已经接触过了，用来设置范围的，就不重复去看了。而 set_default_dead_zone函数根据经验是用来设置死区的。要证明这点其实也简单，我们只需跟踪代码即可。
{
if (!_dead_zone.load()) {
_dead_zone.set(abs(dzone));
}
}
radiolink@ubuntu:~/apm$ grep -nr _dead_zone ./ardupilot/libraries/RC_Channel/
./ardupilot/libraries/RC_Channel/examples/RC_Channel/RC_Channel.pde:71: rc_1.set_default_dead_zone(80);
./ardupilot/libraries/RC_Channel/examples/RC_Channel/RC_Channel.pde:75: rc_2.set_default_dead_zone(80);
./ardupilot/libraries/RC_Channel/examples/RC_Channel/RC_Channel.pde:79: rc_3.set_default_dead_zone(20);
./ardupilot/libraries/RC_Channel/examples/RC_Channel/RC_Channel.pde:82: rc_4.set_default_dead_zone(500);
./ardupilot/libraries/RC_Channel/RC_Channel.cpp:73: // Note: index 4 was used by the previous _dead_zone value. We
./ardupilot/libraries/RC_Channel/RC_Channel.cpp:84: AP_GROUPINFO("DZ", 5, RC_Channel, _dead_zone, 0),
./ardupilot/libraries/RC_Channel/RC_Channel.cpp:115:RC_Channel::set_default_dead_zone(int16_t dzone)
./ardupilot/libraries/RC_Channel/RC_Channel.cpp:117: if (!_dead_zone.load()) {
./ardupilot/libraries/RC_Channel/RC_Channel.cpp:118: _dead_zone.set(abs(dzone));
./ardupilot/libraries/RC_Channel/RC_Channel.cpp:241: int16_t radio_trim_low = radio_min + _dead_zone;
./ardupilot/libraries/RC_Channel/RC_Channel.cpp:258: _dead_zone.load();
./ardupilot/libraries/RC_Channel/RC_Channel.cpp:268: _dead_zone.save();
./ardupilot/libraries/RC_Channel/RC_Channel.cpp:315: return pwm_to_angle_dz(_dead_zone);
./ardupilot/libraries/RC_Channel/RC_Channel.cpp:358: return pwm_to_range_dz(_dead_zone);
./ardupilot/libraries/RC_Channel/RC_Channel.h:59: void set_default_dead_zone(int16_t dzone);
./ardupilot/libraries/RC_Channel/RC_Channel.h:143: AP_Int16 _dead_zone;
radiolink@ubuntu:~/apm$
稍加分析我们就能够发现我们要去查看的其实也就是那两个 return语句。
return an "angle in centidegrees" (normally -4500 to 4500) from
the current radio_in value
*/
int16_t RC_Channel::pwm_to_angle()
{
return pwm_to_angle_dz(_dead_zone);
}
/*
convert a pulse width modulation value to a value in the configured
range
*/
int16_t RC_Channel::pwm_to_range()
{
return pwm_to_range_dz(_dead_zone);
}
那么我们就列举其中一个函数来看看具体是如何使用的：
return an "angle in centidegrees" (normally -4500 to 4500) from
the current radio_in value using the specified dead_zone
*/
int16_t RC_Channel::pwm_to_angle_dz(uint16_t dead_zone)
{
int16_t radio_trim_high = radio_trim + dead_zone;
int16_t radio_trim_low = radio_trim - dead_zone;
// prevent div by 0
if ((radio_trim_low - radio_min) == 0 || (radio_max - radio_trim_high) == 0)
return 0;
if(radio_in > radio_trim_high) {
return _reverse * ((long)_high * (long)(radio_in - radio_trim_high)) / (long)(radio_max - radio_trim_high);
}else if(radio_in < radio_trim_low) {
return _reverse * ((long)_high * (long)(radio_in - radio_trim_low)) / (long)(radio_trim_low - radio_min);
}else
return 0;
}
稍加分析我们就会知道这是量程映射，而且其中有那么一段映射完后值始终为 0，这一段便是死区。那么最后还剩下三个函数： setup_radio、print_radio_values和 read_radio。
{
for (int i=0; i<8; i++) {
hal.console->printf("CH%u: %u|%u\n",
(unsigned)i+1,
(unsigned)rc[i].radio_min,
(unsigned)rc[i].radio_max);
}
}
你会发现这里打印的一个数据格式跟我们校准成功时候的信息是一致的，如：
这是我们校准成功时的一个截图，那么我就有理由认为 hal.console就是跟上位机通信的那个串口。下面我们来看另外一个函数：
{
hal.console->println("\n\nRadio Setup:");
uint8_t i;
for(i = 0; i < 100;i++){
hal.scheduler->delay(20);
read_radio();
}
rc_1.radio_min = rc_1.radio_in;
rc_2.radio_min = rc_2.radio_in;
rc_3.radio_min = rc_3.radio_in;
rc_4.radio_min = rc_4.radio_in;
rc_5.radio_min = rc_5.radio_in;
rc_6.radio_min = rc_6.radio_in;
rc_7.radio_min = rc_7.radio_in;
rc_8.radio_min = rc_8.radio_in;
rc_1.radio_max = rc_1.radio_in;
rc_2.radio_max = rc_2.radio_in;
rc_3.radio_max = rc_3.radio_in;
rc_4.radio_max = rc_4.radio_in;
rc_5.radio_max = rc_5.radio_in;
rc_6.radio_max = rc_6.radio_in;
rc_7.radio_max = rc_7.radio_in;
rc_8.radio_max = rc_8.radio_in;
rc_1.radio_trim = rc_1.radio_in;
rc_2.radio_trim = rc_2.radio_in;
rc_4.radio_trim = rc_4.radio_in;
// 3 is not trimed
rc_5.radio_trim = 1500;
rc_6.radio_trim = 1500;
rc_7.radio_trim = 1500;
rc_8.radio_trim = 1500;
hal.console->println("\nMove all controls to each extreme. Hit Enter to save:");
while(1){
hal.scheduler->delay(20);
// Filters radio input - adjust filters in the radio.pde file
// ----------------------------------------------------------
read_radio();
rc_1.update_min_max();
rc_2.update_min_max();
rc_3.update_min_max();
rc_4.update_min_max();
rc_5.update_min_max();
rc_6.update_min_max();
rc_7.update_min_max();
rc_8.update_min_max();
if(hal.console->available() > 0) {
hal.console->println("Radio calibrated, Showing control values:");
break;
}
}
return;
}
这个函数虽然不短，但它其实就只是获取最大值和最小值。说白了这就是用来校准遥控器的。而 read_radio函数就是用来读取遥控器数据。但我们知道，连接遥控器的是 io，所以 read_radio肯定是通过跟 io通信的方式获取数据的。但这里有个地方让我觉得很奇怪，为什么 3通道没有 "radio_trim"？结合 pwm_to_angle_dz函数我觉得比较合理的解释是 " radio_trim"是通道的中点，而三通是油门通道，不需要对中点进行处理，故而不需要中点。
{
rc_1.set_pwm(hal.rcin->read(CH_1));
rc_2.set_pwm(hal.rcin->read(CH_2));
rc_3.set_pwm(hal.rcin->read(CH_3));
rc_4.set_pwm(hal.rcin->read(CH_4));
rc_5.set_pwm(hal.rcin->read(CH_5));
rc_6.set_pwm(hal.rcin->read(CH_6));
rc_7.set_pwm(hal.rcin->read(CH_7));
rc_8.set_pwm(hal.rcin->read(CH_8));
}
前面我们在 AP_Motors_test函数中看到给 RC_Channel传值是通过 servo_out进行的，但这里我们看到了另外一种方式，即 set_pwm函数。
void RC_Channel::set_pwm(int16_t pwm)
{
radio_in = pwm;
if (_type == RC_CHANNEL_TYPE_RANGE) {
control_in = pwm_to_range();
} else {
//RC_CHANNEL_TYPE_ANGLE, RC_CHANNEL_TYPE_ANGLE_RAW
control_in = pwm_to_angle();
}
}
这里调用的 pwm_to_range跟 pwm_to_angle函数其实就是我们前面看到的，在这里其实就是处理死区跟映射。如果是三通最后是调用 pwm_to_range_dz函数来完成的。
convert a pulse width modulation value to a value in the configured
range, using the specified deadzone
*/
int16_t RC_Channel::pwm_to_range_dz(uint16_t dead_zone)
{
int16_t r_in = constrain_int16(radio_in, radio_min.get(), radio_max.get());
if (_reverse == -1) {
r_in = radio_max.get() - (r_in - radio_min.get());
}
int16_t radio_trim_low = radio_min + dead_zone;
if (r_in > radio_trim_low)
return (_low + ((long)(_high - _low) * (long)(r_in - radio_trim_low)) / (long)(radio_max - radio_trim_low));
else if (dead_zone > 0)
return 0;
else
return _low;
}
经过对比我们看到该函数只是把数据映射到 [_high - _low]， 但并没有使用中点数据。那么我们很容易想到 "hal.rcin->read"函数就是用来读遥控器数据的了。那么这个我们还得从 hal说起。
2. hal
如果我们回到 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 */
{}
static PX4RCInput rcinDriver;
我们看到其中有一个 "rcinput"，它其实就是 "hal.rcin"。类定义为：
class PX4::PX4RCInput : public AP_HAL::RCInput {
public:
void init(void* machtnichts);
bool new_input();
uint8_t num_channels();
uint16_t read(uint8_t ch);
uint8_t read(uint16_t* periods, uint8_t len);
bool set_overrides(int16_t *overrides, uint8_t len);
bool set_override(uint8_t channel, int16_t override);
void clear_overrides();
void _timer_tick(void);
private:
/* override state */
uint16_t _override[RC_INPUT_MAX_CHANNELS];
struct rc_input_values _rcin;
int _rc_sub;
uint64_t _last_read;
bool _override_valid;
perf_counter_t _perf_rcin;
pthread_mutex_t rcin_mutex;
};
我们看到它其实有两个 read函数。我们也需要注意这里这个头文件： " pthread.h"，区区一个头文件为什么值得特别注意呢？因为该头文件表明这个类会去创建任务。其中 _timer_tick函数应该就是这个任务需要执行的函数。而由于该函数不是私有的，所以它可能是由外部进行调用的。
那么下面我们就不妨去看看这两个 read函数。
{
if (ch >= RC_INPUT_MAX_CHANNELS) {
return 0;
}
pthread_mutex_lock(&rcin_mutex);
_last_read = _rcin.timestamp_last_signal;
_override_valid = false;
if (_override[ch]) {
uint16_t v = _override[ch];
pthread_mutex_unlock(&rcin_mutex);
return v;
}
if (ch >= _rcin.channel_count) {
pthread_mutex_unlock(&rcin_mutex);
return 0;
}
uint16_t v = _rcin.values[ch];
pthread_mutex_unlock(&rcin_mutex);
return v;
}
uint8_t PX4RCInput::read(uint16_t* periods, uint8_t len)
{
if (len > RC_INPUT_MAX_CHANNELS) {
len = RC_INPUT_MAX_CHANNELS;
}
for (uint8_t i = 0; i < len; i++){
periods[i] = read(i);
}
return len;
}
最后数据来自 _override跟 _rcin.values，那这两个家伙又是什么呢？我们一个个来跟踪。
{
bool res = false;
for (uint8_t i = 0; i < len; i++) {
res |= set_override(i, overrides[i]);
}
return res;
}
bool
if (override < 0) {
return false; /* -1: no change. */
}
if (channel >= RC_INPUT_MAX_CHANNELS) {
return false;
}
_override[channel] = override;
if (override != 0) {
_override_valid = true;
return true;
}
return false;
}
./ardupilot/ArduCopter/GCS_Mavlink.pde:1046: hal.rcin->set_overrides(v, 8);
radiolink@ubuntu:~/apm$
所以这个就是校准的时候获取的数据。由于我们是从 io获取数据，所以我们的数据来自 _rcin.values。它是在 _timer_tick函数中把数据放进去的：
{
perf_begin(_perf_rcin);
bool rc_updated = false;
if (orb_check(_rc_sub, &rc_updated) == 0 && rc_updated) {
pthread_mutex_lock(&rcin_mutex);
orb_copy(ORB_ID(input_rc), _rc_sub, &_rcin);
pthread_mutex_unlock(&rcin_mutex);
}
// note, we rely on the vehicle code checking new_input()
// and a timeout for the last valid input to handle failsafe
perf_end(_perf_rcin);
}
折腾了半天又是 ORB。但 ORB究竟是什么我却始终未能参透。
{
int ret;
ret = read(handle, buffer, meta->o_size);
if (ret < 0)
return ERROR;
if (ret != (int)meta->o_size) {
errno = EIO;
return ERROR;
}
return OK;
}
从 orb_copy函数使用 read接口操作 _rc_sub来看_rc_sub应该是一个文件描述符。
{
_perf_rcin = perf_alloc(PC_ELAPSED, "APM_rcin");
_rc_sub = orb_subscribe(ORB_ID(input_rc));
if (_rc_sub == -1) {
hal.scheduler->panic("Unable to subscribe to input_rc");
}
clear_overrides();
pthread_mutex_init(&rcin_mutex, NULL);
}
int
{
return node_open(PUBSUB, meta, nullptr, false);
}
* Common implementation for orb_advertise and orb_subscribe.
*
* Handles creation of the object and the initial publication for
* advertisers.
*/
int node_open(Flavor f, const struct orb_metadata *meta, const void *data, bool advertiser)
{
char path[orb_maxpath];
int fd, ret;
/*
* If meta is null, the object was not defined, i.e. it is not
* known to the system. We can't advertise/subscribe such a thing.
*/
if (nullptr == meta) {
errno = ENOENT;
return ERROR;
}
/*
* Advertiser must publish an initial value.
*/
if (advertiser && (data == nullptr)) {
errno = EINVAL;
return ERROR;
}
/*
* Generate the path to the node and try to open it.
*/
ret = node_mkpath(path, f, meta);
if (ret != OK) {
errno = -ret;
return ERROR;
}
/* open the path as either the advertiser or the subscriber */
fd = open(path, (advertiser) ? O_WRONLY : O_RDONLY);
/* we may need to advertise the node... */
if (fd < 0) {
/* try to create the node */
ret = node_advertise(meta);
/* on success, try the open again */
if (ret == OK)
fd = open(path, (advertiser) ? O_WRONLY : O_RDONLY);
}
if (fd < 0) {
errno = EIO;
return ERROR;
}
/* everything has been OK, we can return the handle now */
return fd;
}
代码跟踪到这里我们发现它直接就去 open了一个文件(应该是设备文件)，但总不可能凭空产生一个文件吧，而且就算是凭空产生了一个文件，那 read操作也不可能成功啊。而且这里面为什么要两次open操作？ node_advertise函数有到底有什么密码呢？根据注释 " try to create the node"我觉得 node_advertise函数应该会去创建一个 node，但 node指的是设备文件吗？
* Advertise a node; don't consider it an error if the node has
* already been advertised.
*
* @todo verify that the existing node is the same as the one
* we tried to advertise.
*/
int node_advertise(const struct orb_metadata *meta)
{
int fd = -1;
int ret = ERROR;
/* open the control device */
fd = open(TOPIC_MASTER_DEVICE_PATH, 0);
if (fd < 0)
goto out;
/* advertise the object */
ret = ioctl(fd, ORBIOCADVERTISE, (unsigned long)(uintptr_t)meta);
/* it's OK if it already exists */
if ((OK != ret) && (EEXIST == errno))
ret = OK;
out:
if (fd >= 0)
close(fd);
return ret;
}
真是一波未平一波又起啊，旧的文件都没搞明白，新的文件又出现了。而且貌似这里也没有去创建设备文件啊。那难道是由 TOPIC_MASTER_DEVICE_PATH这个设备来完成的吗？也只有这样能够解释得通了。详细的我们就后面再来专门讨论了。
那么以上就是读数据的大体过程了。
3. loop
loop的代码不多，只有几行语句：
{
hal.scheduler->delay(20);
debug_rcin();
read_radio();
print_pwm();
}
其中 read_radio函数我们前面已经看过了， debug_rcin函数跟 print_pwm函数现在来看就简单多了。瞧：
uint16_t channels[8];
hal.rcin->read(channels, 8);
hal.console->printf_P(
PSTR("rcin: %u %u %u %u %u %u %u %u\r\n"),
channels[0],
channels[1],
channels[2],
channels[3],
channels[4],
channels[5],
channels[6],
channels[7]);
}
void
{
for (int i=0; i<8; i++) {
hal.console->printf("ch%u: %4d ", (unsigned)i+1, (int)rc[i].control_in);
}
hal.console->printf("\n");
}
两个函数都是在向上位机传递数据，区别在于 debug_rcin传的是原始数据， print_pwm传的是映射后的数据。
关于这个测试程序就这命多了，下面我们还是赶紧来看看 ORB到底是怎么回事吧。
PX4 FMU [13] ORB
刚刚我们在 "rcinput"中又一次遇到了 ORB，现在，我们就来详细看一下 ORB到底是怎么回事。
那么我们就从 " _rc_sub = orb_subscribe(ORB_ID(input_rc));"这条语句说起。
* Generates a pointer to the uORB metadata structure for
* a given topic.
*
* The topic must have been declared previously in scope
* with ORB_DECLARE().
*
* @param _name The name of the topic.
*/
#define ORB_ID(_name) &__orb_##_name
int
{
return node_open(PUBSUB, meta, nullptr, false);
}
若是我们进行宏展开，实际上实参就变成了： "__orb_input_rc"，那么我们就需要找到它是在哪里定义的。
./PX4Firmware/src/drivers/drv_rc_input.h:55: * Input data may be obtained by subscribing to the input_rc
./PX4Firmware/src/drivers/drv_rc_input.h:58:#define RC_INPUT_DEVICE_PATH "/dev/input_rc"
./PX4Firmware/src/drivers/drv_rc_input.h:93: * Published to input_rc, may also be published to other names depending
./PX4Firmware/src/drivers/drv_rc_input.h:157:ORB_DECLARE(input_rc);
/*
* ObjDev tag for R/C inputs.
*/
ORB_DECLARE(input_rc);
/**
* Declare (prototype) the uORB metadata for a topic.
*
* Note that optional topics are declared weak; this allows a potential
* subscriber to attempt to subscribe to a topic that is not known to the
* system at runtime. The ORB_ID() macro will return NULL/nullptr for
* such a topic, and attempts to advertise or subscribe to it will
* return -1/ENOENT (see below).
*
* @param _name The name of the topic.
*/
#if defined(__cplusplus)
# define ORB_DECLARE(_name) extern "C" const struct orb_metadata __orb_##_name __EXPORT
# define ORB_DECLARE_OPTIONAL(_name) extern "C"const struct orb_metadata __orb_##_name __EXPORT __attribute__((weak))
#else
# define ORB_DECLARE(_name) extern const struct orb_metadata __orb_##_name __EXPORT
# define ORB_DECLARE_OPTIONAL(_name) extern const struct orb_metadata __orb_##_name __EXPORT __attribute__((weak))
#endif
所以我们绝对不可以直接匹配 "__orb_input_rc"，因为它是通过宏进行定义的。那么 " struct orb_metadata"又包含了哪些数据呢，我们不妨来看看。
* Object metadata.
*/
struct orb_metadata {
const char *o_name; /**< unique object name */
const size_t o_size; /**< object size */
};
"o_name"还是比较好理解的，但 "o_size"就不一样了，我们需要知道 "object"到底是哪个才好理解。这就需要我们深入去分析 node_open函数了。
关于 ORB究竟指的是什么，我后来是偶然间从注释中找到了答案。
./PX4Firmware/src/modules/uORB/topics/airspeed.h:66:ORB_DECLARE(airspeed);
./PX4Firmware/src/modules/uORB/topics/battery_status.h:67:ORB_DECLARE(battery_status);
./PX4Firmware/src/modules/uORB/topics/debug_key_value.h:74:ORB_DECLARE(debug_key_value);
/* register this as object request broker structure */
ORB_DECLARE(airspeed);
所以我认为 ORB指的是： "object request broker"，我觉得应该是“对象请求代理”的意思。
现在我们就来跟踪 node_open函数。
* Common implementation for orb_advertise and orb_subscribe.
*
* Handles creation of the object and the initial publication for
* advertisers.
*/
int node_open(Flavor f, const struct orb_metadata *meta,const void *data, bool advertiser)
{
char path[orb_maxpath];
int fd, ret;
/*
* If meta is null, the object was not defined, i.e. it is not
* known to the system. We can't advertise/subscribe such a thing.
*/
if (nullptr == meta) {
errno = ENOENT;
return ERROR;
}
/*
* Advertiser must publish an initial value.
*/
if (advertiser && (data == nullptr)) {
errno = EINVAL;
return ERROR;
}
/*
* Generate the path to the node and try to open it.
*/
ret = node_mkpath(path, f, meta);
if (ret != OK) {
errno = -ret;
return ERROR;
}
/* open the path as either the advertiser or the subscriber */
fd = open(path, (advertiser) ? O_WRONLY : O_RDONLY);
/* we may need to advertise the node... */
if (fd < 0) {
/* try to create the node */
ret = node_advertise(meta);
/* on success, try the open again */
if (ret == OK)
fd = open(path, (advertiser) ? O_WRONLY : O_RDONLY);
}
if (fd < 0) {
errno = EIO;
return ERROR;
}
/* everything has been OK, we can return the handle now */
return fd;
}
/**
* Advertise a node; don't consider it an error if the node has
* already been advertised.
*
* @todo verify that the existing node is the same as the one
* we tried to advertise.
*/
int node_advertise( const struct orb_metadata *meta)
{
int fd = -1;
int ret = ERROR;
/* open the control device */
fd = open(TOPIC_MASTER_DEVICE_PATH, 0);
if (fd < 0)
goto out;
/* advertise the object */
ret = ioctl(fd, ORBIOCADVERTISE, ( unsigned long)(uintptr_t)meta);
/* it's OK if it already exists */
if ((OK != ret) && (EEXIST == errno))
ret = OK;
out:
if (fd >= 0)
close(fd);
return ret;
}
我当然是很希望快点去看看 TOPIC_MASTER_DEVICE_PATH中究竟有什么名堂，但在那之前我们应该先搞清楚 path具体的值是多少，也就是 node_mkpath函数。
{
unsigned len;
len = snprintf(buf, orb_maxpath, "/%s/%s",
(f == PUBSUB) ? "obj" : "param",
meta->o_name);
if (len >= orb_maxpath)
return -ENAMETOOLONG;
return OK;
}
前面我们传下来的参数是： PUBSUB。所以我们最后的路径是：" /obj/o_name"。那么问题就在于meta->o_name到底是多少。于是我回过头去看，发现原来 ORB_DECLARE没有给 o_name赋值，而我们如果进行匹配：
./PX4Firmware/src/modules/uORB/uORB.cpp:91: meta->o_name);
./PX4Firmware/src/modules/uORB/uORB.cpp:571: objname = strdup(meta->o_name);
./PX4Firmware/src/modules/uORB/uORB.h:53: const char *o_name; /**< unique object name */
radiolink@ubuntu:~/apm$
没有找到给 o_name赋值的语句。既然没有单独赋值，那只能是在定义的时候赋值，所以 ORB_DECLARE只是声明！那么真正的定义又在哪里呢？而且赋值的时候又不是通过o_name很大一个可能是通过宏来初始化的。
./PX4Firmware/src/modules/uORB/objects_common.cpp:74:ORB_DEFINE(input_rc, struct rc_input_values);
/**
* Define (instantiate) the uORB metadata for a topic.
*
* The uORB metadata is used to help ensure that updates and
* copies are accessing the right data.
*
* Note that there must be no more than one instance of this macro
* for each topic.
*
* @param _name The name of the topic.
* @param _struct The structure the topic provides.
*/
#define ORB_DEFINE(_name, _struct) \
const struct orb_metadata __orb_##_name = { \
#_name, \
sizeof(_struct) \
}; struct hack
所以这里才是 "__orb_input_rc"真正的初始化。
好了，现在我们得去会会 TOPIC_MASTER_DEVICE_PATH了。
2.ORBDevMaster
我们还是先来看看 TOPIC_MASTER_DEVICE_PATH的具体路径：
./PX4Firmware/src/modules/uORB/uORB.cpp:540: (f == PUBSUB) ? TOPIC_MASTER_DEVICE_PATH : PARAM_MASTER_DEVICE_PATH),
./PX4Firmware/src/modules/uORB/uORB.cpp:846: fd = open(TOPIC_MASTER_DEVICE_PATH, 0);
./PX4Firmware/src/drivers/drv_orb_dev.h:53:#define TOPIC_MASTER_DEVICE_PATH "/obj/_obj_"
radiolink@ubuntu:~/apm$
nsh> ls /obj
/obj:
_obj_
我们看到板子上也确实存在 "/obj/_obj_"这个文件。在我们的匹配结果中， 540行代码这个地方就应该是创建设备文件。
CDev((f == PUBSUB) ? "obj_master" : "param_master",
(f == PUBSUB) ? TOPIC_MASTER_DEVICE_PATH : PARAM_MASTER_DEVICE_PATH),
_flavor(f)
{
// enable debug() calls
_debug_enabled = true;
}
/**
* Master control device for ObjDev.
*
* Used primarily to create new objects via the ORBIOCCREATE
* ioctl.
*/
class ORBDevMaster : public device::CDev
{
public:
ORBDevMaster(Flavor f);
~ORBDevMaster();
virtual int ioctl(struct file *filp,int cmd, unsigned long arg);
private:
Flavor _flavor;
};
这个地方刚好是 ORBDevMaster类的构造函数，而且该类继承自 CDev，重写了一个 ioctl接口。
但是构造函数显然应该是在创建对象的时候被调用，所以事情还没完的。
./PX4Firmware/src/modules/uORB/uORB.cpp:527:class ORBDevMaster :public device::CDev
./PX4Firmware/src/modules/uORB/uORB.cpp:530: ORBDevMaster(Flavor f);
./PX4Firmware/src/modules/uORB/uORB.cpp:531: ~ORBDevMaster();
./PX4Firmware/src/modules/uORB/uORB.cpp:538:ORBDevMaster::ORBDevMaster(Flavor f) :
./PX4Firmware/src/modules/uORB/uORB.cpp:548:ORBDevMaster::~ORBDevMaster()
./PX4Firmware/src/modules/uORB/uORB.cpp:553:ORBDevMaster::ioctl(struct file *filp,int cmd, unsigned long arg)
./PX4Firmware/src/modules/uORB/uORB.cpp:610:ORBDevMaster *g_dev;
./PX4Firmware/src/modules/uORB/uORB.cpp:780: g_dev = new ORBDevMaster(PUBSUB);
radiolink@ubuntu:~/apm$
int uorb_main(int argc, char *argv[])
{
/*
* Start/load the driver.
*
* XXX it would be nice to have a wrapper for this...
*/
if (!strcmp(argv[1], "start")) {
if (g_dev != nullptr) {
fprintf(stderr, "[uorb] already loaded\n");
/* user wanted to start uorb, its already running, no error */
return 0;
}
/* create the driver */
g_dev = new ORBDevMaster(PUBSUB);
if (g_dev == nullptr) {
fprintf(stderr, "[uorb] driver alloc failed\n");
return -ENOMEM;
}
if (OK != g_dev->init()) {
fprintf(stderr, "[uorb] driver init failed\n");
delete g_dev;
g_dev = nullptr;
return -EIO;
}
printf("[uorb] ready\n");
return OK;
}
/*
* Test the driver/device.
*/
if (!strcmp(argv[1], "test"))
return test();
/*
* Print driver information.
*/
if (!strcmp(argv[1], "status"))
return info();
fprintf(stderr, "unrecognised command, try 'start', 'test' or 'status'\n");
return -EINVAL;
}
好家伙，又是一个 main函数，看来 uorb又是一个命令。而我们确实也可以在 g_builtins数组跟启动脚本中找到这个命令。
{NULL, 0, 0, NULL}
if uorb start
then
echo "uorb started OK"
else
sh /etc/init.d/rc.error
fi
if [ -f /fs/microsd/APM/mkblctrl ]
then
echo "Setting up mkblctrl driver"
echo "Setting up mkblctrl driver" >> $logfile
mkblctrl -d /dev/pwm_output
fi
这样就差那个 ioctl命令了。
int ORBDevMaster::ioctl(struct file *filp, int cmd, unsigned long arg)
{
int ret;
switch (cmd) {
case ORBIOCADVERTISE: {
const struct orb_metadata *meta = (const struct orb_metadata *)arg;
const char *objname;
char nodepath[orb_maxpath];
ORBDevNode *node;
/* construct a path to the node - this also checks the node name */
ret = node_mkpath(nodepath, _flavor, meta);
if (ret != OK)
return ret;
/* driver wants a permanent copy of the node name, so make one here */
objname = strdup(meta->o_name);
if (objname == nullptr)
return -ENOMEM;
/* construct the new node */
node = new ORBDevNode(meta, objname, nodepath);
/* initialise the node - this may fail if e.g. a node with this name already exists */
if (node != nullptr)
ret = node->init();
/* if we didn't get a device, that's bad */
if (node == nullptr)
return -ENOMEM;
/* if init failed, discard the node and its name */
if (ret != OK) {
delete node;
free((void *)objname);
}
return ret;
}
default:
/* give it to the superclass */
return CDev::ioctl(filp, cmd, arg);
}
}
ORBDevNode::ORBDevNode(const struct orb_metadata *meta, const char *name, const char *path) :
CDev(name, path),
_meta(meta),
_data(nullptr),
_last_update(0),
_generation(0),
_publisher(0)
{
// enable debug() calls
_debug_enabled = true;
}
所以最终将由 ORBDevNode类创建这个设备。换句话说每一个 orb_metadata设备都对应了一个 ORBDevNode对象。
我们在回到读数据：
{
perf_begin(_perf_rcin);
bool rc_updated = false;
if (orb_check(_rc_sub, &rc_updated) == 0 && rc_updated) {
pthread_mutex_lock(&rcin_mutex);
orb_copy(ORB_ID(input_rc), _rc_sub, &_rcin);
pthread_mutex_unlock(&rcin_mutex);
}
// note, we rely on the vehicle code checking new_input()
// and a timeout for the last valid input to handle failsafe
perf_end(_perf_rcin);
}
int
{
int ret;
ret = read(handle, buffer, meta->o_size);
if (ret < 0)
return ERROR;
if (ret != (int)meta->o_size) {
errno = EIO;
return ERROR;
}
return OK;
}
实际上这里的系统调用 read最终调用的是 ORBDevNode::read函数，若是 ORBDevNode类没有提供 read方法则根据 C++继承关系由父类提供。但在这里是提供了的。
{
SubscriberData *sd = (SubscriberData *)filp_to_sd(filp);
/* if the object has not been written yet, return zero */
if (_data == nullptr)
return 0;
/* if the caller's buffer is the wrong size, that's an error */
if (buflen != _meta->o_size)
return -EIO;
/*
* Perform an atomic copy & state update
*/
irqstate_t flags = irqsave();
/* if the caller doesn't want the data, don't give it to them */
if (nullptr != buffer)
memcpy(buffer, _data, _meta->o_size);
/* track the last generation that the file has seen */
sd->generation = _generation;
/*
* Clear the flag that indicates that an update has been reported, as
* we have just collected it.
*/
sd->update_reported = false;
irqrestore(flags);
return _meta->o_size;
}
这样我们就知道最终数据是来自 _data。那么有数据出来即必定有数据进去。我们很容易想到是 write函数把数据放进去的，但这不是问题的关键，关键在于谁通过 write函数把数据放进去的。
3.PX4IO
根据经验，写数据应该是通过 "ORB_ID(input_rc)"进行操作的，于是：
./PX4Firmware/src/systemcmds/tests/test_rc.c:66: int _rc_sub = orb_subscribe(ORB_ID(input_rc));
./PX4Firmware/src/systemcmds/tests/test_rc.c:71: orb_copy(ORB_ID(input_rc), _rc_sub, &rc_input);
./PX4Firmware/src/systemcmds/tests/test_rc.c:106: orb_copy(ORB_ID(input_rc), _rc_sub, &rc_input);
./PX4Firmware/src/systemcmds/tests/test_ppm_loopback.c:65: int _rc_sub = orb_subscribe(ORB_ID(input_rc));
./PX4Firmware/src/systemcmds/tests/test_ppm_loopback.c:135: orb_copy(ORB_ID(input_rc), _rc_sub, &rc_input);
./PX4Firmware/src/systemcmds/tests/test_ppm_loopback.c:145: orb_copy(ORB_ID(input_rc), _rc_sub, &rc_input);
./PX4Firmware/src/modules/mavlink/mavlink_messages.cpp:1668: _rc_sub(_mavlink->add_orb_subscription(ORB_ID(input_rc))),
./PX4Firmware/src/modules/sensors/sensors.cpp:1564: orb_copy(ORB_ID(input_rc), _rc_sub, &rc_input);
./PX4Firmware/src/modules/sensors/sensors.cpp:1753: _rc_sub = orb_subscribe(ORB_ID(input_rc));
./PX4Firmware/src/drivers/px4io/px4io.cpp:1655: _to_input_rc = orb_advertise(ORB_ID(input_rc), &rc_val);
./PX4Firmware/src/drivers/px4io/px4io.cpp:1658: orb_publish(ORB_ID(input_rc), _to_input_rc, &rc_val);
./PX4Firmware/src/drivers/px4fmu/fmu.cpp:737: to_input_rc = orb_advertise(ORB_ID(input_rc), &rc_in);
./PX4Firmware/src/drivers/px4fmu/fmu.cpp:740: orb_publish(ORB_ID(input_rc), to_input_rc, &rc_in);
radiolink@ubuntu:~/apm$
但我们并没有如我们所预期的那样看到调用 write函数。但是我们可以先对结果进行排除。我们知道数据是由 io接收上来的，并通过串口发送给 fmu，而前面我们还知道，在 fmu中负责跟 io通信的是 px4io模块，那么最终我们可以确定有效信息只有两条：
./PX4Firmware/src/drivers/px4io/px4io.cpp:1658: orb_publish(ORB_ID(input_rc), _to_input_rc, &rc_val);
int PX4IO::io_publish_raw_rc()
{
/* fetch values from IO */
rc_input_values rc_val;
/* set the RC status flag ORDER MATTERS! */
rc_val.rc_lost = !(_status & PX4IO_P_STATUS_FLAGS_RC_OK);
int ret = io_get_raw_rc_input(rc_val);
if (ret != OK)
return ret;
/* sort out the source of the values */
if (_status & PX4IO_P_STATUS_FLAGS_RC_PPM) {
rc_val.input_source = RC_INPUT_SOURCE_PX4IO_PPM;
} else if (_status & PX4IO_P_STATUS_FLAGS_RC_DSM) {
rc_val.input_source = RC_INPUT_SOURCE_PX4IO_SPEKTRUM;
} else if (_status & PX4IO_P_STATUS_FLAGS_RC_SBUS) {
rc_val.input_source = RC_INPUT_SOURCE_PX4IO_SBUS;
} else {
rc_val.input_source = RC_INPUT_SOURCE_UNKNOWN;
/* we do not know the RC input, only publish if RC OK flag is set */
/* if no raw RC, just don't publish */
if (!(_status & PX4IO_P_STATUS_FLAGS_RC_OK))
return OK;
}
/* lazily advertise on first publication */
if (_to_input_rc == 0) {
_to_input_rc = orb_advertise(ORB_ID(input_rc), &rc_val);
} else {
orb_publish(ORB_ID(input_rc), _to_input_rc, &rc_val);
}
return OK;
}
从源码中我们看到， orb_advertise函数正常情况下只会调用一次，所以 orb_publish函数才是把数据写入到设备文件中的。
{
return ORBDevNode::publish(meta, handle, data);
}
ssize_t ORBDevNode::publish(const orb_metadata *meta, orb_advert_t handle,const void *data)
{
ORBDevNode *devnode = (ORBDevNode *)handle;
int ret;
/* this is a bit risky, since we are trusting the handle in order to deref it */
if (devnode->_meta != meta) {
errno = EINVAL;
return ERROR;
}
/* call the devnode write method with no file pointer */
ret = devnode->write(nullptr, (const char *)data, meta->o_size);
if (ret < 0)
return ERROR;
if (ret != (int)meta->o_size) {
errno = EIO;
return ERROR;
}
return OK;
}
所以它是通过 publish函数来调用 write函数的。但是它还引用了一个 handle指针。而该指针正是 _to_input_rc。那么我们就非得去看看 orb_advertise函数不可了。
{
int result, fd;
orb_advert_t advertiser;
/* open the node as an advertiser */
fd = node_open(PUBSUB, meta, data, true);
if (fd == ERROR)
return ERROR;
/* get the advertiser handle and close the node */
result = ioctl(fd, ORBIOCGADVERTISER, (unsigned long)&advertiser);
close(fd);
if (result == ERROR)
return ERROR;
/* the advertiser must perform an initial publish to initialise the object */
result= orb_publish(meta, advertiser, data);
if (result == ERROR)
return ERROR;
return advertiser;
}
int ORBDevNode::ioctl(struct file *filp, int cmd, unsigned long arg)
{
SubscriberData *sd = filp_to_sd(filp);
switch (cmd) {
/* ... */
case ORBIOCGADVERTISER:
*(uintptr_t *)arg = (uintptr_t)this;
return OK;
default:
/* give it to the superclass */
return CDev::ioctl(filp, cmd, arg);
}
}
看完这段代码真心觉得是不是有点多此一举啊，通过 ORB_ID(input_rc)直接就可以调用到 write函数还非得插个指针进去。直接调连publish函数都省了。
现在还有一个问题没解决，就是 PX4IO是怎么读数据的。在 "io_publish_raw_rc"函数中我们看到是把 rc_val中的数据写入到 ORB_ID(input_rc)中，那么读数据就应该是 io_get_raw_rc_input函数来完成的。
{
uint32_t channel_count;
int ret;
/* we don't have the status bits, so input_source has to be set elsewhere */
input_rc.input_source = RC_INPUT_SOURCE_UNKNOWN;
static const unsigned prolog = (PX4IO_P_RAW_RC_BASE - PX4IO_P_RAW_RC_COUNT);
uint16_t regs[RC_INPUT_MAX_CHANNELS + prolog];
/*
* Read the channel count and the first 9 channels.
*
* This should be the common case (9 channel R/C control being a reasonable upper bound).
*/
input_rc.timestamp_publication = hrt_absolute_time();
ret = io_reg_get(PX4IO_PAGE_RAW_RC_INPUT, PX4IO_P_RAW_RC_COUNT, ®s[0], prolog + 9);
if (ret != OK)
return ret;
/*
* Get the channel count any any extra channels. This is no more expensive than reading the
* channel count once.
*/
channel_count = regs[PX4IO_P_RAW_RC_COUNT];
if (channel_count != _rc_chan_count)
perf_count(_perf_chan_count);
_rc_chan_count = channel_count;
input_rc.rc_ppm_frame_length = regs[PX4IO_P_RAW_RC_DATA];
input_rc.rssi = regs[PX4IO_P_RAW_RC_NRSSI];
input_rc.rc_failsafe = (regs[PX4IO_P_RAW_RC_FLAGS] & PX4IO_P_RAW_RC_FLAGS_FAILSAFE);
input_rc.rc_lost_frame_count = regs[PX4IO_P_RAW_LOST_FRAME_COUNT];
input_rc.rc_total_frame_count = regs[PX4IO_P_RAW_FRAME_COUNT];
/* rc_lost has to be set before the call to this function */
if (!input_rc.rc_lost && !input_rc.rc_failsafe)
_rc_last_valid = input_rc.timestamp_publication;
input_rc.timestamp_last_signal = _rc_last_valid;
if (channel_count > 9) {
ret = io_reg_get(PX4IO_PAGE_RAW_RC_INPUT, PX4IO_P_RAW_RC_BASE + 9, ®s[prolog + 9], channel_count - 9);
if (ret != OK)
return ret;
}
input_rc.channel_count = channel_count;
memcpy(input_rc.values, ®s[prolog], channel_count * 2);
return ret;
}
io_reg_get函数我们知道是通过 _interface(这里是串口)调用 read函数从 io中读取数据的，那这样一切就都说得通了。而现在我们要看的是 io_publish_raw_rc函数是如何被调用的。这个工作是由 task_main函数完成的。
{
/* ... */
/* loop talking to IO */
while (!_task_should_exit) {
/* ... */
if (now >= poll_last + IO_POLL_INTERVAL) {
/* run at 50Hz */
poll_last = now;
/* pull status and alarms from IO */
io_get_status();
/* get raw R/C input from IO */
io_publish_raw_rc();
/* fetch PWM outputs from IO */
io_publish_pwm_outputs();
}
/* ... */
}
void
{
g_dev->task_main();
}
{
/* ... */
/* start the IO interface task */
_task = task_create("px4io", SCHED_PRIORITY_ACTUATOR_OUTPUTS, 2048, (main_t)&PX4IO::task_main_trampoline, nullptr);
if (_task < 0) {
debug("task start failed: %d", errno);
return -errno;
}
mavlink_log_info(_mavlink_fd, "[IO] init ok");
return OK;
}
init函数是在创建类的时候会去调用的。这样我们就把整个一条线给理顺了。
看完 ORB，我觉得 PX4是不是搞的太复杂了点，当 PX4IO读到 RC数据直接通过进程间通信发过去不就得了，何必绕这么大个弯子，还创建了这许多的设备文件？如果直接使用进程间通信岂不是简单很多？作为一个嵌入式操作系统如果说 Nuttx不支持进程间通信这毕竟说不过去，因为任务调度，进程间通信这些属于操作系统的基本功能，我不认为 Nuttx不会提供。但是使用这种方式倒也不是一无是处，这种方式通信的好处是数据收发双方都只跟设备文件打交道，一个数据发送可以有多个接收者。相对进程间通信，个人认为这种通信方式开销要大，在 Linux中尤其如此，因为在 Linux中增加了内核跟用户空间的切换，所以在 Linux中显然不适合用这种方式来进行进程间通信。
又折腾了一天，不过还好把 ORB给搞清楚了，收获还不算小。那么下一步我又该接着去分析哪个模块呢，也许是时候拿 io固件来耍耍了。
PX4 IO [14] serial
我们已经不止一次接触到 fmu跟 IO通讯，之前我们看到的只是 fmu部分源码，那么 IO中又是怎么处理的呢？下面我们就来瞧瞧。
IO固件的 mk文件为： "./PX4Firmware/makefiles/config_px4io-v2_default.mk"，其内容如下：
# Makefile for the px4iov2_default configuration
#
# Board support modules
#
MODULES += drivers/stm32
MODULES += drivers/boards/px4io-v2
MODULES += modules/px4iofirmware
adc.c dsm.c mixer.cpp protocol.h px4io.h safety.c serial.c
controls.c i2c.c module.mk px4io.c registers.c sbus.c
radiolink@ubuntu:~/apm$
前面我们看到，在 io_get_raw_rc_input函数中使用下面的代码来获取遥控器数据：
ret = io_reg_get(PX4IO_PAGE_RAW_RC_INPUT, PX4IO_P_RAW_RC_BASE + 9, ®s[prolog + 9], channel_count - 9);
if (ret != OK)
return ret;
}
int PX4IO::io_reg_get(uint8_t page, uint8_t offset, uint16_t *values,unsigned num_values)
{
/* range check the transfer */
if (num_values > ((_max_transfer) / sizeof(*values))) {
debug("io_reg_get: too many registers (%u, max %u)", num_values, _max_transfer / 2);
return -EINVAL;
}
int ret = _interface->read((page << 8) | offset, reinterpret_cast<void *>(values), num_values);
if (ret != (int)num_values) {
debug("io_reg_get(%u,%u,%u): data error %d", page, offset, num_values, ret);
return -1;
}
return OK;
}
./PX4Firmware/src/modules/px4iofirmware/px4io.h:77:extern uint16_t r_page_raw_rc_input[]; /* PX4IO_PAGE_RAW_RC_INPUT */
./PX4Firmware/src/modules/px4iofirmware/protocol.h:140:#define PX4IO_PAGE_RAW_RC_INPUT 4
./PX4Firmware/src/modules/px4iofirmware/registers.c:866: casePX4IO_PAGE_RAW_RC_INPUT:
radiolink@ubuntu:~/apm$
int registers_get(uint8_t page, uint8_t offset, uint16_t **values,unsigned *num_values)
{
#define SELECT_PAGE(_page_name) \
do { \
*values = &_page_name[0]; \
*num_values = sizeof(_page_name) / sizeof(_page_name[0]); \
} while(0)
switch (page) {
/* ... */
/* status pages */
case PX4IO_PAGE_CONFIG:
SELECT_PAGE(r_page_config);
break;
case PX4IO_PAGE_ACTUATORS:
SELECT_PAGE(r_page_actuators);
break;
case PX4IO_PAGE_SERVOS:
SELECT_PAGE(r_page_servos);
break;
case PX4IO_PAGE_RAW_RC_INPUT:
SELECT_PAGE(r_page_raw_rc_input);
break;
{
/* check packet CRC */
uint8_t crc = dma_packet.crc;
dma_packet.crc = 0;
if (crc != crc_packet(&dma_packet)) {
perf_count(pc_crcerr);
/* send a CRC error reply */
dma_packet.count_code = PKT_CODE_CORRUPT;
dma_packet.page = 0xff;
dma_packet.offset = 0xff;
return;
}
if (PKT_CODE(dma_packet) == PKT_CODE_WRITE) {
/* it's a blind write - pass it on */
if (registers_set(dma_packet.page, dma_packet.offset, &dma_packet.regs[0], PKT_COUNT(dma_packet))) {
perf_count(pc_regerr);
dma_packet.count_code = PKT_CODE_ERROR;
} else {
dma_packet.count_code = PKT_CODE_SUCCESS;
}
return;
}
if (PKT_CODE(dma_packet) == PKT_CODE_READ) {
/* it's a read - get register pointer for reply */
unsigned count;
uint16_t *registers;
if (registers_get(dma_packet.page, dma_packet.offset, ®isters, &count) < 0) {
perf_count(pc_regerr);
dma_packet.count_code = PKT_CODE_ERROR;
} else {
/* constrain reply to requested size */
if (count > PKT_MAX_REGS)
count = PKT_MAX_REGS;
if (count > PKT_COUNT(dma_packet))
count = PKT_COUNT(dma_packet);
/* copy reply registers into DMA buffer */
memcpy((void *)&dma_packet.regs[0], registers, count * 2);
dma_packet.count_code = count | PKT_CODE_SUCCESS;
}
return;
}
/* send a bad-packet error reply */
dma_packet.count_code = PKT_CODE_CORRUPT;
dma_packet.page = 0xff;
dma_packet.offset = 0xfe;
}
static
{
/* ... */
rx_handle_packet();
./PX4Firmware/src/modules/px4iofirmware/controls.c:216: bool ppm_updated = ppm_input(r_raw_rc_values, &r_raw_rc_count, &r_page_raw_rc_input[PX4IO_P_RAW_RC_DATA]);
./PX4Firmware/src/modules/px4iofirmware/controls.c:230: r_page_raw_rc_input[PX4IO_P_RAW_RC_NRSSI] = rssi;
./PX4Firmware/src/modules/px4iofirmware/px4io.h:77:extern uint16_t r_page_raw_rc_input[]; /* PX4IO_PAGE_RAW_RC_INPUT */
./PX4Firmware/src/modules/px4iofirmware/px4io.h:97:#define r_raw_rc_count r_page_raw_rc_input[PX4IO_P_RAW_RC_COUNT]
./PX4Firmware/src/modules/px4iofirmware/px4io.h:98:#define r_raw_rc_values (&r_page_raw_rc_input[PX4IO_P_RAW_RC_BASE])
./PX4Firmware/src/modules/px4iofirmware/px4io.h:99:#define r_raw_rc_flags r_page_raw_rc_input[PX4IO_P_RAW_RC_FLAGS]
./PX4Firmware/src/modules/px4iofirmware/registers.c:114:uint16_t r_page_raw_rc_input[] =
./PX4Firmware/src/modules/px4iofirmware/registers.c:867: SELECT_PAGE(r_page_raw_rc_input);
radiolink@ubuntu:~/apm$
* XXX each S.bus frame will cause a PPM decoder interrupt
* storm (lots of edges). It might be sensible to actually
* disable the PPM decoder completely if we have S.bus signal.
*/
perf_begin(c_gather_ppm);
bool ppm_updated = ppm_input(r_raw_rc_values, &r_raw_rc_count, &r_page_raw_rc_input[PX4IO_P_RAW_RC_DATA]);
if (ppm_updated) {
r_status_flags |= PX4IO_P_STATUS_FLAGS_RC_PPM;
r_raw_rc_flags &= ~(PX4IO_P_RAW_RC_FLAGS_FRAME_DROP);
r_raw_rc_flags &= ~(PX4IO_P_RAW_RC_FLAGS_FAILSAFE);
}
perf_end(c_gather_ppm);
/* limit number of channels to allowable data size */
if (r_raw_rc_count > PX4IO_RC_INPUT_CHANNELS)
r_raw_rc_count = PX4IO_RC_INPUT_CHANNELS;
/* store RSSI */
r_page_raw_rc_input[PX4IO_P_RAW_RC_NRSSI] = rssi;
static bool ppm_input(uint16_t *values, uint16_t *num_values, uint16_t *frame_len)
{
bool result = false;
/* avoid racing with PPM updates */
irqstate_t state = irqsave();
/*
* If we have received a new PPM frame within the last 200ms, accept it
* and then invalidate it.
*/
if (hrt_elapsed_time(&ppm_last_valid_decode) < 200000) {
/* PPM data exists, copy it */
*num_values = ppm_decoded_channels;
if (*num_values > PX4IO_RC_INPUT_CHANNELS)
*num_values = PX4IO_RC_INPUT_CHANNELS;
for (unsigned i = 0; i < *num_values; i++)
values[i] = ppm_buffer[i];
/* clear validity */
ppm_last_valid_decode = 0;
/* store PPM frame length */
if (num_values)
*frame_len = ppm_frame_length;
/* good if we got any channels */
result = (*num_values > 0);
}
irqrestore(state);
return result;
}
* PAGE 0
*
* Static configuration parameters.
*/
static const uint16_t r_page_config[] = {
[PX4IO_P_CONFIG_PROTOCOL_VERSION] = PX4IO_PROTOCOL_VERSION,
#ifdef CONFIG_ARCH_BOARD_PX4IO_V2
[PX4IO_P_CONFIG_HARDWARE_VERSION] = 2,
#else
[PX4IO_P_CONFIG_HARDWARE_VERSION] = 1,
#endif
[PX4IO_P_CONFIG_BOOTLOADER_VERSION] = 3, /* XXX hardcoded magic number */
[PX4IO_P_CONFIG_MAX_TRANSFER] = 64, /* XXX hardcoded magic number */
[PX4IO_P_CONFIG_CONTROL_COUNT] = PX4IO_CONTROL_CHANNELS,
[PX4IO_P_CONFIG_ACTUATOR_COUNT] = PX4IO_SERVO_COUNT,
[PX4IO_P_CONFIG_RC_INPUT_COUNT] = PX4IO_RC_INPUT_CHANNELS,
[PX4IO_P_CONFIG_ADC_INPUT_COUNT] = PX4IO_ADC_CHANNEL_COUNT,
[PX4IO_P_CONFIG_RELAY_COUNT] = PX4IO_RELAY_CHANNELS,
};
./PX4Firmware/src/systemcmds/tests/test_hrt.c:90:extern uint16_t ppm_buffer[];
./PX4Firmware/src/systemcmds/tests/test_hrt.c:103: printf(" %u\n", ppm_buffer[i]);
./PX4Firmware/src/modules/systemlib/ppm_decode.c:91:uint16_t ppm_buffer[PPM_MAX_CHANNELS];
./PX4Firmware/src/modules/systemlib/ppm_decode.c:179: ppm_buffer[i] = ppm_temp_buffer[i];
./PX4Firmware/src/modules/systemlib/ppm_decode.h:59:__EXPORT extern uint16_t ppm_buffer[PPM_MAX_CHANNELS]; /**< decoded PPM channel values */
./PX4Firmware/src/modules/px4iofirmware/controls.c:472: values[i] = ppm_buffer[i];
./PX4Firmware/src/drivers/stm32/drv_hrt.c:352:__EXPORT uint16_t ppm_buffer[PPM_MAX_CHANNELS];
./PX4Firmware/src/drivers/stm32/drv_hrt.c:503: ppm_buffer[i] = ppm_temp_buffer[i];
./PX4Firmware/src/drivers/px4fmu/fmu.cpp:722: rc_in.values[i] = ppm_buffer[i];
radiolink@ubuntu:~/apm$
{
uint16_t width;
uint16_t interval;
unsigned i;
/* if we missed an edge, we have to give up */
if (reset)
goto error;
/* how long since the last edge? */
width = count - ppm.last_edge;
if (count < ppm.last_edge)
width += ppm.count_max; /* handle wrapped count */
ppm.last_edge = count;
if (width >= PPM_MIN_START) {
if (ppm.next_channel != ppm_decoded_channels) {
/* ... */
} else {
/* frame channel count matches expected, let's use it */
if (ppm.next_channel > PPM_MIN_CHANNELS) {
for (i = 0; i < ppm.next_channel; i++)
ppm_buffer[i] = ppm_temp_buffer[i];
ppm_last_valid_decode = hrt_absolute_time();
}
}
/* reset for the next frame */
ppm.next_channel = 0;
/* next edge is the reference for the first channel */
ppm.phase = ARM;
return;
}
radiolink@ubuntu:~/apm$ grep -nr ppm_input_decode ./PX4Firmware/src/
./PX4Firmware/src/modules/systemlib/ppm_decode.c:123:ppm_input_decode(bool reset,unsigned count)
./PX4Firmware/src/modules/systemlib/ppm_decode.h:68: * ppm_input_decode, used to determine how to
./PX4Firmware/src/modules/systemlib/ppm_decode.h:84:__EXPORT void ppm_input_decode(bool reset,unsigned count);
radiolink@ubuntu:~/apm$
* Handle the PPM decoder state machine.
*/
static void hrt_ppm_decode(uint32_t status)
{
uint16_t count = rCCR_PPM;
uint16_t width;
uint16_t interval;
unsigned i;
/* if we missed an edge, we have to give up */
if (status & SR_OVF_PPM)
goto error;
/* how long since the last edge? - this handles counter wrapping implicitely. */
width = count - ppm.last_edge;
ppm_edge_history[ppm_edge_next++] = width;
if (ppm_edge_next >= 32)
ppm_edge_next = 0;
/*
* if this looks like a start pulse, then push the last set of values
* and reset the state machine
*/
if (width >= PPM_MIN_START) {
/*
* If the number of channels changes unexpectedly, we don't want
* to just immediately jump on the new count as it may be a result
* of noise or dropped edges. Instead, take a few frames to settle.
*/
if (ppm.next_channel != ppm_decoded_channels) {
static unsigned new_channel_count;
static unsigned new_channel_holdoff;
if (new_channel_count != ppm.next_channel) {
/* start the lock counter for the new channel count */
new_channel_count = ppm.next_channel;
new_channel_holdoff = PPM_CHANNEL_LOCK;
} else if (new_channel_holdoff > 0) {
/* this frame matched the last one, decrement the lock counter */
new_channel_holdoff--;
} else {
/* we have seen PPM_CHANNEL_LOCK frames with the new count, accept it */
ppm_decoded_channels = new_channel_count;
new_channel_count = 0;
}
} else {
/* frame channel count matches expected, let's use it */
if (ppm.next_channel > PPM_MIN_CHANNELS) {
for (i = 0; i < ppm.next_channel; i++)
ppm_buffer[i] = ppm_temp_buffer[i];
ppm_last_valid_decode = hrt_absolute_time();
}
}
/* reset for the next frame */
ppm.next_channel = 0;
/* next edge is the reference for the first channel */
ppm.phase = ARM;
ppm.last_edge = count;
return;
}
/**
* Handle the compare interupt by calling the callout dispatcher
* and then re-scheduling the next deadline.
*/
static int
hrt_tim_isr( int irq, void *context)
{
uint32_t status;
/* grab the timer for latency tracking purposes */
latency_actual = rCNT;
/* copy interrupt status */
status = rSR;
/* ack the interrupts we just read */
rSR = ~status;
# ifdef HRT_PPM_CHANNEL
/* was this a PPM edge? */
if (status & (SR_INT_PPM | SR_OVF_PPM)) {
/* if required, flip edge sensitivity */
# ifdef PPM_EDGE_FLIP
rCCER ^= CCER_PPM_FLIP;
# endif
hrt_ppm_decode(status);
}
# endif
case UNSYNCH:
/* we are waiting for a start pulse - nothing useful to do here */
break;
case ARM:
/* we expect a pulse giving us the first mark */
if (width < PPM_MIN_PULSE_WIDTH || width > PPM_MAX_PULSE_WIDTH)
goto error; /* pulse was too short or too long */
/* record the mark timing, expect an inactive edge */
ppm.last_mark = ppm.last_edge;
/* frame length is everything including the start gap */
ppm_frame_length = (uint16_t)(ppm.last_edge - ppm.frame_start);
ppm.frame_start = ppm.last_edge;
ppm.phase = ACTIVE;
break;
case INACTIVE:
/* we expect a short pulse */
if (width < PPM_MIN_PULSE_WIDTH || width > PPM_MAX_PULSE_WIDTH)
goto error; /* pulse was too short or too long */
/* this edge is not interesting, but now we are ready for the next mark */
ppm.phase = ACTIVE;
break;
case ACTIVE:
/* determine the interval from the last mark */
interval = count - ppm.last_mark;
ppm.last_mark = count;
ppm_pulse_history[ppm_pulse_next++] = interval;
if (ppm_pulse_next >= 32)
ppm_pulse_next = 0;
/* if the mark-mark timing is out of bounds, abandon the frame */
if ((interval < PPM_MIN_CHANNEL_VALUE) || (interval > PPM_MAX_CHANNEL_VALUE))
goto error;
/* if we have room to store the value, do so */
if (ppm.next_channel < PPM_MAX_CHANNELS)
ppm_temp_buffer[ppm.next_channel++] = interval;
ppm.phase = INACTIVE;
break;
}
ppm.last_edge = count;
return;
/* the state machine is corrupted; reset it */
error:
/* we don't like the state of the decoder, reset it and try again */
ppm.phase = UNSYNCH;
ppm_decoded_channels = 0;
}
关于脉宽测量，我们目前还没有必要去研究它，所以就不深入其细节了。
2.user_start
现在，我们就来看看 ppm_input函数是如何被调用的。
controls_tick() {
uint16_t rssi = 0;
#ifdef ADC_RSSI
if (r_setup_features & PX4IO_P_SETUP_FEATURES_ADC_RSSI) {
unsigned counts = adc_measure(ADC_RSSI);
if (counts != 0xffff) {
/* use 1:1 scaling on 3.3V ADC input */
unsigned mV = counts * 3300 / 4096;
/* scale to 0..253 */
rssi = mV / 13;
}
}
#endif
perf_begin(c_gather_dsm);
uint16_t temp_count = r_raw_rc_count;
bool dsm_updated = dsm_input(r_raw_rc_values, &temp_count);
if (dsm_updated) {
r_raw_rc_flags |= PX4IO_P_STATUS_FLAGS_RC_DSM;
r_raw_rc_count = temp_count & 0x7fff;
if (temp_count & 0x8000)
r_raw_rc_flags |= PX4IO_P_RAW_RC_FLAGS_RC_DSM11;
else
r_raw_rc_flags &= ~PX4IO_P_RAW_RC_FLAGS_RC_DSM11;
r_raw_rc_flags &= ~(PX4IO_P_RAW_RC_FLAGS_FRAME_DROP);
r_raw_rc_flags &= ~(PX4IO_P_RAW_RC_FLAGS_FAILSAFE);
}
perf_end(c_gather_dsm);
perf_begin(c_gather_sbus);
bool sbus_status = (r_status_flags & PX4IO_P_STATUS_FLAGS_RC_SBUS);
bool sbus_failsafe, sbus_frame_drop;
bool sbus_updated = sbus_input(r_raw_rc_values, &r_raw_rc_count, &sbus_failsafe, &sbus_frame_drop, PX4IO_RC_INPUT_CHANNELS);
if (sbus_updated) {
r_status_flags |= PX4IO_P_STATUS_FLAGS_RC_SBUS;
rssi = 255;
if (sbus_frame_drop) {
r_raw_rc_flags |= PX4IO_P_RAW_RC_FLAGS_FRAME_DROP;
rssi = 100;
} else {
r_raw_rc_flags &= ~(PX4IO_P_RAW_RC_FLAGS_FRAME_DROP);
}
if (sbus_failsafe) {
r_raw_rc_flags |= PX4IO_P_RAW_RC_FLAGS_FAILSAFE;
rssi = 0;
} else {
r_raw_rc_flags &= ~(PX4IO_P_RAW_RC_FLAGS_FAILSAFE);
}
}
perf_end(c_gather_sbus);
/*
* XXX each S.bus frame will cause a PPM decoder interrupt
* storm (lots of edges). It might be sensible to actually
* disable the PPM decoder completely if we have S.bus signal.
*/
perf_begin(c_gather_ppm);
bool ppm_updated = ppm_input(r_raw_rc_values, &r_raw_rc_count, &r_page_raw_rc_input[PX4IO_P_RAW_RC_DATA]);
if (ppm_updated) {
r_status_flags |= PX4IO_P_STATUS_FLAGS_RC_PPM;
r_raw_rc_flags &= ~(PX4IO_P_RAW_RC_FLAGS_FRAME_DROP);
r_raw_rc_flags &= ~(PX4IO_P_RAW_RC_FLAGS_FAILSAFE);
}
perf_end(c_gather_ppm);
/* limit number of channels to allowable data size */
if (r_raw_rc_count > PX4IO_RC_INPUT_CHANNELS)
r_raw_rc_count = PX4IO_RC_INPUT_CHANNELS;
/* store RSSI */
r_page_raw_rc_input[PX4IO_P_RAW_RC_NRSSI] = rssi;
./PX4Firmware/src/modules/px4iofirmware/controls.c:145:controls_tick() {
./PX4Firmware/src/modules/px4iofirmware/px4io.h:217:extern void controls_tick(void);
./PX4Firmware/src/modules/px4iofirmware/px4io.c:354: controls_tick();
radiolink@ubuntu:~/apm$
int user_start(int argc, char *argv[])
{
/* ... */
for (;;) {
/* track the rate at which the loop is running */
perf_count(loop_perf);
/* kick the mixer */
perf_begin(mixer_perf);
mixer_tick();
perf_end(mixer_perf);
/* kick the control inputs */
perf_begin(controls_perf);
controls_tick();
以上就是 IO中遥控器的输入。说完了输入，我们还得说说输出。这个我们稍后再来看。
PX4 IO [15] mixer
这个我们当然得先知道在 fmu中是怎么往 IO发这些输出数据的。
回过头去看第十一篇笔记，我们会看到 " AP_MotorsMatrix::output_test"函数的源码如下：
// motor_seq is the motor's sequence number from 1 to the number of motors on the frame
// pwm value is an actual pwm value that will be output, normally in the range of 1000 ~ 2000
void AP_MotorsMatrix::output_test(uint8_t motor_seq, int16_t pwm)
{
// exit immediately if not armed
if (!_flags.armed) {
return;
}
// loop through all the possible orders spinning any motors that match that description
for (uint8_t i=0; i
// turn on this motor
hal.rcout->write(pgm_read_byte(&_motor_to_channel_map[i]), pwm);
}
}
}
也就是说通过 " hal.rcout->write"接口下发数据。" rcout"类型为 "PX4RCOutput"。从前面的分析中我们知道 "PX4RCOutput"其实是通过设备文件对 px4io进行调用，最终调用的是 "PX4IO::write"函数，源码如下：
/* Make it obvious that file * isn't used here */
{
unsigned count = len / 2;
if (count > _max_actuators)
count = _max_actuators;
if (count > 0) {
perf_begin(_perf_write);
int ret = io_reg_set(PX4IO_PAGE_DIRECT_PWM, 0, (uint16_t *)buffer, count);
perf_end(_perf_write);
if (ret != OK)
return ret;
}
return count * 2;
}
px4io在我们的 V2版本中通过串口跟 io板通信。所以两者通信的关键在于 " PX4IO_PAGE_DIRECT_PWM"这个宏，
./PX4Firmware/src/modules/px4iofirmware/protocol.h:258:#define PX4IO_PAGE_DIRECT_PWM 54 /**< 0..CONFIG_ACTUATOR_COUNT-1 */
./PX4Firmware/src/modules/px4iofirmware/registers.c:284: case PX4IO_PAGE_DIRECT_PWM:
./PX4Firmware/src/modules/px4iofirmware/registers.c:883: case PX4IO_PAGE_DIRECT_PWM:
./PX4Firmware/src/drivers/px4io/px4io.cpp:2366: ret = io_reg_set(PX4IO_PAGE_DIRECT_PWM, channel, arg);
./PX4Firmware/src/drivers/px4io/px4io.cpp:2649: int ret = io_reg_set(PX4IO_PAGE_DIRECT_PWM, 0, (uint16_t *)buffer, count);
radiolink@ubuntu:~/apm$
{
switch (page) {
/* ... */
/* handle raw PWM input */
case PX4IO_PAGE_DIRECT_PWM:
/* copy channel data */
while ((offset < PX4IO_CONTROL_CHANNELS) && (num_values > 0)) {
/* XXX range-check value? */
r_page_servos[offset] = *values;
offset++;
num_values--;
values++;
}
system_state.fmu_data_received_time = hrt_absolute_time();
r_status_flags |= PX4IO_P_STATUS_FLAGS_FMU_OK | PX4IO_P_STATUS_FLAGS_RAW_PWM;
break;
./PX4Firmware/src/modules/px4iofirmware/px4io.h:76:extern uint16_t r_page_servos[]; /* PX4IO_PAGE_SERVOS */
./PX4Firmware/src/modules/px4iofirmware/mixer.cpp:217: r_page_servos[i] = r_page_servo_failsafe[i];
./PX4Firmware/src/modules/px4iofirmware/mixer.cpp:220: r_page_actuators[i] = FLOAT_TO_REG((r_page_servos[i] - 1500) / 600.0f);
./PX4Firmware/src/modules/px4iofirmware/mixer.cpp:237: pwm_limit_calc(should_arm, mixed, r_page_servo_disarmed, r_page_servo_control_min, r_page_servo_control_max, outputs, r_page_servos, &pwm_limit);
./PX4Firmware/src/modules/px4iofirmware/mixer.cpp:240: r_page_servos[i] = 0;
./PX4Firmware/src/modules/px4iofirmware/mixer.cpp:276: up_pwm_servo_set(i, r_page_servos[i]);
./PX4Firmware/src/modules/px4iofirmware/registers.c:107:uint16_t r_page_servos[PX4IO_SERVO_COUNT];
./PX4Firmware/src/modules/px4iofirmware/registers.c:221: * PAGE 104 uses r_page_servos.
./PX4Firmware/src/modules/px4iofirmware/registers.c:291: r_page_servos[offset] = *values;
./PX4Firmware/src/modules/px4iofirmware/registers.c:864: SELECT_PAGE(r_page_servos);
./PX4Firmware/src/modules/px4iofirmware/registers.c:884: SELECT_PAGE(r_page_servos);
radiolink@ubuntu:~/apm$
{
/* ... */
for (;;) {
/* track the rate at which the loop is running */
perf_count(loop_perf);
/* kick the mixer */
perf_begin(mixer_perf);
mixer_tick();
perf_end(mixer_perf);
/* kick the control inputs */
perf_begin(controls_perf);
controls_tick();
perf_end(controls_perf);
{
/* check that we are receiving fresh data from the FMU */
if (hrt_elapsed_time(&system_state.fmu_data_received_time) > FMU_INPUT_DROP_LIMIT_US) {
/* too long without FMU input, time to Go to failsafe */
if (r_status_flags & PX4IO_P_STATUS_FLAGS_FMU_OK) {
isr_debug(1, "AP RX timeout");
}
r_status_flags &= ~(PX4IO_P_STATUS_FLAGS_FMU_OK);
r_status_alarms |= PX4IO_P_STATUS_ALARMS_FMU_LOST;
} else {
r_status_flags |= PX4IO_P_STATUS_FLAGS_FMU_OK;
}
#define FMU_INPUT_DROP_LIMIT_US 200000
source = MIX_FAILSAFE;
/*
* Decide which set of controls we're using.
*/
/* do not mix if RAW_PWM mode is on and FMU is good */
if ((r_status_flags & PX4IO_P_STATUS_FLAGS_RAW_PWM) &&
(r_status_flags & PX4IO_P_STATUS_FLAGS_FMU_OK)) {
/* don't actually mix anything - we already have raw PWM values */
source = MIX_NONE;
} else {
if (!(r_status_flags & PX4IO_P_STATUS_FLAGS_OVERRIDE) &&
(r_status_flags & PX4IO_P_STATUS_FLAGS_FMU_OK) &&
(r_status_flags & PX4IO_P_STATUS_FLAGS_MIXER_OK)) {
/* mix from FMU controls */
source = MIX_FMU;
}
if ( (r_status_flags & PX4IO_P_STATUS_FLAGS_OVERRIDE) &&
(r_status_flags & PX4IO_P_STATUS_FLAGS_RC_OK) &&
(r_status_flags & PX4IO_P_STATUS_FLAGS_MIXER_OK) &&
!(r_setup_arming & PX4IO_P_SETUP_ARMING_RC_HANDLING_DISABLED) &&
!(r_status_flags & PX4IO_P_STATUS_FLAGS_FMU_OK)) {
/* if allowed, mix from RC inputs directly */
source = MIX_OVERRIDE;
} else if ( (r_status_flags & PX4IO_P_STATUS_FLAGS_OVERRIDE) &&
(r_status_flags & PX4IO_P_STATUS_FLAGS_RC_OK) &&
(r_status_flags & PX4IO_P_STATUS_FLAGS_MIXER_OK) &&
!(r_setup_arming & PX4IO_P_SETUP_ARMING_RC_HANDLING_DISABLED) &&
(r_status_flags & PX4IO_P_STATUS_FLAGS_FMU_OK)) {
/* if allowed, mix from RC inputs directly up to available rc channels */
source = MIX_OVERRIDE_FMU_OK;
}
}
/*
* Set failsafe status flag depending on mixing source
*/
if (source == MIX_FAILSAFE) {
r_status_flags |= PX4IO_P_STATUS_FLAGS_FAILSAFE;
} else {
r_status_flags &= ~(PX4IO_P_STATUS_FLAGS_FAILSAFE);
}
* Decide whether the servos should be armed right now.
*
* We must be armed, and we must have a PWM source; either raw from
* FMU or from the mixer.
*
* XXX correct behaviour for failsafe may require an additional case
* here.
*/
should_arm = (
/* IO initialised without error */ (r_status_flags & PX4IO_P_STATUS_FLAGS_INIT_OK)
/* and IO is armed */ && (r_status_flags & PX4IO_P_STATUS_FLAGS_SAFETY_OFF)
/* and FMU is armed */ && (
((r_setup_arming & PX4IO_P_SETUP_ARMING_FMU_ARMED)
/* and there is valid input via or mixer */ && (r_status_flags & PX4IO_P_STATUS_FLAGS_MIXER_OK) )
/* or direct PWM is set */ || (r_status_flags & PX4IO_P_STATUS_FLAGS_RAW_PWM)
/* or failsafe was set manually */ || ((r_setup_arming & PX4IO_P_SETUP_ARMING_FAILSAFE_CUSTOM) && !(r_status_flags & PX4IO_P_STATUS_FLAGS_FMU_OK))
)
);
should_always_enable_pwm = (r_setup_arming & PX4IO_P_SETUP_ARMING_ALWAYS_PWM_ENABLE)
&& (r_status_flags & PX4IO_P_STATUS_FLAGS_INIT_OK)
&& (r_status_flags & PX4IO_P_STATUS_FLAGS_FMU_OK);
* Run the mixers.
*/
if (source == MIX_FAILSAFE) {
/* copy failsafe values to the servo outputs */
for (unsigned i = 0; i < PX4IO_SERVO_COUNT; i++) {
r_page_servos[i] = r_page_servo_failsafe[i];
/* safe actuators for FMU feedback */
r_page_actuators[i] = FLOAT_TO_REG((r_page_servos[i] - 1500) / 600.0f);
}
} else if (source != MIX_NONE && (r_status_flags & PX4IO_P_STATUS_FLAGS_MIXER_OK)) {
float outputs[PX4IO_SERVO_COUNT];
unsigned mixed;
/* mix */
/* poor mans mutex */
in_mixer = true;
mixed = mixer_group.mix(&outputs[0], PX4IO_SERVO_COUNT);
in_mixer = false;
pwm_limit_calc(should_arm, mixed, r_page_servo_disarmed, r_page_servo_control_min, r_page_servo_control_max, outputs, r_page_servos, &pwm_limit);
for (unsigned i = mixed; i < PX4IO_SERVO_COUNT; i++)
r_page_servos[i] = 0;
for (unsigned i = 0; i < PX4IO_SERVO_COUNT; i++) {
r_page_actuators[i] = FLOAT_TO_REG(outputs[i]);
}
}
bool needs_to_arm = (should_arm || should_always_enable_pwm);
/* check any conditions that prevent arming */
if (r_setup_arming & PX4IO_P_SETUP_ARMING_LOCKDOWN) {
needs_to_arm = false;
}
if (!should_arm && !should_always_enable_pwm) {
needs_to_arm = false;
}
if (needs_to_arm && !mixer_servos_armed) {
/* need to arm, but not armed */
up_pwm_servo_arm(true);
mixer_servos_armed = true;
r_status_flags |= PX4IO_P_STATUS_FLAGS_OUTPUTS_ARMED;
isr_debug(5, "> PWM enabled");
} else if (!needs_to_arm && mixer_servos_armed) {
/* armed but need to disarm */
up_pwm_servo_arm(false);
mixer_servos_armed = false;
r_status_flags &= ~(PX4IO_P_STATUS_FLAGS_OUTPUTS_ARMED);
isr_debug(5, "> PWM disabled");
}
if (mixer_servos_armed && should_arm) {
/* update the servo outputs. */
for (unsigned i = 0; i < PX4IO_SERVO_COUNT; i++)
up_pwm_servo_set(i, r_page_servos[i]);
} else if (mixer_servos_armed && should_always_enable_pwm) {
/* set the disarmed servo outputs. */
for (unsigned i = 0; i < PX4IO_SERVO_COUNT; i++)
up_pwm_servo_set(i, r_page_servo_disarmed[i]);
}
int up_pwm_servo_set(unsigned channel, servo_position_t value)
{
if (channel >= PWM_SERVO_MAX_CHANNELS)
return -1;
unsigned timer = pwm_channels[channel].timer_index;
/* test timer for validity */
if ((pwm_timers[timer].base == 0) ||
(pwm_channels[channel].gpio == 0))
return -1;
/* configure the channel */
if (value > 0)
value--;
switch (pwm_channels[channel].timer_channel) {
case 1:
rCCR1(timer) = value;
break;
case 2:
rCCR2(timer) = value;
break;
case 3:
rCCR3(timer) = value;
break;
case 4:
rCCR4(timer) = value;
break;
default:
return -1;
}
return 0;
}
而其中具体每一个参数是做什么的，这个就要搞懂了流程以后详细去分析了。
在文件 " ardupilot/ArduCopter/ArduCopter.pde"中有个数组： scheduler_tasks。这是 ArduCopter内部的任务队列，不依赖于 Nuttx操作系统。
{ rc_loop, 4, 10 },
{ throttle_loop, 8, 45 },
{ update_GPS, 8, 90 },
{ update_batt_compass, 40, 72 },
{ read_aux_switches, 40, 5 },
{ arm_motors_check, 40, 1 },
其中 rc_loop是用来获取遥控器数据的，代码如下：
// called at 100hz
static void rc_loop()
{
// Read radio and 3-position switch on radio
// -----------------------------------------
read_radio();
read_control_switch();
}
#define FAILSAFE_RADIO_TIMEOUT_MS 2000 // 2 seconds
static void read_radio()
{
static uint32_t last_update = 0;
if (hal.rcin->new_input()) {
last_update = millis();
ap.new_radio_frame = true;
uint16_t periods[8];
hal.rcin->read(periods,8);
g.rc_1.set_pwm(periods[rcmap.roll()-1]);
g.rc_2.set_pwm(periods[rcmap.pitch()-1]);
set_throttle_and_failsafe(periods[rcmap.throttle()-1]);
g.rc_4.set_pwm(periods[rcmap.yaw()-1]);
g.rc_5.set_pwm(periods[4]);
g.rc_6.set_pwm(periods[5]);
g.rc_7.set_pwm(periods[6]);
g.rc_8.set_pwm(periods[7]);
// flag we must have an rc receiver attached
if (!failsafe.rc_override_active) {
ap.rc_receiver_present = true;
}
// update output on any aux channels, for manual passthru
RC_Channel_aux::output_ch_all();
}else{
uint32_t elapsed = millis() - last_update;
// turn on throttle failsafe if no update from ppm encoder for 2 seconds
if ((elapsed >= FAILSAFE_RADIO_TIMEOUT_MS)
&& g.failsafe_throttle && motors.armed() && !failsafe.radio) {
Log_Write_Error(ERROR_SUBSYSTEM_RADIO, ERROR_CODE_RADIO_LATE_FRAME);
set_failsafe_radio(true);
}
}
}
// read input from APM_RC - create a control_in value
void RC_Channel::set_pwm(int16_t pwm)
{
radio_in = pwm;
if (_type == RC_CHANNEL_TYPE_RANGE) {
control_in = pwm_to_range();
} else {
//RC_CHANNEL_TYPE_ANGLE, RC_CHANNEL_TYPE_ANGLE_RAW
control_in = pwm_to_angle();
}
}
// called at 10hz
static void arm_motors_check()
{
static int16_t arming_counter;
bool allow_arming = false;
// ensure throttle is down
if (g.rc_3.control_in > 0) {
arming_counter = 0;
return;
}
// allow arming/disarming in fully manual flight modes ACRO, STABILIZE, SPORT and DRIFT
if (manual_flight_mode(control_mode)) {
allow_arming = true;
}
// allow arming/disarming in Loiter and AltHold if landed
if (ap.land_complete && (control_mode == LOITER || control_mode == ALT_HOLD || control_mode == POSHOLD || control_mode == AUTOTUNE)) {
allow_arming = true;
}
// kick out other flight modes
if (!allow_arming) {
arming_counter = 0;
return;
}
{
// if no change, exit immediately
if( ap.land_complete == b )
return;
if(b){
Log_Write_Event(DATA_LAND_COMPLETE);
}else{
Log_Write_Event(DATA_NOT_LANDED);
}
ap.land_complete = b;
}
bitcraze@bitcraze-vm:~/apm$ grep -nr set_land_complete ./ardupilot/ArduCopter/
./ardupilot/ArduCopter/control_autotune.pde:237: set_land_complete(false);
./ardupilot/ArduCopter/control_land.pde:201: set_land_complete(true);
./ardupilot/ArduCopter/control_land.pde:209: set_land_complete(false);
./ardupilot/ArduCopter/AP_State.pde:97:void set_land_complete(bool b)
./ardupilot/ArduCopter/control_althold.pde:53: set_land_complete(false);
./ardupilot/ArduCopter/motors.pde:534: set_land_complete(true);
./ardupilot/ArduCopter/control_loiter.pde:61: set_land_complete(false);
./ardupilot/ArduCopter/control_ofloiter.pde:63: set_land_complete(false);
./ardupilot/ArduCopter/system.pde:317: set_land_complete(true);
./ardupilot/ArduCopter/control_sport.pde:72: set_land_complete(false);
./ardupilot/ArduCopter/control_poshold.pde:180: set_land_complete(false);
./ardupilot/ArduCopter/control_circle.pde:58: set_land_complete(false);
bitcraze@bitcraze-vm:~/apm$
// returns true if we have landed
static bool update_land_detector()
{
// detect whether we have landed by watching for low climb rate and minimum throttle
if (abs(climb_rate) < 40 && motors.limit.throttle_lower) {
if (!ap.land_complete) {
// run throttle controller if accel based throttle controller is enabled and active (active means it has been given a target)
if( land_detector < LAND_DETECTOR_TRIGGER) {
land_detector++;
}else{
set_land_complete(true);
land_detector = 0;
}
}
} else if (g.rc_3.control_in != 0 || failsafe.radio) {
// we've sensed movement up or down so reset land_detector
land_detector = 0;
if(ap.land_complete) {
set_land_complete(false);
}
}
// return current state of landing
return ap.land_complete;
}
// throttle_loop - should be run at 50 hz
// ---------------------------
static void throttle_loop()
{
// get altitude and climb rate from inertial lib
read_inertial_altitude();
// check if we've landed
update_land_detector();
// check auto_armed status
update_auto_armed();
#if FRAME_CONFIG == HELI_FRAME
// update rotor speed
heli_update_rotor_speed_targets();
// update trad heli swash plate movement
heli_update_landing_swash();
#endif
}
当然，用过 PX4的都知道，解锁并不仅仅是油门最低就可以解锁，而是通过打摇杆的方式进行解锁的。
// full right
if (tmp > 4000) {
// increase the arming counter to a maximum of 1 beyond the auto trim counter
if( arming_counter <= AUTO_TRIM_DELAY ) {
arming_counter++;
}
// arm the motors and configure for flight
if (arming_counter == ARM_DELAY && !motors.armed()) {
// run pre-arm-checks and display failures
pre_arm_checks(true);
if(ap.pre_arm_check && arm_checks(true)) {
init_arm_motors();
}else{
// reset arming counter if pre-arm checks fail
arming_counter = 0;
AP_Notify::flags.arming_failed = true;
}
}
// arm the motors and configure for flight
if (arming_counter == AUTO_TRIM_DELAY && motors.armed() && control_mode == STABILIZE) {
auto_trim_counter = 250;
}
// full left
}else if (tmp < -4000) {
// increase the counter to a maximum of 1 beyond the disarm delay
if( arming_counter <= DISARM_DELAY ) {
arming_counter++;
}
// disarm the motors
if (arming_counter == DISARM_DELAY && motors.armed()) {
init_disarm_motors();
}
// Yaw is centered so reset arming counter
}else{
AP_Notify::flags.arming_failed = false;
arming_counter = 0;
}
}
if (hal.util->safety_switch_state() == AP_HAL::Util::SAFETY_DISARMED) {
if (display_failure) {
gcs_send_text_P(SEVERITY_HIGH,PSTR("Arm: Safety Switch"));
}
return false;
}
static void init_arm_motors()
{
// arming marker
// Flag used to track if we have armed the motors the first time.
// This is used to decide if we should run the ground_start routine
// which calibrates the IMU
static bool did_ground_start = false;
// disable cpu failsafe because initialising everything takes a while
failsafe_disable();
// disable inertial nav errors temporarily
inertial_nav.ignore_next_error();
#if LOGGING_ENABLED == ENABLED
// start dataflash
start_logging();
#endif
#if HIL_MODE != HIL_MODE_DISABLED || CONFIG_HAL_BOARD == HAL_BOARD_AVR_SITL
gcs_send_text_P(SEVERITY_HIGH, PSTR("ARMING MOTORS"));
#endif
// Remember Orientation
// --------------------
init_simple_bearing();
initial_armed_bearing = ahrs.yaw_sensor;
// Reset home position
// -------------------
if (ap.home_is_set) {
init_home();
calc_distance_and_bearing();
}
if(did_ground_start == false) {
did_ground_start = true;
startup_ground(true);
}
// fast baro calibration to reset ground pressure
init_barometer(false);
// reset inertial nav alt to zero
inertial_nav.set_altitude(0.0f);
// go back to normal AHRS gains
ahrs.set_fast_gains(false);
// enable gps velocity based centrefugal force compensation
ahrs.set_correct_centrifugal(true);
ahrs.set_armed(true);
// set hover throttle
motors.set_mid_throttle(g.throttle_mid);
// Cancel arming if throttle is raised too high so that copter does not suddenly take off
read_radio();
if (g.rc_3.control_in > g.throttle_cruise && g.throttle_cruise > 100) {
motors.output_min();
failsafe_enable();
return;
}
#if SPRAYER == ENABLED
// turn off sprayer's test if on
sprayer.test_pump(false);
#endif
// enable output to motors
output_min();
// finally actually arm the motors
motors.armed(true);
// log arming to dataflash
Log_Write_Event(DATA_ARMED);
// reenable failsafe
failsafe_enable();
}
那么，关于解锁我们就先看到这里。
1.stabilize_init
到目前为止，在 APM官网上公布的飞行模式有十几种，" stabilize"也只不过是其中一种，即自稳，或者叫姿态。处于该飞行模式下， roll跟 pitch摇杆控制的是飞行器的倾斜角度，油门控制的是电机的平均转速，特点是摇杆回中则姿态回到水平。
在 "ardupilot/ArduCopter"目录下有不少以 "control_"开头的源文件，大部分都各自对应了一种飞行模式。
ardupilot/ArduCopter/control_acro.pde ardupilot/ArduCopter/control_flip.pde ardupilot/ArduCopter/control_poshold.pde
ardupilot/ArduCopter/control_althold.pde ardupilot/ArduCopter/control_guided.pde ardupilot/ArduCopter/control_rtl.pde
ardupilot/ArduCopter/control_auto.pde ardupilot/ArduCopter/control_land.pde ardupilot/ArduCopter/control_sport.pde
ardupilot/ArduCopter/control_autotune.pde ardupilot/ArduCopter/control_loiter.pde ardupilot/ArduCopter/control_stabilize.pde
ardupilot/ArduCopter/control_circle.pde ardupilot/ArduCopter/control_modes.pde
ardupilot/ArduCopter/control_drift.pde ardupilot/ArduCopter/control_ofloiter.pde
bitcraze@bitcraze-vm:~/apm$
所以" stabilize"模式的相关代码在 "ardupilot/ArduCopter/control_stabilize.pde"文件中。
在 "ardupilot/ArduCopter/control_stabilize.pde"源文件中其实就俩函数： stabilize_init跟 stabilize_run。从函数名就很容易看出来前者是用来初始化的，后者则是该飞行模式区别与其它飞行模式的主要代码。
static bool stabilize_init(bool ignore_checks)
{
// set target altitude to zero for reporting
// To-Do: make pos controller aware when it's active/inactive so it can always report the altitude error?
pos_control.set_alt_target(0);
// stabilize should never be made to fail
return true;
}
初始化倒是挺简单的，但其实 "pos_control"并没有在 stabilize_run函数中使用，而这里显然是设置高度的初值的。既然都不使用，那么设置它干嘛？但是别忘了还有定高定点这样一些模式，这里是不使用，但是它们要用啊。你想想，当我以 "stabilize"模式起飞，在半空中直接切到定高模式，那么总不能以此时的高度作为初始高度，这显然不合适，而初始高度最合理的当然是起飞是的高度，所以最好的办法就是在这里设置初值。
下面一个问题是 stabilize_init函数是在哪里调用的？这个跟踪下代码就清楚了：
ardupilot/ArduCopter/flight_mode.pde:34: success = heli_stabilize_init(ignore_checks);
ardupilot/ArduCopter/flight_mode.pde:36: success = stabilize_init(ignore_checks);
ardupilot/ArduCopter/control_stabilize.pde:7:// stabilize_init - initialise stabilize controller
ardupilot/ArduCopter/control_stabilize.pde:8:static bool stabilize_init(bool ignore_checks)
ardupilot/ArduCopter/heli_control_stabilize.pde:7:// stabilize_init - initialise stabilize controller
ardupilot/ArduCopter/heli_control_stabilize.pde:8:static bool heli_stabilize_init(bool ignore_checks)
bitcraze@bitcraze-vm:~/apm$
static bool set_mode(uint8_t mode)
{
// boolean to record if flight mode could be set
bool success = false;
bool ignore_checks = !motors.armed(); // allow switching to any mode if disarmed. We rely on the arming check to perform
// return immediately if we are already in the desired mode
if (mode == control_mode) {
return true;
}
switch(mode) {
case STABILIZE:
#if FRAME_CONFIG == HELI_FRAME
success = heli_stabilize_init(ignore_checks);
#else
success = stabilize_init(ignore_checks);
#endif
break;
代码跟踪到这里直接匹配的话结果很多，工作量比较大，而且很多类都由这个方法。
此时又得回到 scheduler_tasks数组中了，其中第一个函数是：rc_loop，
{
// Read radio and 3-position switch on radio
// -----------------------------------------
read_radio();
read_control_switch();
}
#define CONTROL_SWITCH_COUNTER 20 // 20 iterations at 100hz (i.e. 2/10th of a second) at a new switch position will cause flight mode change
static void read_control_switch()
{
static uint8_t switch_counter = 0;
uint8_t switchPosition = readSwitch();
// has switch moved?
// ignore flight mode changes if in failsafe
if (oldSwitchPosition != switchPosition && !failsafe.radio && failsafe.radio_counter == 0) {
switch_counter++;
if(switch_counter >= CONTROL_SWITCH_COUNTER) {
oldSwitchPosition = switchPosition;
switch_counter = 0;
// set flight mode and simple mode setting
if (set_mode(flight_modes[switchPosition])) {
if(g.ch7_option != AUX_SWITCH_SIMPLE_MODE && g.ch8_option != AUX_SWITCH_SIMPLE_MODE && g.ch7_option != AUX_SWITCH_SUPERSIMPLE_MODE && g.ch8_option != AUX_SWITCH_SUPERSIMPLE_MODE) {
// set Simple mode using stored paramters from Mission planner
// rather than by the control switch
if (BIT_IS_SET(g.super_simple, switchPosition)) {
set_simple_mode(2);
}else{
set_simple_mode(BIT_IS_SET(g.simple_modes, switchPosition));
}
}
}
}
}else{
// reset switch_counter if there's been no change
// we don't want 10 intermittant blips causing a flight mode change
switch_counter = 0;
}
}
可能你会觉得为什么是在这里？在别的地方其实可以找到想这样的调用：" set_mode(STABILIZE)"，但那不是我们要找的代码，为什么？因为这里才是我们在上位机上设置的飞行模式。也许那部分代码也值得去看一看，但鉴于对 PX4源码还不是很熟悉，暂时就先放一放。
2.stabilize_run
那么现在我们就来看看 stabilize_run这个函数。
// should be called at 100hz or more
static void stabilize_run()
{
int16_t target_roll, target_pitch;
float target_yaw_rate;
int16_t pilot_throttle_scaled;
// if not armed or throttle at zero, set throttle to zero and exit immediately
if(!motors.armed() || g.rc_3.control_in <= 0) {
attitude_control.relax_bf_rate_controller();
attitude_control.set_yaw_target_to_current_heading();
attitude_control.set_throttle_out(0, false);
return;
}
// apply SIMPLE mode transform to pilot inputs
update_simple_mode();
// convert pilot input to lean angles
// To-Do: convert get_pilot_desired_lean_angles to return angles as floats
get_pilot_desired_lean_angles(g.rc_1.control_in, g.rc_2.control_in, target_roll, target_pitch);
// get pilot's desired yaw rate
target_yaw_rate = get_pilot_desired_yaw_rate(g.rc_4.control_in);
// get pilot's desired throttle
pilot_throttle_scaled = get_pilot_desired_throttle(g.rc_3.control_in);
// call attitude controller
attitude_control.angle_ef_roll_pitch_rate_ef_yaw_smooth(target_roll, target_pitch, target_yaw_rate, get_smoothing_gain());
// body-frame rate controller is run directly from 100hz loop
// output pilot's throttle
attitude_control.set_throttle_out(pilot_throttle_scaled, true);
}
这个函数代码倒不是很多，理解起来困难也不是太大。
当然，任何飞行模式都需要区分解锁跟加锁。需要说明的是 PX4采用的是 PID控制器级联的方式进行姿态控制的，即 PD+PID。而我们在这里看到的其实只是 PD控制器。
在没有解锁或者油门为 0的时候，做了三件事。关闭输出自然不必多说，设置机头也能够理解，关键是 "relax_bf_rate_controller"，这是个什么东西？
void AC_AttitudeControl::relax_bf_rate_controller()
{
// ensure zero error in body frame rate controllers
const Vector3f& gyro = _ins.get_gyro();
_rate_bf_target = gyro * AC_ATTITUDE_CONTROL_DEGX100;
}
从代码上看的话，其实就是关闭 PD输出，从而关闭 PID输出。
接下来的 update_simple_mode函数其实包含了简单模式跟超级简单模式。两者都是无头模式，更多的介绍见 APM官网介绍。
然后就是四个通道的映射。其实前面已经映射过一次了，为什么这里还要再映射一次？最直接的解释就是前面映射的并不是我们这里需要的。其实那一步映射每一种飞行模式都需要用到，那我想两次映射也许是 APM觉得比较好的一个做法了吧。
// returns desired angle in centi-degrees
static void get_pilot_desired_lean_angles(int16_t roll_in, int16_t pitch_in, int16_t &roll_out, int16_t &pitch_out)
{
static float _scaler = 1.0;
static int16_t _angle_max = 0;
// range check the input
roll_in = constrain_int16(roll_in, -ROLL_PITCH_INPUT_MAX, ROLL_PITCH_INPUT_MAX);
pitch_in = constrain_int16(pitch_in, -ROLL_PITCH_INPUT_MAX, ROLL_PITCH_INPUT_MAX);
// return filtered roll if no scaling required
if (aparm.angle_max == ROLL_PITCH_INPUT_MAX) {
roll_out = roll_in;
pitch_out = pitch_in;
return;
}
// check if angle_max has been updated and redo scaler
if (aparm.angle_max != _angle_max) {
_angle_max = aparm.angle_max;
_scaler = (float)aparm.angle_max/(float)ROLL_PITCH_INPUT_MAX;
}
// convert pilot input to lean angle
roll_out = (int16_t)((float)roll_in * _scaler);
pitch_out = (int16_t)((float)pitch_in * _scaler);
}
// get_pilot_desired_heading - transform pilot's yaw input into a desired heading
// returns desired angle in centi-degrees
// To-Do: return heading as a float?
static float get_pilot_desired_yaw_rate(int16_t stick_angle)
{
// convert pilot input to the desired yaw rate
return stick_angle * g.acro_yaw_p;
}
上面我们看到的是姿态跟偏航的映射。对于之态，如果行程不一样(默认是+/-45°)则计算一个系数进行修正。而对于偏航则通过一个系数进行调正。但是对于三通道油门就稍微复杂一点，原因是三通道进行了分段：
// used only for manual throttle modes
// returns throttle output 0 to 1000
#define THROTTLE_IN_MIDDLE 500 // the throttle mid point
static int16_t get_pilot_desired_throttle(int16_t throttle_control)
{
int16_t throttle_out;
// exit immediately in the simple cases
if( throttle_control == 0 || g.throttle_mid == 500) {
return throttle_control;
}
// ensure reasonable throttle values
throttle_control = constrain_int16(throttle_control,0,1000);
g.throttle_mid = constrain_int16(g.throttle_mid,300,700);
// check throttle is above, below or in the deadband
if (throttle_control < THROTTLE_IN_MIDDLE) {
// below the deadband
throttle_out = g.throttle_min + ((float)(throttle_control-g.throttle_min))*((float)(g.throttle_mid - g.throttle_min))/((float)(500-g.throttle_min));
}else if(throttle_control > THROTTLE_IN_MIDDLE) {
// above the deadband
throttle_out = g.throttle_mid + ((float)(throttle_control-500))*(float)(1000-g.throttle_mid)/500.0f;
}else{
// must be in the deadband
throttle_out = g.throttle_mid;
}
return throttle_out;
}
虽然进行了分段，但把每一段剖开其实还是等比例映射。最复杂的当然还是 angle_ef_roll_pitch_rate_ef_yaw_smooth，它是 PD控制器，自然不会简单。所以我们还是先从最简单的开始，先来看看 set_throttle_out函数：
// provide 0 to cut motors
void AC_AttitudeControl::set_throttle_out(int16_t throttle_out, bool apply_angle_boost)
{
if (apply_angle_boost) {
_motors.set_throttle(get_angle_boost(throttle_out));
}else{
_motors.set_throttle(throttle_out);
// clear angle_boost for logging purposes
_angle_boost = 0;
}
// update compass with throttle value
// To-Do: find another method to grab the throttle out and feed to the compass. Could be done completely outside this class
//compass.set_throttle((float)g.rc_3.servo_out/1000.0f);
}
void set_throttle(int16_t throttle_in) { _rc_throttle.servo_out = throttle_in; }; // range 0 ~ 1000
其实我们看到，最终将油门数据写入电机是可以选择进行补偿的，补偿方式如下：
// throttle value should be 0 ~ 1000
int16_t AC_AttitudeControl::get_angle_boost(int16_t throttle_pwm)
{
float temp = _ahrs.cos_pitch() * _ahrs.cos_roll();
int16_t throttle_out;
temp = constrain_float(temp, 0.5f, 1.0f);
// reduce throttle if we go inverted
temp = constrain_float(9000-max(labs(_ahrs.roll_sensor),labs(_ahrs.pitch_sensor)), 0, 3000) / (3000 * temp);
// apply scale and constrain throttle
// To-Do: move throttle_min and throttle_max into the AP_Vehicles class?
throttle_out = constrain_float((float)(throttle_pwm-_motors.throttle_min()) * temp + _motors.throttle_min(), _motors.throttle_min(), 1000);
// record angle boost for logging
_angle_boost = throttle_out - throttle_pwm;
return throttle_out;
}
根据注释，这段代码是使用 roll/pitch对油门进行补偿，那么具体又是如何补偿的呢？
==>
throttle_out = (throttle_pwm-_motors.throttle_min()) * temp + _motors.throttle_min();
代码处理之后就好理解得多。其实就是把高于 throttle_min的部分通过 temp进行补偿，所以关键在于 temp的计算。那么这里为了便于理解，我们假定 pitch为 0，即 cos_pitch为 1，那么把代码处理一下：
temp = constrain_float(9000-labs(_ahrs.roll_sensor), 0, 3000) / (3000 * temp);
//正常情况下 labs(_ahrs.roll_sensor)值在 4500以内，所以
float temp = _ahrs.cos_roll();
temp = 3000/ (3000 * temp);
==>
float temp = 1/_ahrs.cos_roll();
所以正常情况下，考虑两个方向的姿态，补偿系数就是： " temp = 1/(_ahrs.cos_pitch() * _ahrs.cos_roll());"。但是从数学上分析，temp是可以无穷大的，显然不能这么去补偿，所以这时候语句 "temp = constrain_float(temp, 0.5f, 1.0f);"就起作用了，它可以对结果进行约束，使得最大补偿为 1倍，即最大 2倍油门输出。当然，这里还有一个变量："_angle_boost"，它是用来记录 log的，这里我们可以不必关心。
那么，现在这里有个问题，就是什么情况下需要进行油门补偿？这个太简单了，匹配一下不就知道了嘛：
ardupilot/ArduCopter/control_guided.pde:95: attitude_control.set_throttle_out(0,false);
ardupilot/ArduCopter/heli_control_acro.pde:46: attitude_control.set_throttle_out(g.rc_3.control_in,false);
ardupilot/ArduCopter/control_autotune.pde:218: attitude_control.set_throttle_out(0,false);
ardupilot/ArduCopter/control_autotune.pde:247: attitude_control.set_throttle_out(0,false);
ardupilot/ArduCopter/control_land.pde:59: attitude_control.set_throttle_out(0,false);
ardupilot/ArduCopter/control_land.pde:126: attitude_control.set_throttle_out(0,false);
ardupilot/ArduCopter/control_auto.pde:108: attitude_control.set_throttle_out(0,false);
ardupilot/ArduCopter/control_auto.pde:157: attitude_control.set_throttle_out(0,false);
ardupilot/ArduCopter/control_auto.pde:215: attitude_control.set_throttle_out(0,false);
ardupilot/ArduCopter/control_auto.pde:284: attitude_control.set_throttle_out(0,false);
ardupilot/ArduCopter/control_flip.pde:175: attitude_control.set_throttle_out(throttle_out,false);
ardupilot/ArduCopter/control_althold.pde:33: attitude_control.set_throttle_out(0,false);
ardupilot/ArduCopter/control_althold.pde:63: attitude_control.set_throttle_out(0,false);
ardupilot/ArduCopter/control_rtl.pde:132: attitude_control.set_throttle_out(0,false);
ardupilot/ArduCopter/control_rtl.pde:190: attitude_control.set_throttle_out(0,false);
ardupilot/ArduCopter/control_rtl.pde:261: attitude_control.set_throttle_out(0,false);
ardupilot/ArduCopter/control_rtl.pde:325: attitude_control.set_throttle_out(0,false);
ardupilot/ArduCopter/control_loiter.pde:40: attitude_control.set_throttle_out(0,false);
ardupilot/ArduCopter/control_loiter.pde:75: attitude_control.set_throttle_out(0,false);
ardupilot/ArduCopter/control_ofloiter.pde:41: attitude_control.set_throttle_out(0,false);
ardupilot/ArduCopter/control_ofloiter.pde:73: attitude_control.set_throttle_out(0,false);
ardupilot/ArduCopter/control_acro.pde:25: attitude_control.set_throttle_out(0,false);
ardupilot/ArduCopter/control_acro.pde:39: attitude_control.set_throttle_out(pilot_throttle_scaled,false);
ardupilot/ArduCopter/control_sport.pde:31: attitude_control.set_throttle_out(0,false);
ardupilot/ArduCopter/control_sport.pde:82: attitude_control.set_throttle_out(0,false);
ardupilot/ArduCopter/control_poshold.pde:162: attitude_control.set_throttle_out(0,false);
ardupilot/ArduCopter/control_poshold.pde:191: attitude_control.set_throttle_out(0,false);
ardupilot/ArduCopter/control_drift.pde:49: attitude_control.set_throttle_out(0,false);
ardupilot/ArduCopter/control_drift.pde:108: attitude_control.set_throttle_out(pilot_throttle_scaled + thr_assist,true);
ardupilot/ArduCopter/control_stabilize.pde:30: attitude_control.set_throttle_out(0,false);
ardupilot/ArduCopter/control_stabilize.pde:53: attitude_control.set_throttle_out(pilot_throttle_scaled,true);
ardupilot/ArduCopter/control_circle.pde:40: attitude_control.set_throttle_out(0,false);
ardupilot/ArduCopter/heli_control_stabilize.pde:57: attitude_control.set_throttle_out(pilot_throttle_scaled,false);
bitcraze@bitcraze-vm:~/apm$
根据匹配结果，只有 stabilize跟 drift两种飞行模式需要对油门进行补偿，为什么？ drift是漂移模式，在 APM网站上是这样解释的：
飘移模式能让用户就像飞行安装有“自动协调转弯”的飞机一样飞行多旋翼飞行器。
用户直接控制Yaw和Pitch，但是Roll是由自动驾驶仪控制的。 如果使用美国手的发射机，可以非常方便的用一个控制杆来控制的飞行器
其实 drift跟 stabilize是相似的，只不过 Roll变成了自动控制。那么这两种飞行模式跟其它的飞行模式之间有什么不同呢？其它一些飞行模式除 acro(特技)外都是带气压计定高的，既然有定高那么自然就不需要油门补偿了，因为油门补偿本身就是为了应对打舵时引起的高度降低。那么为什么特技模式不需要呢？这是因为特技模式本身的特点所决定的，在特技模式遥控器对飞行器进行直接的控制。
那么下面我们就来看看 angle_ef_roll_pitch_rate_ef_yaw_smooth具体是怎么回事。
3.angle_ef_roll_pitch_rate_ef_yaw_smooth
该函数是类 AC_AttitudeControl的方法，实现导航级的 PD控制，但是这里的 PD控制更我们之前用的 PD控制在实现形式上大有不同，传统 PID是按照下面这样的形式来实现的：
从形式上我们也可以看出，这里的控制输入是 e，但 APM并没有这么设计，因为归根结底， PD控制器中， P跟 D它是一个微分的关系，换句话说只要符合 PD这样一个微分关系即可构成 PD控制器。具体我们用代码来进行分析。
不过，在调用该函数的时候，实参除了映射之后的摇杆数据还有一个数据是由函数 get_smoothing_gain获取的，
// result is a number from 2 to 12 with 2 being very sluggish and 12 being very crisp
float get_smoothing_gain()
{
return (2.0f + (float)g.rc_feel_rp/10.0f);
}
从注释来看，这是用来获取平滑的增益的，具体这个增益是做什么用的，我们下面会看到。如果我们在上位机上把该参数给读出来，我们将会看到 g.rc_feel_rp值为 100，即该函数返回 12。
// angle_ef_roll_pitch_rate_ef_yaw_smooth - attempts to maintain a roll and pitch angle and yaw rate (all earth frame) while smoothing the attitude based on the feel parameter
// smoothing_gain : a number from 1 to 50 with 1 being sluggish and 50 being very crisp
void AC_AttitudeControl::angle_ef_roll_pitch_rate_ef_yaw_smooth(float roll_angle_ef,float pitch_angle_ef, float yaw_rate_ef, float smoothing_gain)
{
Vector3f angle_ef_error; // earth frame angle errors
float rate_change_limit;
// sanity check smoothing gain
smoothing_gain = constrain_float(smoothing_gain,1.0f,50.0f);
float linear_angle = _accel_rp_max/(smoothing_gain*smoothing_gain);
rate_change_limit = _accel_rp_max * _dt;
float rate_ef_desired;
float angle_to_target;
if (_accel_rp_max > 0.0f) {
// calculate earth-frame feed forward roll rate using linear response when close to the target, sqrt response when we're further away
angle_to_target = roll_angle_ef - _angle_ef_target.x;
if (angle_to_target > linear_angle) {
rate_ef_desired = safe_sqrt(2.0f*_accel_rp_max*(fabs(angle_to_target)-(linear_angle/2.0f)));
} else if (angle_to_target < -linear_angle) {
rate_ef_desired = -safe_sqrt(2.0f*_accel_rp_max*(fabs(angle_to_target)-(linear_angle/2.0f)));
} else {
rate_ef_desired = smoothing_gain*angle_to_target;
}
_rate_ef_desired.x = constrain_float(rate_ef_desired, _rate_ef_desired.x-rate_change_limit, _rate_ef_desired.x+rate_change_limit);
// update earth-frame roll angle target using desired roll rate
update_ef_roll_angle_and_error(_rate_ef_desired.x, angle_ef_error);
// calculate earth-frame feed forward pitch rate using linear response when close to the target, sqrt response when we're further away
angle_to_target = pitch_angle_ef - _angle_ef_target.y;
if (angle_to_target > linear_angle) {
rate_ef_desired = safe_sqrt(2.0f*_accel_rp_max*(fabs(angle_to_target)-(linear_angle/2.0f)));
} else if (angle_to_target < -linear_angle) {
rate_ef_desired = -safe_sqrt(2.0f*_accel_rp_max*(fabs(angle_to_target)-(linear_angle/2.0f)));
} else {
rate_ef_desired = smoothing_gain*angle_to_target;
}
_rate_ef_desired.y = constrain_float(rate_ef_desired, _rate_ef_desired.y-rate_change_limit, _rate_ef_desired.y+rate_change_limit);
// update earth-frame pitch angle target using desired pitch rate
update_ef_pitch_angle_and_error(_rate_ef_desired.y, angle_ef_error);
} else {
// target roll and pitch to desired input roll and pitch
_angle_ef_target.x = roll_angle_ef;
angle_ef_error.x = wrap_180_cd_float(_angle_ef_target.x - _ahrs.roll_sensor);
_angle_ef_target.y = pitch_angle_ef;
angle_ef_error.y = wrap_180_cd_float(_angle_ef_target.y - _ahrs.pitch_sensor);
// set roll and pitch feed forward to zero
_rate_ef_desired.x = 0;
_rate_ef_desired.y = 0;
}
if (_accel_y_max > 0.0f) {
// set earth-frame feed forward rate for yaw
rate_change_limit = _accel_y_max * _dt;
float rate_change = yaw_rate_ef - _rate_ef_desired.z;
rate_change = constrain_float(rate_change, -rate_change_limit, rate_change_limit);
_rate_ef_desired.z += rate_change;
// calculate yaw target angle and angle error
update_ef_yaw_angle_and_error(_rate_ef_desired.z, angle_ef_error);
} else {
// set yaw feed forward to zero
_rate_ef_desired.z = 0;
// calculate yaw target angle and angle error
update_ef_yaw_angle_and_error(yaw_rate_ef, angle_ef_error);
}
// constrain earth-frame angle targets
_angle_ef_target.x = constrain_float(_angle_ef_target.x, -_aparm.angle_max, _aparm.angle_max);
_angle_ef_target.y = constrain_float(_angle_ef_target.y, -_aparm.angle_max, _aparm.angle_max);
// convert earth-frame angle errors to body-frame angle errors
frame_conversion_ef_to_bf(angle_ef_error, _angle_bf_error);
// convert earth-frame feed forward rates to body-frame feed forward rates
frame_conversion_ef_to_bf(_rate_ef_desired, _rate_bf_desired);
// convert body-frame angle errors to body-frame rate targets
update_rate_bf_targets();
// add body frame rate feed forward
if (_rate_bf_ff_enabled) {
_rate_bf_target += _rate_bf_desired;
}
// body-frame to motor outputs should be called separately
}
我们现在看到的是该函数完整的代码。为了便于理解，我们需要对代码作一些处理，我们假设 _accel_y_max为 0，那么：
// angle_ef_roll_pitch_rate_ef_yaw_smooth - attempts to maintain a roll and pitch angle and yaw rate (all earth frame) while smoothing the attitude based on the feel parameter
// smoothing_gain : a number from 1 to 50 with 1 being sluggish and 50 being very crisp
void AC_AttitudeControl::angle_ef_roll_pitch_rate_ef_yaw_smooth(float roll_angle_ef,float pitch_angle_ef, float yaw_rate_ef, float smoothing_gain)
{
Vector3f angle_ef_error; // earth frame angle errors
float rate_change_limit;
// sanity check smoothing gain
smoothing_gain = constrain_float(smoothing_gain,1.0f,50.0f);
float linear_angle = _accel_rp_max/(smoothing_gain*smoothing_gain);
rate_change_limit = _accel_rp_max * _dt;
float rate_ef_desired;
float angle_to_target;
if (_accel_rp_max > 0.0f) {
} else {
// target roll and pitch to desired input roll and pitch
_angle_ef_target.x = roll_angle_ef;
angle_ef_error.x = wrap_180_cd_float(_angle_ef_target.x - _ahrs.roll_sensor);
_angle_ef_target.y = pitch_angle_ef;
angle_ef_error.y = wrap_180_cd_float(_angle_ef_target.y - _ahrs.pitch_sensor);
// set roll and pitch feed forward to zero
_rate_ef_desired.x = 0;
_rate_ef_desired.y = 0;
}
if (_accel_y_max > 0.0f) {
} else {
// set yaw feed forward to zero
_rate_ef_desired.z = 0;
// calculate yaw target angle and angle error
update_ef_yaw_angle_and_error(yaw_rate_ef, angle_ef_error);
}
// constrain earth-frame angle targets
_angle_ef_target.x = constrain_float(_angle_ef_target.x, -_aparm.angle_max, _aparm.angle_max);
_angle_ef_target.y = constrain_float(_angle_ef_target.y, -_aparm.angle_max, _aparm.angle_max);
// convert earth-frame angle errors to body-frame angle errors
frame_conversion_ef_to_bf(angle_ef_error, _angle_bf_error);
// convert earth-frame feed forward rates to body-frame feed forward rates
frame_conversion_ef_to_bf(_rate_ef_desired, _rate_bf_desired);
// convert body-frame angle errors to body-frame rate targets
update_rate_bf_targets();
// add body frame rate feed forward
if (_rate_bf_ff_enabled) {
_rate_bf_target += _rate_bf_desired;
}
// body-frame to motor outputs should be called separately
}
// update_ef_yaw_angle_and_error - update _angle_ef_target.z using an earth frame yaw rate request
void AC_AttitudeControl::update_ef_yaw_angle_and_error(float yaw_rate_ef, Vector3f &angle_ef_error)
{
// calculate angle error with maximum of +- max angle overshoot
angle_ef_error.z = wrap_180_cd(_angle_ef_target.z - _ahrs.yaw_sensor);
angle_ef_error.z = constrain_float(angle_ef_error.z, -AC_ATTITUDE_RATE_STAB_YAW_OVERSHOOT_ANGLE_MAX, AC_ATTITUDE_RATE_STAB_YAW_OVERSHOOT_ANGLE_MAX);
// update yaw angle target to be within max angle overshoot of our current heading
_angle_ef_target.z = angle_ef_error.z + _ahrs.yaw_sensor;
// increment the yaw angle target
_angle_ef_target.z += yaw_rate_ef * _dt;
_angle_ef_target.z = wrap_360_cd(_angle_ef_target.z);
}
// update_rate_bf_targets - converts body-frame angle error to body-frame rate targets for roll, pitch and yaw axis
// targets rates in centi-degrees taken from _angle_bf_error
// results in centi-degrees/sec put into _rate_bf_target
void AC_AttitudeControl::update_rate_bf_targets()
{
// stab roll calculation
_rate_bf_target.x = _p_angle_roll.kP() * _angle_bf_error.x;
// constrain roll rate request
if (_flags.limit_angle_to_rate_request) {
_rate_bf_target.x = constrain_float(_rate_bf_target.x,-_angle_rate_rp_max,_angle_rate_rp_max);
}
// stab pitch calculation
_rate_bf_target.y = _p_angle_pitch.kP() * _angle_bf_error.y;
// constrain pitch rate request
if (_flags.limit_angle_to_rate_request) {
_rate_bf_target.y = constrain_float(_rate_bf_target.y,-_angle_rate_rp_max,_angle_rate_rp_max);
}
// stab yaw calculation
_rate_bf_target.z = _p_angle_yaw.kP() * _angle_bf_error.z;
// constrain yaw rate request
if (_flags.limit_angle_to_rate_request) {
_rate_bf_target.z = constrain_float(_rate_bf_target.z,-_angle_rate_y_max,_angle_rate_y_max);
}
_rate_bf_target.x += -_angle_bf_error.y * _ins.get_gyro().z;
_rate_bf_target.y += _angle_bf_error.x * _ins.get_gyro().z;
}
把代码处理了一下就很清楚了，原来也只不过是计算角度差 angle_ef_error，然后通过 update_rate_bf_targets函数进行 P运算。函数 frame_conversion_ef_to_bf则是用来进行坐标系之间的转换，将参考坐标系中的误差转换到机体坐标系。但是前面我们不是是说了 PX4姿态级使用的是 PD控制的吗，为什么值看到了 P控制，D呢？别忘了，在函数的最后还有一条语句："_rate_bf_target += _rate_bf_desired"，注释说这是前反馈，实际上就是 D控制，只不过现在我们所看到的是 0而已。
如果我们把参数通过上位机给读出来，我们将会看到这样两个参数：
ATC_ACCEL_Y_MAX,36000
也就是说实际情况是 _accel_rp_max跟 _accel_y_max是大于 0的值，执行的是 if段代码：
float rate_change_limit;
// sanity check smoothing gain
smoothing_gain = constrain_float(smoothing_gain,1.0f,50.0f);
float linear_angle = _accel_rp_max/(smoothing_gain*smoothing_gain);
rate_change_limit = _accel_rp_max * _dt;
float rate_ef_desired;
float angle_to_target;
// calculate earth-frame feed forward roll rate using linear response when close to the target, sqrt response when we're further away
angle_to_target = roll_angle_ef - _angle_ef_target.x;
if (angle_to_target > linear_angle) {
rate_ef_desired = safe_sqrt(2.0f*_accel_rp_max*(fabs(angle_to_target)-(linear_angle/2.0f)));
} else if (angle_to_target < -linear_angle) {
rate_ef_desired = -safe_sqrt(2.0f*_accel_rp_max*(fabs(angle_to_target)-(linear_angle/2.0f)));
} else {
rate_ef_desired = smoothing_gain*angle_to_target;
}
_rate_ef_desired.x = constrain_float(rate_ef_desired, _rate_ef_desired.x-rate_change_limit, _rate_ef_desired.x+rate_change_limit);
// update earth-frame roll angle target using desired roll rate
update_ef_roll_angle_and_error(_rate_ef_desired.x, angle_ef_error);
// calculate earth-frame feed forward pitch rate using linear response when close to the target, sqrt response when we're further away
angle_to_target = pitch_angle_ef - _angle_ef_target.y;
if (angle_to_target > linear_angle) {
rate_ef_desired = safe_sqrt(2.0f*_accel_rp_max*(fabs(angle_to_target)-(linear_angle/2.0f)));
} else if (angle_to_target < -linear_angle) {
rate_ef_desired = -safe_sqrt(2.0f*_accel_rp_max*(fabs(angle_to_target)-(linear_angle/2.0f)));
} else {
rate_ef_desired = smoothing_gain*angle_to_target;
}
_rate_ef_desired.y = constrain_float(rate_ef_desired, _rate_ef_desired.y-rate_change_limit, _rate_ef_desired.y+rate_change_limit);
// update earth-frame pitch angle target using desired pitch rate
update_ef_pitch_angle_and_error(_rate_ef_desired.y, angle_ef_error);
// set earth-frame feed forward rate for yaw
rate_change_limit = _accel_y_max * _dt;
float rate_change = yaw_rate_ef - _rate_ef_desired.z;
rate_change = constrain_float(rate_change, -rate_change_limit, rate_change_limit);
_rate_ef_desired.z += rate_change;
// calculate yaw target angle and angle error
update_ef_yaw_angle_and_error(_rate_ef_desired.z, angle_ef_error);
这段代码分析起来会比较绕。但说透了它其实也就是在 _angle_ef_target跟 _ahrs之间插了一个值，就是 _angle_ef_target。这样的好处是什么呢？好处在于，这样处理之后速率是像抛物线一样先增后减的，而不是一打舵猛地给速率控制器一个很高的速率，然后慢慢往 0靠拢。按我的说法就是 PX4在导航级做了一个平滑处理。想了解更多的细节我们还需要去看下 update_ef_roll_angle_and_error跟 update_ef_pitch_angle_and_error函数：
void AC_AttitudeControl::update_ef_roll_angle_and_error(float roll_rate_ef, Vector3f &angle_ef_error)
{
// calculate angle error with maximum of +- max angle overshoot
angle_ef_error.x = wrap_180_cd(_angle_ef_target.x - _ahrs.roll_sensor);
angle_ef_error.x = constrain_float(angle_ef_error.x, -AC_ATTITUDE_RATE_STAB_ROLL_OVERSHOOT_ANGLE_MAX, AC_ATTITUDE_RATE_STAB_ROLL_OVERSHOOT_ANGLE_MAX);
// update roll angle target to be within max angle overshoot of our roll angle
_angle_ef_target.x = angle_ef_error.x + _ahrs.roll_sensor;
// increment the roll angle target
_angle_ef_target.x += roll_rate_ef * _dt;
_angle_ef_target.x = wrap_180_cd(_angle_ef_target.x);
}
// update_ef_pitch_angle_and_error - update _angle_ef_target.y using an earth frame pitch rate request
void AC_AttitudeControl::update_ef_pitch_angle_and_error(float pitch_rate_ef, Vector3f &angle_ef_error)
{
// calculate angle error with maximum of +- max angle overshoot
// To-Do: should we do something better as we cross 90 degrees?
angle_ef_error.y = wrap_180_cd(_angle_ef_target.y - _ahrs.pitch_sensor);
angle_ef_error.y = constrain_float(angle_ef_error.y, -AC_ATTITUDE_RATE_STAB_PITCH_OVERSHOOT_ANGLE_MAX, AC_ATTITUDE_RATE_STAB_PITCH_OVERSHOOT_ANGLE_MAX);
// update pitch angle target to be within max angle overshoot of our pitch angle
_angle_ef_target.y = angle_ef_error.y + _ahrs.pitch_sensor;
// increment the pitch angle target
_angle_ef_target.y += pitch_rate_ef * _dt;
_angle_ef_target.y = wrap_180_cd(_angle_ef_target.y);
}
这样我们就清楚了，PX4是通过 smoothing_gain得到一个新的速率 rate_ef_desired，然后用这个新的速率去更新 _angle_ef_target，并使用 _angle_ef_target计算姿态误差参与控制。这个时候可能你会觉得 linear_angle是用来干嘛的？通过前面我们给出的数据，可以算出 linear_angle值为 875，PX4是精确到 0.01°，所以对应的是 8.75°。也就是说在 8.75°以内是一个线性区域，速率是线性变化的，超出这个角度就采用飞线性的方式计算 rate_ef_desired。具体是什么方式呢？我们可以根据我们的数据稍微整理下得到：
==>
rate_ef_desired = safe_sqrt(252000)*safe_sqrt(fabs(angle_to_target)-437.5);
==>
rate_ef_desired = 501*safe_sqrt(fabs(angle_to_target)-437.5);
当然，如果能够写成数学表达式就更加清晰了。
而这个时候，由于 _rate_ef_desired不为 0，所以 _rate_bf_desired也不为 0。而且从计算过程来看，PX4是通过对 _rate_ef_desired积分计算姿态，所以姿态级使用的是 PD控制，只不过将 PD控制器中的主角稍稍换了一下。
那么下面我们将要看到的是姿态控制中相当重要的一环：速率控制器，即 PID控制器。
4.rate_controller_run
在源码 " ardupilot/ArduCopter/ArduCopter.pde"中有这样一段代码：
static void fast_loop()
{
// IMU DCM Algorithm
// --------------------
read_AHRS();
// run low level rate controllers that only require IMU data
attitude_control.rate_controller_run();
我们没知道，fast_loop是快速循环，直接由 loop调用。而这里的 rate_controller_run便是我们的 PID控制器，即速率控制器。
// should be called at 100hz or more
void AC_AttitudeControl::rate_controller_run()
{
// call rate controllers and send output to motors object
// To-Do: should the outputs from get_rate_roll, pitch, yaw be int16_t which is the input to the motors library?
// To-Do: skip this step if the throttle out is zero?
_motors.set_roll(rate_bf_to_motor_roll(_rate_bf_target.x));
_motors.set_pitch(rate_bf_to_motor_pitch(_rate_bf_target.y));
_motors.set_yaw(rate_bf_to_motor_yaw(_rate_bf_target.z));
}
_rate_bf_target就是前面导航级计算得到的速率，rate_bf_to_motor_用来把速率转换成输出，它用的就是我们传统的 PID形式。
float AC_AttitudeControl::rate_bf_to_motor_roll(float rate_target_cds)
{
float p,i,d; // used to capture pid values for logging
float current_rate; // this iteration's rate
float rate_error; // simply target_rate - current_rate
// get current rate
// To-Do: make getting gyro rates more efficient?
current_rate = (_ins.get_gyro().x * AC_ATTITUDE_CONTROL_DEGX100);
// calculate error and call pid controller
rate_error = rate_target_cds - current_rate;
p = _pid_rate_roll.get_p(rate_error);
// get i term
i = _pid_rate_roll.get_integrator();
// update i term as long as we haven't breached the limits or the I term will certainly reduce
if (!_motors.limit.roll_pitch
前面我们看过了 PX4的 " stabilize"模式，" althold"完整的叫做："Altitude Hold"，即高度保持模式，也就是定高模式。"althold"跟 " stabilize"想比最明显的特点是油门不控制电机的平均转数，而是控制升降速率，也就是油门摇杆回中的时候保持当前高度不变。其姿态级导航源码在源文件 "ardupilot/ArduCopter/control_althold.pde"中。它里边也同样只有两个函数：init跟 run。当然它们所起的作用也是一样的。
static bool althold_init(bool ignore_checks)
{
// initialize vertical speeds and leash lengths
pos_control.set_speed_z(-g.pilot_velocity_z_max, g.pilot_velocity_z_max);
pos_control.set_accel_z(g.pilot_accel_z);
// initialise altitude target to stopping point
pos_control.set_target_to_stopping_point_z();
return true;
}
我们看到 init都是对 pos_control进行初始化，前两个是设置升降速率跟加速度，其实是设置了升降速率的最大值，为 250，即 2.5 m/s。加速度同样是 250，即 2.5 m/s/s。
/// To-Do: call this in the main code as part of flight mode initialisation
/// calc_leash_length_z should be called afterwards
/// speed_down should be a negative number
void AC_PosControl::set_speed_z(float speed_down,float speed_up)
{
// ensure speed_down is always negative
speed_down = -fabs(speed_down);
if ((fabs(_speed_down_cms-speed_down) > 1.0f) || (fabs(_speed_up_cms-speed_up) > 1.0f)) {
_speed_down_cms = speed_down;
_speed_up_cms = speed_up;
_flags.recalc_leash_z = true;
}
}
/// set_accel_z - set vertical acceleration in cm/s/s
void AC_PosControl::set_accel_z(float accel_cmss)
{
if (fabs(_accel_z_cms-accel_cmss) > 1.0f) {
_accel_z_cms = accel_cmss;
_flags.recalc_leash_z = true;
}
}
从代码看，要确定 " g.pilot_velocity_z_max"为升降速率的极限值只需要匹配一下就可以了：
./ardupilot/libraries/AC_AttitudeControl/AC_PosControl.cpp:41: _speed_down_cms(POSCONTROL_SPEED_DOWN),
./ardupilot/libraries/AC_AttitudeControl/AC_PosControl.cpp:96: if ((fabs(_speed_down_cms-speed_down) > 1.0f) || (fabs(_speed_up_cms-speed_up) > 1.0f)) {
./ardupilot/libraries/AC_AttitudeControl/AC_PosControl.cpp:97: _speed_down_cms = speed_down;
./ardupilot/libraries/AC_AttitudeControl/AC_PosControl.cpp:122: _pos_target.z += constrain_float(alt_change, _speed_down_cms*dt, _speed_up_cms*dt);
./ardupilot/libraries/AC_AttitudeControl/AC_PosControl.cpp:232: _leash_down_z = calc_leash_length(-_speed_down_cms, _accel_z_cms, _p_alt_pos.kP());
./ardupilot/libraries/AC_AttitudeControl/AC_PosControl.cpp:300: if (_vel_target.z < _speed_down_cms) {
./ardupilot/libraries/AC_AttitudeControl/AC_PosControl.cpp:301: _vel_target.z = _speed_down_cms;
./ardupilot/libraries/AC_AttitudeControl/AC_PosControl.h:80: float get_speed_down()const { return _speed_down_cms; }
./ardupilot/libraries/AC_AttitudeControl/AC_PosControl.h:340: float _speed_down_cms; // max descent rate in cm/s
bitcraze@bitcraze-vm:~/apm$
288 // rate_to_accel_z - calculates desired accel required to achieve the velocity target
289 // calculates desired acceleration and calls accel throttle controller
290 void AC_PosControl::rate_to_accel_z()
291 {
292 const Vector3f& curr_vel = _inav.get_velocity();
293 float p; // used to capture pid values for logging
294 float desired_accel; // the target acceleration if the accel based throttle is enabled, otherwise the output to be sent to the motors
295
296 // check speed limits
297 // To-Do: check these speed limits here or in the pos->rate controller
298 _limit.vel_up = false;
299 _limit.vel_down = false;
300 if (_vel_target.z < _speed_down_cms) {
301 _vel_target.z = _speed_down_cms;
302 _limit.vel_down = true;
303 }
304 if (_vel_target.z > _speed_up_cms) {
305 _vel_target.z = _speed_up_cms;
306 _limit.vel_up = true;
307 }
匹配结果非常直观，用来限制 _vel_target.z的大小。函数 rate_to_accel_z是定高里边很重要的一个函数，后面我们会具体碰到，这里就先不分析这个函数了。对于加计我们可以使用同样的方式进行处理：
./ardupilot/libraries/AC_AttitudeControl/AC_PosControl.cpp:44: _accel_z_cms(POSCONTROL_ACCEL_Z),
./ardupilot/libraries/AC_AttitudeControl/AC_PosControl.cpp:106: if (fabs(_accel_z_cms-accel_cmss) > 1.0f) {
./ardupilot/libraries/AC_AttitudeControl/AC_PosControl.cpp:107: _accel_z_cms = accel_cmss;
./ardupilot/libraries/AC_AttitudeControl/AC_PosControl.cpp:173: linear_velocity = _accel_z_cms/_p_alt_pos.kP();
./ardupilot/libraries/AC_AttitudeControl/AC_PosControl.cpp:179: linear_distance = _accel_z_cms/(2.0f*_p_alt_pos.kP()*_p_alt_pos.kP());
./ardupilot/libraries/AC_AttitudeControl/AC_PosControl.cpp:181: stopping_point.z = curr_pos_z + (linear_distance + curr_vel_z*curr_vel_z/(2.0f*_accel_z_cms));
./ardupilot/libraries/AC_AttitudeControl/AC_PosControl.cpp:183: stopping_point.z = curr_pos_z - (linear_distance + curr_vel_z*curr_vel_z/(2.0f*_accel_z_cms));
./ardupilot/libraries/AC_AttitudeControl/AC_PosControl.cpp:231: _leash_up_z = calc_leash_length(_speed_up_cms, _accel_z_cms, _p_alt_pos.kP());
./ardupilot/libraries/AC_AttitudeControl/AC_PosControl.cpp:232: _leash_down_z = calc_leash_length(-_speed_down_cms, _accel_z_cms, _p_alt_pos.kP());
./ardupilot/libraries/AC_AttitudeControl/AC_PosControl.cpp:272: linear_distance = _accel_z_cms/(2.0f*_p_alt_pos.kP()*_p_alt_pos.kP());
./ardupilot/libraries/AC_AttitudeControl/AC_PosControl.cpp:274: _vel_target.z = safe_sqrt(2.0f*_accel_z_cms*(_pos_error.z-linear_distance));
./ardupilot/libraries/AC_AttitudeControl/AC_PosControl.cpp:276: _vel_target.z = -safe_sqrt(2.0f*_accel_z_cms*(-_pos_error.z-linear_distance));
./ardupilot/libraries/AC_AttitudeControl/AC_PosControl.h:87: float get_accel_z()const { return _accel_z_cms; }
./ardupilot/libraries/AC_AttitudeControl/AC_PosControl.h:343: float _accel_z_cms; // max vertical acceleration in cm/s/s
bitcraze@bitcraze-vm:~/apm$
270 // check kP to avoid division by zero
271 if (_p_alt_pos.kP() != 0) {
272 linear_distance = _accel_z_cms/(2.0f*_p_alt_pos.kP()*_p_alt_pos.kP());
273 if (_pos_error.z > 2*linear_distance ) {
274 _vel_target.z = safe_sqrt(2.0f*_accel_z_cms*(_pos_error.z-linear_distance));
275 }else if (_pos_error.z < -2.0f*linear_distance) {
276 _vel_target.z = -safe_sqrt(2.0f*_accel_z_cms*(-_pos_error.z-linear_distance));
277 }else{
278 _vel_target.z = _p_alt_pos.get_p(_pos_error.z);
279 }
280 }else{
281 _vel_target.z = 0;
282 }
283
284 // call rate based throttle controller which will update accel based throttle controller targets
285 rate_to_accel_z();
286 }
这段代码出自函数 pos_to_rate_z，也是我们后面要接触的函数。实际上它跟我们前面 " stabilize"模式中导航级的处理有几分相像，都有一个线性区域跟非线性区域，细节我们暂时就不去分析了。
但是 set_target_to_stopping_point_z较前面两个就稍微不那么好理解，根据注释它是用来初始化目标高度的，但是 "stopping point"又怎么去理解？如果按照 “停止点”来翻译，那就应当值的是初始高度，显然跟 "target"是矛盾的。
void AC_PosControl::set_target_to_stopping_point_z()
{
// check if z leash needs to be recalculated
calc_leash_length_z();
get_stopping_point_z(_pos_target);
}
/// calc_leash_length - calculates the vertical leash lengths from maximum speed, acceleration
/// called by pos_to_rate_z if z-axis speed or accelerations are changed
void AC_PosControl::calc_leash_length_z()
{
if (_flags.recalc_leash_z) {
_leash_up_z = calc_leash_length(_speed_up_cms, _accel_z_cms, _p_alt_pos.kP());
_leash_down_z = calc_leash_length(-_speed_down_cms, _accel_z_cms, _p_alt_pos.kP());
_flags.recalc_leash_z = false;
}
}
/// get_stopping_point_z - sets stopping_point.z to a reasonable stopping altitude in cm above home
void AC_PosControl::get_stopping_point_z(Vector3f& stopping_point)const
{
const float curr_pos_z = _inav.get_altitude();
float curr_vel_z = _inav.get_velocity_z();
float linear_distance; // half the distance we swap between linear and sqrt and the distance we offset sqrt
float linear_velocity; // the velocity we swap between linear and sqrt
// if position controller is active add current velocity error to avoid sudden jump in acceleration
if (is_active_z()) {
curr_vel_z += _vel_error.z;
}
// calculate the velocity at which we switch from calculating the stopping point using a linear function to a sqrt function
linear_velocity = _accel_z_cms/_p_alt_pos.kP();
if (fabs(curr_vel_z) < linear_velocity) {
// if our current velocity is below the cross-over point we use a linear function
stopping_point.z = curr_pos_z + curr_vel_z/_p_alt_pos.kP();
} else {
linear_distance = _accel_z_cms/(2.0f*_p_alt_pos.kP()*_p_alt_pos.kP());
if (curr_vel_z > 0){
stopping_point.z = curr_pos_z + (linear_distance + curr_vel_z*curr_vel_z/(2.0f*_accel_z_cms));
} else {
stopping_point.z = curr_pos_z - (linear_distance + curr_vel_z*curr_vel_z/(2.0f*_accel_z_cms));
}
}
stopping_point.z = constrain_float(stopping_point.z, curr_pos_z - POSCONTROL_STOPPING_DIST_Z_MAX, curr_pos_z + POSCONTROL_STOPPING_DIST_Z_MAX);
}
这里主要设置了三个数据：_leash_up_z、_leash_down_z和 _pos_target。前两个是极值，谁的呢？高度差的极值。而 _pos_target其实包含了 xyz，z是高度，xy是经纬度。这个暂时就不去证实了。
2.althold_run
与 " stabilize"想比，run函数还是有几分相似的。其实我们我不难想到，除了油门，两种飞行模式应该都是一样的，所以：
// should be called at 100hz or more
static void althold_run()
{
int16_t target_roll, target_pitch;
float target_yaw_rate;
int16_t target_climb_rate;
// if not auto armed set throttle to zero and exit immediately
if(!ap.auto_armed) {
// To-Do: reset altitude target if we're somehow not landed?
attitude_control.relax_bf_rate_controller();
attitude_control.set_yaw_target_to_current_heading();
attitude_control.set_throttle_out(0, false);
return;
}
// apply SIMPLE mode transform to pilot inputs
update_simple_mode();
// get pilot desired lean angles
// To-Do: convert get_pilot_desired_lean_angles to return angles as floats
get_pilot_desired_lean_angles(g.rc_1.control_in, g.rc_2.control_in, target_roll, target_pitch);
// get pilot's desired yaw rate
target_yaw_rate = get_pilot_desired_yaw_rate(g.rc_4.control_in);
// get pilot desired climb rate
target_climb_rate = get_pilot_desired_climb_rate(g.rc_3.control_in);
// check for pilot requested take-off
if (ap.land_complete && target_climb_rate > 0) {
// indicate we are taking off
set_land_complete(false);
// clear i term when we're taking off
set_throttle_takeoff();
}
// reset target lean angles and heading while landed
if (ap.land_complete) {
attitude_control.relax_bf_rate_controller();
attitude_control.set_yaw_target_to_current_heading();
// move throttle to minimum to keep us on the ground
attitude_control.set_throttle_out(0, false);
}else{
// call attitude controller
attitude_control.angle_ef_roll_pitch_rate_ef_yaw_smooth(target_roll, target_pitch, target_yaw_rate, get_smoothing_gain());
// body-frame rate controller is run directly from 100hz loop
// call throttle controller
if (sonar_alt_health >= SONAR_ALT_HEALTH_MAX) {
// if sonar is ok, use surface tracking
target_climb_rate = get_throttle_surface_tracking(target_climb_rate, pos_control.get_alt_target(), G_Dt);
}
// call position controller
pos_control.set_alt_target_from_climb_rate(target_climb_rate, G_Dt);
pos_control.update_z_controller();
}
}
我们看到这个时候 3通由油门变成了速率。它是按照下面的方式进行映射的：
// climb rate in cm/s. we use radio_in instead of control_in to get the full range
// without any deadzone at the bottom
#define THROTTLE_IN_DEADBAND_TOP (THROTTLE_IN_MIDDLE+THROTTLE_IN_DEADBAND) // top of the deadband
#define THROTTLE_IN_DEADBAND_BOTTOM (THROTTLE_IN_MIDDLE-THROTTLE_IN_DEADBAND) // bottom of the deadband
static int16_t get_pilot_desired_climb_rate(int16_t throttle_control)
{
int16_t desired_rate = 0;
// throttle failsafe check
if( failsafe.radio ) {
return 0;
}
// ensure a reasonable throttle value
throttle_control = constrain_int16(throttle_control,0,1000);
// check throttle is above, below or in the deadband
if (throttle_control < THROTTLE_IN_DEADBAND_BOTTOM) {
// below the deadband
desired_rate = (int32_t)g.pilot_velocity_z_max * (throttle_control-THROTTLE_IN_DEADBAND_BOTTOM) / (THROTTLE_IN_MIDDLE - THROTTLE_IN_DEADBAND);
}else if (throttle_control > THROTTLE_IN_DEADBAND_TOP) {
// above the deadband
desired_rate = (int32_t)g.pilot_velocity_z_max * (throttle_control-THROTTLE_IN_DEADBAND_TOP) / (THROTTLE_IN_MIDDLE - THROTTLE_IN_DEADBAND);
}else{
// must be in the deadband
desired_rate = 0;
}
// desired climb rate for logging
desired_climb_rate = desired_rate;
return desired_rate;
}
也就是摇杆在中间的时候速率为 0，这中间还有一个死区。当摇杆不在中间的时候，就以 pilot_velocity_z_max为最大速率进行映射，映射的当然也只是超出死区的部分。
在前面的笔记中我说过 ap.land_complete是用于落地检测的，这个其实也不难理解，因为落地之后需要保持在怠速状态，而定高模式因为油门拉到最低并不表示已经着地，所以需要通过其它办法来检测是否已经着地，而这个检测的结果就由 ap.land_complete进行保存。所以，根据代码分析，定高模式起飞油门不是不再中间，而是应该在上半部，毕竟飞行器解锁时已经处于地面了，总不能往地底下钻。
当飞行器着陆时，如果油门摇杆处于中间以下，将执行 "reset target lean angles and heading while landed"部分代码，做了三件事，前两件事跟没解锁的时候是一样的，关闭控制级输出并不停的重置机头。而第三件事我们当前看到的跟未解锁也是一样的，但其实在早起版本中并不是这样，而是："attitude_control.set_throttle_out(get_throttle_pre_takeoff(g.rc_3.control_in), false);"。但看到这段代码我想问的是，解锁后起飞前电机不是以怠速在转的吗，既然都跟没解锁的时候都一样，那电机岂不是就不转了？关于这点呢，因为我通常都是以 "stabilize"起飞，所以并不知道真实情况是怎样的。只是就我目前看到的代码而论，如果以定高模式起飞，这个时候电机应该是不转的。
而当摇杆打到上半部(速率大于 0)时，从代码来看，将先做两件事。首先当然是修改 ap.land_complete的值，因为这个时候就要起飞了，不能再让控制器以为飞行器是停在地面上的，这个工作由 set_land_complete函数来完成。这第二件事情就是清除积分，由 set_throttle_takeoff函数来完成。
static void
set_throttle_takeoff()
{
// tell position controller to reset alt target and reset I terms
pos_control.init_takeoff();
// tell motors to do a slow start
motors.slow_start(true);
}
/// init_takeoff - initialises target altitude if we are taking off
void AC_PosControl::init_takeoff()
{
const Vector3f& curr_pos = _inav.get_position();
_pos_target.z = curr_pos.z + POSCONTROL_TAKEOFF_JUMP_CM;
// clear i term from acceleration controller
if (_pid_alt_accel.get_integrator() < 0) {
_pid_alt_accel.reset_I();
}
}
// slow_start - set to true to slew motors from current speed to maximum
// Note: this must be set immediately before a step up in throttle
void AP_Motors::slow_start(bool true_false)
{
// set slow_start flag
_flags.slow_start = true;
// initialise maximum throttle to current throttle
_max_throttle = constrain_int16(_rc_throttle.servo_out, 0, AP_MOTORS_DEFAULT_MAX_THROTTLE);
}
其实清除积分只是它所做的一件事情而已，它还设置了 _pos_target.z跟 _max_throttle的值。前者是目标高度，这个我想不必过多解释，那么后者是什么？最简单的办法当然是匹配源码，
./ardupilot/libraries/AP_Motors/AP_MotorsMatrix.cpp:139: if (_rc_throttle.servo_out > _max_throttle) {
./ardupilot/libraries/AP_Motors/AP_MotorsMatrix.cpp:140: _rc_throttle.servo_out = _max_throttle;
./ardupilot/libraries/AP_Motors/AP_MotorsSingle.cpp:161: _rc_throttle.servo_out = constrain_int16(_rc_throttle.servo_out, 0, _max_throttle);
./ardupilot/libraries/AP_Motors/AP_Motors_Class.cpp:81: _max_throttle(AP_MOTORS_DEFAULT_MAX_THROTTLE),
./ardupilot/libraries/AP_Motors/AP_Motors_Class.cpp:137: update_max_throttle();
./ardupilot/libraries/AP_Motors/AP_Motors_Class.cpp:192: _max_throttle = constrain_int16(_rc_throttle.servo_out, 0, AP_MOTORS_DEFAULT_MAX_THROTTLE);
./ardupilot/libraries/AP_Motors/AP_Motors_Class.cpp:195:// update_max_throttle - updates the limits on _max_throttle if necessary taking into account slow_start_throttle flag
./ardupilot/libraries/AP_Motors/AP_Motors_Class.cpp:196:void AP_Motors::update_max_throttle()
./ardupilot/libraries/AP_Motors/AP_Motors_Class.cpp:213: _max_throttle += AP_MOTOR_SLOW_START_INCREMENT;
./ardupilot/libraries/AP_Motors/AP_Motors_Class.cpp:216: if (_max_throttle >= _rc_throttle.servo_out) {
./ardupilot/libraries/AP_Motors/AP_Motors_Class.cpp:217: _max_throttle = AP_MOTORS_DEFAULT_MAX_THROTTLE;
./ardupilot/libraries/AP_Motors/AP_MotorsTri.cpp:94: _rc_throttle.servo_out = constrain_int16(_rc_throttle.servo_out, 0, _max_throttle);
./ardupilot/libraries/AP_Motors/AP_Motors_Class.h:97: int16_t throttle_max()const { return _max_throttle;}
./ardupilot/libraries/AP_Motors/AP_Motors_Class.h:147: // update_max_throttle - updates the limits on _max_throttle if necessary taking into account slow_start_throttle flag
./ardupilot/libraries/AP_Motors/AP_Motors_Class.h:148: void update_max_throttle();
./ardupilot/libraries/AP_Motors/AP_Motors_Class.h:175: int16_t _max_throttle; // the maximum throttle to be sent to the motors (sometimes limited by slow start)
./ardupilot/libraries/AP_Motors/AP_MotorsCoax.cpp:155: _rc_throttle.servo_out = constrain_int16(_rc_throttle.servo_out, 0, _max_throttle);
bitcraze@bitcraze-vm:~/apm$
根据源文件 AP_MotorsMatrix.cpp的结果，我们知道 _max_throttle是用来对油门进行约束的，更具体点是什么作用呢？
void AP_Motors::update_max_throttle()
{
// ramp up minimum spin speed if necessary
if (_flags.slow_start_low_end) {
_spin_when_armed_ramped += AP_MOTOR_SLOW_START_LOW_END_INCREMENT;
if (_spin_when_armed_ramped >= _spin_when_armed) {
_spin_when_armed_ramped = _spin_when_armed;
_flags.slow_start_low_end = false;
}
}
// return max throttle if we're not slow_starting
if (!_flags.slow_start) {
return;
}
// increase slow start throttle
_max_throttle += AP_MOTOR_SLOW_START_INCREMENT;
// turn off slow start if we've reached max throttle
if (_max_throttle >= _rc_throttle.servo_out) {
_max_throttle = AP_MOTORS_DEFAULT_MAX_THROTTLE;
_flags.slow_start = false;
}
}
// output - sends commands to the motors
void AP_Motors::output()
{
// update max throttle
update_max_throttle();
// output to motors
if (_flags.armed ) {
output_armed();
}else{
output_disarmed();
}
};
所以我们看到，_max_throttle是缓慢增大的。也就是说，如果电机想要最大转速运行是需要一个时间的，在这个时间以内即便油门输出已经最大了电机的实际转速并不一定很大。那这么处理可有什么好处呢？这么说吧，我之前自己写代码的时候，起飞的时候老是会忘一边斜，但是参考这段代码进行处理之后就基本上实现了垂直起飞了。这就是它的作用。按我的话说就是它让飞控在起飞的时候有足够的反应时间来调整姿态。因为起飞的时候并不是很稳定，可能是太接近地面收到了风的影响吧，具体原因我没有进一步深究。而且咱又不搞空气动力学，大多数我想也就了解四轴飞行器的基本飞行原理，更深入的空气动力学原理恐怕了解也不多。所以问题就这样很意外地解决了。
那么最后就是我们的重头戏："call throttle controller"，姿态控制呢跟 " stabilize"是一样的，这个回过头去看前一篇笔记就可以了。
if (sonar_alt_health >= SONAR_ALT_HEALTH_MAX) {
// if sonar is ok, use surface tracking
target_climb_rate = get_throttle_surface_tracking(target_climb_rate, pos_control.get_alt_target(), G_Dt);
}
// call position controller
pos_control.set_alt_target_from_climb_rate(target_climb_rate, G_Dt);
pos_control.update_z_controller();
其中 "sonar_alt_health"部分是超声波的，我之前飞行的时候没有使用超声波，所以先跳过。而在这里，set_alt_target_from_climb_rate只是更新目标高度，而 update_z_controller才是对高度进行控制。
/// should be called continuously (with dt set to be the expected time between calls)
/// actual position target will be moved no faster than the speed_down and speed_up
/// target will also be stopped if the motors hit their limits or leash length is exceeded
void AC_PosControl::set_alt_target_from_climb_rate(float climb_rate_cms,float dt)
{
// adjust desired alt if motors have not hit their limits
// To-Do: add check of _limit.pos_up and _limit.pos_down?
if ((climb_rate_cms<0 && !_motors.limit.throttle_lower) || (climb_rate_cms>0 && !_motors.limit.throttle_upper)) {
_pos_target.z += climb_rate_cms * dt;
}
}
/// update_z_controller - fly to altitude in cm above home
void AC_PosControl::update_z_controller()
{
// check time since last cast
uint32_t now = hal.scheduler->millis();
if (now - _last_update_z_ms > POSCONTROL_ACTIVE_TIMEOUT_MS) {
_flags.reset_rate_to_accel_z = true;
_flags.reset_accel_to_throttle = true;
}
_last_update_z_ms = now;
// check if leash lengths need to be recalculated
calc_leash_length_z();
// call position controller
pos_to_rate_z();
}
设置目标高度其实也就是对速率进行积分。但我们也看到，update_z_controller函数主要是检查是否超时，并重置数据。calc_leash_length_z函数前面在初始化的时候我们已经遇到过了，计算误差的极值的。所以最后把核心任务还是交给了 pos_to_rate_z函数。然后才是真正进入了 PID控制器。定高的 PID控制器总共有三级，分别是 pos_to_rate_z、rate_to_accel_z跟 accel_to_throttle。那么下面我们就来瞧瞧这几个控制器。
3. PID
我们先来看一张图：
这是从 APM网站上下载下来的，描述了 PX4中定高的整体框图。就这张图分析，定高过程中有三个量参与控制：Altitude、Rate跟 Accelerometer。而控制器是以 P、PID、PID的方式级联的。接下来我们就结合代码作更深入的了解。
首先当然是 pos_to_rate_z函数，
// calculates desired rate in earth-frame z axis and passes to rate controller
// vel_up_max, vel_down_max should have already been set before calling this method
void AC_PosControl::pos_to_rate_z()
{
float curr_alt = _inav.get_altitude();
float linear_distance; // half the distance we swap between linear and sqrt and the distance we offset sqrt.
// clear position limit flags
_limit.pos_up = false;
_limit.pos_down = false;
// do not let target alt get above limit
if (_alt_max > 0 && _pos_target.z > _alt_max) {
_pos_target.z = _alt_max;
_limit.pos_up = true;
}
// calculate altitude error
_pos_error.z = _pos_target.z - curr_alt;
// do not let target altitude get too far from current altitude
if (_pos_error.z > _leash_up_z) {
_pos_target.z = curr_alt + _leash_up_z;
_pos_error.z = _leash_up_z;
_limit.pos_up = true;
}
if (_pos_error.z < -_leash_down_z) {
_pos_target.z = curr_alt - _leash_down_z;
_pos_error.z = -_leash_down_z;
_limit.pos_down = true;
}
// check kP to avoid division by zero
if (_p_alt_pos.kP() != 0) {
linear_distance = _accel_z_cms/(2.0f*_p_alt_pos.kP()*_p_alt_pos.kP());
if (_pos_error.z > 2*linear_distance ) {
_vel_target.z = safe_sqrt(2.0f*_accel_z_cms*(_pos_error.z-linear_distance));
}else if (_pos_error.z < -2.0f*linear_distance) {
_vel_target.z = -safe_sqrt(2.0f*_accel_z_cms*(-_pos_error.z-linear_distance));
}else{
_vel_target.z = _p_alt_pos.get_p(_pos_error.z);
}
}else{
_vel_target.z = 0;
}
// call rate based throttle controller which will update accel based throttle controller targets
rate_to_accel_z();
}
这段代码我们看到其中有一半的篇幅是用来对数据进行检查的，主要检查了两个值，最大高度跟最大偏差，最大偏差实际上可以理解为控制器允许的最大输入量。当然有一点我们需要搞清楚，就是这里高度的单位是 "cm"。这个简单查看下 _pos_target变量定义时候的注释就清楚了。关于极限值，前面我们已经讲过了，这里就不重复。
接下来就是我们的 P控制器。但是从代码看，这里同样是分为线性区跟非线性区。当处于线性区域的时候就是我们熟悉的 P控制，但如果不再线性区域时就稍稍有点复杂了。
这里我们需要先确定两个值：_accel_z_cms跟 kP。默认值是 250跟 1，所以 linear_distance的值为 125，也就是线性区域的临界点是 250。于是稍微整理一下我们就得到：
==>
_vel_target.z = safe_sqrt(500*(_pos_error.z-125));
这一步计算得到的是下一级控制器的控制量，即以多大的速率升降。根据框图我们知道其极限值是 250，当然，这个我就不去算了。然后就是 rate_to_accel_z对速率进行 PID。
// calculates desired acceleration and calls accel throttle controller
void AC_PosControl::rate_to_accel_z()
{
const Vector3f& curr_vel = _inav.get_velocity();
float p; // used to capture pid values for logging
float desired_accel; // the target acceleration if the accel based throttle is enabled, otherwise the output to be sent to the motors
// check speed limits
// To-Do: check these speed limits here or in the pos->rate controller
_limit.vel_up = false;
_limit.vel_down = false;
if (_vel_target.z < _speed_down_cms) {
_vel_target.z = _speed_down_cms;
_limit.vel_down = true;
}
if (_vel_target.z > _speed_up_cms) {
_vel_target.z = _speed_up_cms;
_limit.vel_up = true;
}
// reset last velocity target to current target
if (_flags.reset_rate_to_accel_z) {
_vel_last.z = _vel_target.z;
_flags.reset_rate_to_accel_z = false;
}
// feed forward desired acceleration calculation
if (_dt > 0.0f) {
if (!_flags.freeze_ff_z) {
_accel_feedforward.z = (_vel_target.z - _vel_last.z)/_dt;
} else {
// stop the feed forward being calculated during a known discontinuity
_flags.freeze_ff_z = false;
}
} else {
_accel_feedforward.z = 0.0f;
}
// store this iteration's velocities for the next iteration
_vel_last.z = _vel_target.z;
// reset velocity error and filter if this controller has just been engaged
if (_flags.reset_rate_to_accel_z) {
// Reset Filter
_vel_error.z = 0;
desired_accel = 0;
_flags.reset_rate_to_accel_z = false;
} else {
_vel_error.z = (_vel_target.z - curr_vel.z);
}
// calculate p
p = _p_alt_rate.kP() * _vel_error.z;
// consolidate and constrain target acceleration
desired_accel = _accel_feedforward.z + p;
desired_accel = constrain_int32(desired_accel, -32000, 32000);
// set target for accel based throttle controller
accel_to_throttle(desired_accel);
}
在这里可能 reset_rate_to_accel_z会让我们觉得代码不太好理解，但其实我们如果回到 update_z_controller函数中我们就会发现 reset_rate_to_accel_z只有在解锁时刻才为真值，所以正常飞行时不需要考虑。然后我们再看代码的时候将看到整个处理过程也是先检查极限值，然后计算了一个值：feedforward。字面理解是前反馈，实际上就是 D项。这点从它的计算就可以看出。只不过这里的 D项是对 target的计算，这一点跟我们所熟悉的 PID是不同的。在算完了 D之后再是 P项，最后就是限幅输出并进入下一级控制。我觉得这里 P项没有什么好讨论的，主要是输出范围，这里我们看到是 32000，单位应该是 "cm/s/s"。但是总的来说，框图中说中间这一级控制器用的是 PID控制，也许早起的代码是这吗设计的，但至少我们现在看到的代码使用的是 PD控制。
最后就是我们相当重要的一个环节：PID控制器。
// calculates a desired throttle which is sent directly to the motors
void AC_PosControl::accel_to_throttle(float accel_target_z)
{
float z_accel_meas; // actual acceleration
int32_t p,i,d; // used to capture pid values for logging
// Calculate Earth Frame Z acceleration
z_accel_meas = -(_ahrs.get_accel_ef().z + GRAVITY_MSS) * 100.0f;
// reset target altitude if this controller has just been engaged
if (_flags.reset_accel_to_throttle) {
// Reset Filter
_accel_error.z = 0;
_flags.reset_accel_to_throttle = false;
} else {
// calculate accel error and Filter with fc = 2 Hz
// To-Do: replace constant below with one that is adjusted for update rate
_accel_error.z = _accel_error.z + 0.11164f * (constrain_float(accel_target_z - z_accel_meas, -32000, 32000) - _accel_error.z);
}
// separately calculate p, i, d values for logging
p = _pid_alt_accel.get_p(_accel_error.z);
// get i term
i = _pid_alt_accel.get_integrator();
// update i term as long as we haven't breached the limits or the I term will certainly reduce
// To-Do: should this be replaced with limits check from attitude_controller?
if ((!_motors.limit.throttle_lower && !_motors.limit.throttle_upper) || (i>0&&_accel_error.z<0) || (i<0&&_accel_error.z>0)) {
i = _pid_alt_accel.get_i(_accel_error.z, _dt);
}
// get d term
d = _pid_alt_accel.get_d(_accel_error.z, _dt);
// To-Do: pull min/max throttle from motors
// To-Do: we had a contraint here but it's now removed, is this ok? with the motors library handle it ok?
_attitude_control.set_throttle_out((int16_t)p+i+d+_throttle_hover, true);
// to-do add back in PID logging?
}
这里的 PID就是我们所熟悉的 PID，跟前面我们看到的姿态控制中的控制级 PID形式上是一致的。只不过这里对 "error"的计算可能我们一时半会儿看不明白，那么下面我们就稍稍分析一下。
其实按照我们通常使用 PID的习惯，误差应该是 "accel_target_z - z_accel_meas"，即便是考虑到对其进行限制，那也应当是 "constrain_float(accel_target_z - z_accel_meas, -32000, 32000)"，这是我们习惯的做法。按照注释的说法是这里使用了滤波，为 2Hz。我们可以用数学方法把代码稍微变个形式：
==>
_accel_error.z = (1-0.11164f)*_accel_error.z + 0.11164f * constrain_float(accel_target_z - z_accel_meas, -32000, 32000);
那么如果你熟悉线性插值你一眼就可以看出来这里用的就是线性插值。使用线性插值是为了得到一个尽可能真实的数据。因为这里每一个 控制环节都有可能引入误差，尤其是 P控制环节的误差会随着控制器的传递被逐级放大。
在 PID计算完了之后，就下来当然就要输出了。 set_throttle_out这个还是我们之前已经碰到过了，用来设置油门的。在i前一篇笔记中我以为定高模式应该传递 "false"，而这里实际传递的是 " true"，仔细一看才发现原来那时候看到的都是没有飞起来的时候，因为第一个参数全为 0。也就是说在定高模式虽然说油门已经通过气压计进行控制，但还是会对油门进行补偿。这时候的补偿有点让人无法理解，照理说补偿是为了应对打舵时候引起的高度降低而设计的，而这个时候已经有气压计定高了应该就没有必要在进行补偿了才对。但既然人家已经这样做了那肯定是有原因的，我觉得比较合理的解释是假如机体现在锁定在某一高度，这个时候如果我打舵，那么油门输出还是处于抵销机体重力的状态，但由于打舵的原因，升力要在水平方向分出一部分，因此也就不足以保持高度不变化虽然说高度降低之后很快将会被修正，但在用户看来应该就是飞行器在上下摆动。毕竟，从我们所看到的定高这一过程来看，并没有把在水平方向的分力考虑进来，那么打舵对于飞行效果多少还是有影响的，因此即便是定高模式，补偿仍然是有必要的。
最后还有一个问题就是 _throttle_hover是什么？可能我们会觉得 PID的结果应该直接输出才对，但是别忘了 PID的结果是带符号值，应该说正值表示上升，负值表示下降。但油门显然是不应该出现负值的，因为这个时候我们只好采用截断的方式进行处理，也就是说一旦下降飞行器就直接做自由落体运动了，这显然是不应该发生的！回想一下在姿态控制的时候，PID输出也是带符号值，但它是在油门的基础上进心修正，所以这里也应该在某一个值的基础上进行修正，所以 _throttle_hover是悬停油门量！为什么这么说呢？从变量名跟定义时的注释 " // estimated throttle required to maintain a level hover"可以大致这么推测，另外当锁定在某一高度时，由于高度差为 0，控制器的输出也应该是 0，即这个时候的油门输出就是 _throttle_hover，而此时飞行器是处于悬停状态的，所以 _throttle_hover就是悬停油门。
但是悬停油门显然不应该是一个常数，原因很简单，因为随着电池电量的消耗，电池电压会降低。所以除非这可电池有电没电都能够以同样的电压输出。当然我不确定是否真的有这样的电池，但问题是如果真的有这样的电池，那么电池没电了用户是没法知道的，知道啪嚓一声炸机了才发现电池没电了，这个肯定谁也不想。
那既然它是可变的问题又来了，那么怎么能够知道悬停油门呢？或者说如果我知道悬停油门那我还要用气压计干什么？这确实有点矛盾！
ardupilot/libraries/AC_AttitudeControl/AC_PosControl.cpp:14: AP_GROUPINFO("THR_HOVER", 0, AC_PosControl, _throttle_hover, POSCONTROL_THROTTLE_HOVER),
ardupilot/libraries/AC_AttitudeControl/AC_PosControl.cpp:389: _attitude_control.set_throttle_out((int16_t)p+i+d+_throttle_hover,true);
ardupilot/libraries/AC_AttitudeControl/AC_PosControl.h:93: /// set_throttle_hover - update estimated throttle required to maintain hover
ardupilot/libraries/AC_AttitudeControl/AC_PosControl.h:94: void set_throttle_hover(float throttle) { _throttle_hover = throttle; }
ardupilot/libraries/AC_AttitudeControl/AC_PosControl.h:333: AP_Float _throttle_hover; // estimated throttle required to maintain a level hover
bitcraze@bitcraze-vm:~/apm$
bitcraze@bitcraze-vm:~/apm$ grep -nr set_throttle_hover ardupilot/
ardupilot/Build.ArduCopter/ArduCopter.cpp:2159: pos_control.set_throttle_hover(throttle_avg);
ardupilot/Build.ArduCopter/ArduCopter.cpp:2175: pos_control.set_throttle_hover(throttle_avg);
ardupilot/ArduCopter/Attitude.pde:88: pos_control.set_throttle_hover(throttle_avg);
ardupilot/ArduCopter/Attitude.pde:104: pos_control.set_throttle_hover(throttle_avg);
ardupilot/libraries/AC_AttitudeControl/AC_PosControl.h:93: /// set_throttle_hover - update estimated throttle required to maintain hover
ardupilot/libraries/AC_AttitudeControl/AC_PosControl.h:94: void set_throttle_hover(float throttle) { _throttle_hover = throttle; }
bitcraze@bitcraze-vm:~/apm$
这段匹配结果其实也正好说明了 _throttle_hover是可变的。那我们就不妨去看看具体是怎么计算悬停油门的。
// should be called at 100hz
static void update_thr_cruise()
{
// ensure throttle_avg has been initialised
if( throttle_avg == 0 ) {
throttle_avg = g.throttle_cruise;
// update position controller
pos_control.set_throttle_hover(throttle_avg);
}
// if not armed or landed exit
if (!motors.armed() || ap.land_complete) {
return;
}
// get throttle output
int16_t throttle = g.rc_3.servo_out;
// calc average throttle if we are in a level hover
if (throttle > g.throttle_min && abs(climb_rate) < 60 && labs(ahrs.roll_sensor) < 500 && labs(ahrs.pitch_sensor) < 500) {
throttle_avg = throttle_avg * 0.99f + (float)throttle * 0.01f;
g.throttle_cruise = throttle_avg;
// update position controller
pos_control.set_throttle_hover(throttle_avg);
}
}
bitcraze@bitcraze-vm:~/apm$ grep -nr update_thr_cruise ardupilot/ArduCopter/
ardupilot/ArduCopter/ArduCopter.pde:780: { update_thr_cruise, 40, 10 },
ardupilot/ArduCopter/ArduCopter.pde:844: { update_thr_cruise, 1, 50 },
ardupilot/ArduCopter/Attitude.pde:80:// update_thr_cruise - update throttle cruise if necessary
ardupilot/ArduCopter/Attitude.pde:82:static void update_thr_cruise()
bitcraze@bitcraze-vm:~/apm$
对 throttle_avg的计算使用的还是线性插值，但最重要的还是它的判断条件。首先油门不能处于最低这是必须的，然后对速率进行检查，极限是 0.6 m/s，最后是姿态必须在 5°以内。那么前面我所提到的矛盾其实是不存在的，显然速率的计算用到了气压计。目前为止我还没有去证实 PX4对速率计算的精确度是多少，估计也不会太好算。其实当油门跟速率满足判断条件时飞行器至少是低速升降，因为速率的计算精确度可能并不是那么高，所以 PX4认为 0.6 m/s以内都是悬停状态。其实就算精确度很高也不能把 0当作是悬停状态，仍然是有一个范围的。虽然说这个时候高度可能没有太大的变化，但如果仅凭这两个条件就认为此时的油门为悬停油门那可就大错特错了！为什么？前面我也提到过打舵的时候高度会降低所以需要进行补偿。那么同样的道理，如果此时舵量很大则计算得到的悬停油门误差也很大，定高效果就不会好。我认为这就是之所以要对姿态进行判断。至于 throttle_avg为 0的时候自然需要设置一个初始的悬停油门，这个值是可以通过调参软件修改的，但是记住，它只是一个初值而已。而默认值是 450。从注释中我们也看到 update_thr_cruise函数调用的频率为 100Hz，但根据我们的匹配结果，在 400Hz的时候实际上是以 10Hz调用的，这个我暂时还没有办法解释。
关于定高我们就先看到这里。那么下面我想我们应该去看下我们在定高时用到的三个数据：_inav.get_altitude、_inav.get_velocity、_ahrs.get_accel_ef().z是怎么获得的。
PX4 FMU [19] 运动数据
1.inertial_nav
前面在分析定高的时候我们主要集中在控制算法上，但对于其中相当重要的三个数据：_inav.get_altitude、_inav.get_velocity、_ahrs.get_accel_ef().z我们并没有详细分析他们是如何得到的。那么现在我们就稍微来看下这些数据是怎么来的。
因为前面我们看到的定高是由类 AC_PosControl实现的，那么我们自然就有必要看下其构造函数。
// Note that the Vector/Matrix constructors already implicitly zero
// their values.
//
AC_PosControl::AC_PosControl(const AP_AHRS& ahrs, const AP_InertialNav& inav,
const AP_Motors& motors, AC_AttitudeControl& attitude_control,
AC_P& p_alt_pos, AC_P& p_alt_rate, AC_PID& pid_alt_accel,
AC_P& p_pos_xy, AC_PID& pid_rate_lat, AC_PID& pid_rate_lon) :
_ahrs(ahrs),
_inav(inav),
AC_PosControl pos_control(ahrs, inertial_nav, motors, attitude_control,
g.p_alt_hold, g.p_throttle_rate, g.pid_throttle_accel,
g.p_loiter_pos, g.pid_loiter_rate_lat, g.pid_loiter_rate_lon);
// Inertial Navigation
#if AP_AHRS_NAVEKF_AVAILABLE
static AP_InertialNav_NavEKF inertial_nav(ahrs, barometer, gps_glitch);
#else
static AP_InertialNav inertial_nav(ahrs, barometer, gps_glitch);
#endif
// 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是由头文件 " ./ardupilot/libraries/AP_AHRS/AP_AHRS_NavEKF.h"定义的，所以我们应该很清楚 _ahrs跟 _inav分别引用了哪个类实例。但我们同时也看到 inertial_nav是引用了 ahrs的。所以我们倒不妨先看看高度跟速率的获取：
* get_altitude - get latest altitude estimate in cm
* @return
*/
float AP_InertialNav_NavEKF::get_altitude() const
{
if (_ahrs.have_inertial_nav()) {
return _relpos_cm.z;
}
return AP_InertialNav::get_altitude();
}
/**
* get_velocity_z - returns the current climbrate.
*
* @see get_velocity().z
*
* @return climbrate in cm/s
*/
float AP_InertialNav_NavEKF::get_velocity_z() const
{
if (_ahrs.have_inertial_nav()) {
return _velocity_cm.z;
}
return AP_InertialNav::get_velocity_z();
}
从这里我们看到数据来源有两条途径，但我们使用的实际上应该是后一途径，原因在于 have_inertial_nav。
bool AP_AHRS_NavEKF::have_inertial_nav(void) const
{
return using_EKF();
}
bool AP_AHRS_NavEKF::using_EKF(void) const
{
return ekf_started && _ekf_use && EKF.healthy();
}
所以，如果想要使用第一条途径获取数据，那么就需要三个值均为真。但巧就巧在第二个数据： _ekf_use，我们还是来看代码吧。
ardupilot/libraries/AP_AHRS/AP_AHRS.cpp:120: AP_GROUPINFO("EKF_USE", 13, AP_AHRS, _ekf_use, 0),
ardupilot/libraries/AP_AHRS/AP_AHRS_NavEKF.h:93: void set_ekf_use(bool setting) { _ekf_use.set(setting); }
ardupilot/libraries/AP_AHRS/AP_AHRS_NavEKF.cpp:252: return ekf_started && _ekf_use && EKF.healthy();
ardupilot/libraries/AP_AHRS/AP_AHRS_NavEKF.cpp:260: if (_ekf_use) {
ardupilot/libraries/AP_AHRS/AP_AHRS.h:340: AP_Int8 _ekf_use;
bitcraze@bitcraze-vm:~/apm$
bitcraze@bitcraze-vm:~/apm$ grep -nr set_ekf_use ./ardupilot/
./ardupilot/Build.ArduCopter/ArduCopter.cpp:9824: ahrs.set_ekf_use(ch_flag==AUX_SWITCH_HIGH);
./ardupilot/ArduCopter/control_modes.pde:380: ahrs.set_ekf_use(ch_flag==AUX_SWITCH_HIGH);
./ardupilot/libraries/AP_AHRS/AP_AHRS_NavEKF.h:93: void set_ekf_use(bool setting) { _ekf_use.set(setting); }
./ardupilot/Tools/Replay/Replay.pde:234: ahrs.set_ekf_use(true);
bitcraze@bitcraze-vm:~/apm$
// do_aux_switch_function - implement the function invoked by the ch7 or ch8 switch
static void do_aux_switch_function(int8_t ch_function, uint8_t ch_flag)
{
int8_t tmp_function = ch_function;
// multi mode check
if(ch_function == AUX_SWITCH_MULTI_MODE) {
if (g.rc_6.radio_in < CH6_PWM_TRIGGER_LOW) {
tmp_function = AUX_SWITCH_FLIP;
}else if (g.rc_6.radio_in > CH6_PWM_TRIGGER_HIGH) {
tmp_function = AUX_SWITCH_SAVE_WP;
}else{
tmp_function = AUX_SWITCH_RTL;
}
}
switch(tmp_function) {
#if AP_AHRS_NAVEKF_AVAILABLE
case AUX_SWITCH_EKF:
ahrs.set_ekf_use(ch_flag==AUX_SWITCH_HIGH);
break;
#endif
这段代码实际上就告诉了我如果想使用第一条途径获取数据，那就必须设置一个辅助通道，并打开该辅助通道功能。但我们在飞的时候可能就只用了 5个通道，根本就没有开这个功能，所以我们就只考虑第二种途径。即：
* get_altitude - get latest altitude estimate in cm above the
* reference position
* @return
*/
virtual float get_altitude() const { return _position.z; }
/**
* get_velocity_z - returns the current climbrate.
*
* @see get_velocity().z
*
* @return climbrate in cm/s (positive up)
*/
virtual float get_velocity_z() const {return _velocity.z; }
也就是所最后我们考虑的是 _position跟 _velocity变量。由于存在继承关系，所以这两个变量的计算可以在父类中也可以在子类中，所以我们要找出具体是怎么计算的。
但其实啊，在 AP_InertialNav类中有一个很特别的方法：update。说它特别并不是因为其函数名，而是因为该函数的作用。
update internal state
*/
void AP_InertialNav_NavEKF::update(float dt)
{
AP_InertialNav::update(dt);
_ahrs.get_relative_position_NED(_relpos_cm);
_relpos_cm *= 100; // convert to cm
_haveabspos = _ahrs.get_position(_abspos);
_ahrs.get_velocity_NED(_velocity_cm);
_velocity_cm *= 100; // convert to cm/s
// InertialNav is NEU
_relpos_cm.z = - _relpos_cm.z;
_velocity_cm.z = -_velocity_cm.z;
}
从这段代码来看的话，如果是第一条途径，则数据是有 ahrs负责计算的，而第二条途径则由父类 AP_InertialNav进行计算。而 AP_InertialNav::update函数就有点啰嗦。
那现在我们不妨把视线稍稍一开，我们来看看 update函数是如何被调用的。
./ardupilot/ArduCopter/inertia.pde:7: inertial_nav.update(G_Dt);
bitcraze@bitcraze-vm:~/apm$
// read_inertia - read inertia in from accelerometers
static void read_inertia()
{
// inertial altitude estimates
inertial_nav.update(G_Dt);
}
bitcraze@bitcraze-vm:~/apm$ grep -nr read_inertia ./ardupilot/ArduCopter/
./ardupilot/ArduCopter/inertia.pde:3:// read_inertia - read inertia in from accelerometers
./ardupilot/ArduCopter/inertia.pde:4:static void read_inertia()
./ardupilot/ArduCopter/inertia.pde:10:// read_inertial_altitude - pull altitude and climb rate from inertial nav library
./ardupilot/ArduCopter/inertia.pde:11:static void read_inertial_altitude()
./ardupilot/ArduCopter/ArduCopter.pde:984: read_inertia();
./ardupilot/ArduCopter/ArduCopter.pde:1014: read_inertial_altitude();
bitcraze@bitcraze-vm:~/apm$
// 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();
#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();
在这里呢它是通过 fast_loop函数进行调用的，并不像我们前面看到的几个是通过任务来调度的。
2.AP_InertialNav::update
在 PX4源码中，我们会看到很多类都有 update方法，从其名字不难看出该方法是用来对其数据进行处理的，当然在该方法里边也会用到其它方法提供的数据。
void AP_InertialNav::update(float dt)
{
// discard samples where dt is too large
if( dt > 0.1f ) {
return;
}
// decrement ignore error count if required
if (_flags.ignore_error > 0) {
_flags.ignore_error--;
}
// check if new baro readings have arrived and use them to correct vertical accelerometer offsets.
check_baro();
// check if new gps readings have arrived and use them to correct position estimates
check_gps();
Vector3f accel_ef = _ahrs.get_accel_ef();
// remove influence of gravity
accel_ef.z += GRAVITY_MSS;
accel_ef *= 100;
// remove xy if not enabled
if( !_xy_enabled ) {
accel_ef.x = 0;
accel_ef.y = 0;
}
//Convert North-East-Down to North-East-Up
accel_ef.z = -accel_ef.z;
// convert ef position error to horizontal body frame
Vector2f position_error_hbf;
position_error_hbf.x = _position_error.x * _ahrs.cos_yaw() + _position_error.y * _ahrs.sin_yaw();
position_error_hbf.y = -_position_error.x * _ahrs.sin_yaw() + _position_error.y * _ahrs.cos_yaw();
float tmp = _k3_xy * dt;
accel_correction_hbf.x += position_error_hbf.x * tmp;
accel_correction_hbf.y += position_error_hbf.y * tmp;
accel_correction_hbf.z += _position_error.z * _k3_z * dt;
tmp = _k2_xy * dt;
_velocity.x += _position_error.x * tmp;
_velocity.y += _position_error.y * tmp;
_velocity.z += _position_error.z * _k2_z * dt;
tmp = _k1_xy * dt;
_position_correction.x += _position_error.x * tmp;
_position_correction.y += _position_error.y * tmp;
_position_correction.z += _position_error.z * _k1_z * dt;
// convert horizontal body frame accel correction to earth frame
Vector2f accel_correction_ef;
accel_correction_ef.x = accel_correction_hbf.x * _ahrs.cos_yaw() - accel_correction_hbf.y * _ahrs.sin_yaw();
accel_correction_ef.y = accel_correction_hbf.x * _ahrs.sin_yaw() + accel_correction_hbf.y * _ahrs.cos_yaw();
// calculate velocity increase adding new acceleration from accelerometers
Vector3f velocity_increase;
velocity_increase.x = (accel_ef.x + accel_correction_ef.x) * dt;
velocity_increase.y = (accel_ef.y + accel_correction_ef.y) * dt;
velocity_increase.z = (accel_ef.z + accel_correction_hbf.z) * dt;
// calculate new estimate of position
_position_base += (_velocity + velocity_increase*0.5) * dt;
// update the corrected position estimate
_position = _position_base + _position_correction;
// calculate new velocity
_velocity += velocity_increase;
// store 3rd order estimate (i.e. estimated vertical position) for future use
_hist_position_estimate_z.push_back(_position_base.z);
// store 3rd order estimate (i.e. horizontal position) for future use at 10hz
_historic_xy_counter++;
if( _historic_xy_counter >= AP_INTERTIALNAV_SAVE_POS_AFTER_ITERATIONS ) {
_historic_xy_counter = 0;
_hist_position_estimate_x.push_back(_position_base.x);
_hist_position_estimate_y.push_back(_position_base.y);
}
}
这段代码最开始我看了 N遍总觉得莫名其妙，比如说 "_velocity"，我们稍加整理就会得到其计算公式为："_velocity += (_position_error * temp + velocity_increase)"。根据物理定律，加速度是速度的微分 "velocity_increase"这一项是没有问题的，但问题就在于 "_position_error * temp"，前者是位置误差，其实就是距离啦，而后者包含了时间。所以如果按照物理定义，距离与时间的积是什么？但肯定不会是速度！因为速度应该是距离对时间的微分！但我觉得 APM团队显然不可能不知道这些物理学知识，那么我们该如何去理解这段代码？这不妨想想我们之前的定高，在定高中就有这样的情况，高度 -->速率 -->加速度，给人的感觉也是反的，虽然里边没有时间量。在姿态控制当中也是一样的道理。所以我觉得唯一的解释是 PX4使用控制原理用位置误差对速率进行修正。其实仔细想想这也并非完全没有道理，如果换成控制的话，位置误差经 PID控制器得到的就是我们让飞行器运动的速率，那么把 PID控制器换个方式去使用用来修正飞行器的速度也未尝不可。这样以分析之后我就可以理解 "_velocity"为什么这样计算了。而加速度那一项则是对物理公式的直接实现，这个我想只要知道加速度是什么就都能理解。
那么现在让人迷惑的另一个问题是加速度。我们看到加速度的修正使用了三角函数值，这个一开始仍然是让我百思不得其解！但是 z轴的修正量却又稍稍有点区别 "_position_error.z * _k3_z * dt"，而这正好是我们的突破口。仔细看是我们会发现，关于三角函数的计算其实都是在水平面内进行旋转，具体可以去查阅平面旋转相关计算。那么如果我们忽略旋转其实 xy跟 z是一样的，都是通过姿态误差来修正加计。但其实这样做是粗糙的做法，因为我们回过头去看定高就会发现严格来讲应该是速度的误差得到加计的修正。但不管怎么说，速度最终还是要通过加计进行修正，所以如果真的这样去实现就让人觉得有点奇怪，感觉就像在兜圈子。
而关于旋转，仔细对照公式我们将看到其实一个是逆时针旋转，一个是顺时针旋转，而且旋转的角度都是 yaw。那显然转来转去又转回原来的方向了，所以让人觉得很奇怪，那你转它干嘛？虽然说经过两次旋转还是在原来的方向，但是根据注释，没一次旋转有个东西变了：坐标系。第一次旋转之后坐标系是 "body frame"，而第二次旋转之后坐标系是 "earth frame"，那么一种合理的解释是 "accel_correction_hbf"需要在 "body frame"坐标系计算，也就是说 "_k3"在 "body frame"中更容易确定其大小。从代码中我们也可以看出，"_k3"在 "body frame"中是常量，但是到了 "earth frame"就会随 yaw的大小而变化。那么答案就显而易见了，我们只能在 "body frame"中对 "accel_correction_hbf"进行计算。
于是我们忽略旋转对代码稍微处理一下将得到：
accel_correction_hbf.x += _position_error.x * tmp;
accel_correction_hbf.y += _position_error.y * tmp;
accel_correction_hbf.z += _position_error.z * _k3_z * dt;
tmp = _k2_xy * dt;
_velocity.x += _position_error.x * tmp;
_velocity.y += _position_error.y * tmp;
_velocity.z += _position_error.z * _k2_z * dt;
tmp = _k1_xy * dt;
_position_correction.x += _position_error.x * tmp;
_position_correction.y += _position_error.y * tmp;
_position_correction.z += _position_error.z * _k1_z * dt;
// calculate velocity increase adding new acceleration from accelerometers
Vector3f velocity_increase;
velocity_increase.x = (accel_ef.x + accel_correction_hbf.x) * dt;
velocity_increase.y = (accel_ef.y + accel_correction_hbf.y) * dt;
velocity_increase.z = (accel_ef.z + accel_correction_hbf.z) * dt;
// calculate new estimate of position
_position_base += (_velocity + velocity_increase*0.5) * dt;
// update the corrected position estimate
_position = _position_base + _position_correction;
// calculate new velocity
_velocity += velocity_increase;
从这里我们就知道加计、速度跟位置都是通过位置误差使用 PID控制的方式进行修正。所以这里很关键的一点在于 _position_error的计算。前面我们说过它是位置误差，而实际上它是对应了经纬度跟高度。
./ardupilot/libraries/AP_GPS/AP_GPS_SIRF.h:61: uint32_t horizontal_position_error;
./ardupilot/libraries/AP_GPS/AP_GPS_SIRF.h:62: uint32_t vertical_position_error;
./ardupilot/libraries/AP_InertialNav/AP_InertialNav.h:302: Vector3f _position_error; // current position error in cm - is set by the check_* methods and used by update method to calculate the correction terms
./ardupilot/libraries/AP_InertialNav/AP_InertialNav.cpp:70: position_error_hbf.x = _position_error.x * _ahrs.cos_yaw() + _position_error.y * _ahrs.sin_yaw();
./ardupilot/libraries/AP_InertialNav/AP_InertialNav.cpp:71: position_error_hbf.y = -_position_error.x * _ahrs.sin_yaw() + _position_error.y * _ahrs.cos_yaw();
./ardupilot/libraries/AP_InertialNav/AP_InertialNav.cpp:76: accel_correction_hbf.z += _position_error.z * _k3_z * dt;
./ardupilot/libraries/AP_InertialNav/AP_InertialNav.cpp:79: _velocity.x += _position_error.x * tmp;
./ardupilot/libraries/AP_InertialNav/AP_InertialNav.cpp:80: _velocity.y += _position_error.y * tmp;
./ardupilot/libraries/AP_InertialNav/AP_InertialNav.cpp:81: _velocity.z += _position_error.z * _k2_z * dt;
./ardupilot/libraries/AP_InertialNav/AP_InertialNav.cpp:84: _position_correction.x += _position_error.x * tmp;
./ardupilot/libraries/AP_InertialNav/AP_InertialNav.cpp:85: _position_correction.y += _position_error.y * tmp;
./ardupilot/libraries/AP_InertialNav/AP_InertialNav.cpp:86: _position_correction.z += _position_error.z * _k1_z * dt;
./ardupilot/libraries/AP_InertialNav/AP_InertialNav.cpp:157: _position_error.x *= 0.9886;
./ardupilot/libraries/AP_InertialNav/AP_InertialNav.cpp:158: _position_error.y *= 0.9886;
./ardupilot/libraries/AP_InertialNav/AP_InertialNav.cpp:191: _position_error.x *= 0.7943;
./ardupilot/libraries/AP_InertialNav/AP_InertialNav.cpp:192: _position_error.y *= 0.7943;
./ardupilot/libraries/AP_InertialNav/AP_InertialNav.cpp:200: _position_error.x = 0;
./ardupilot/libraries/AP_InertialNav/AP_InertialNav.cpp:201: _position_error.y = 0;
./ardupilot/libraries/AP_InertialNav/AP_InertialNav.cpp:214: _position_error.x = x - (hist_position_base_x + _position_correction.x);
./ardupilot/libraries/AP_InertialNav/AP_InertialNav.cpp:215: _position_error.y = y - (hist_position_base_y + _position_correction.y);
./ardupilot/libraries/AP_InertialNav/AP_InertialNav.cpp:358: _position_error.z = baro_alt - (hist_position_base_z + _position_correction.z);
bitcraze@bitcraze-vm:~/apm$
我们看到在最后几行代码是用来计算 _position_error的，它们来自两个函数： AP_InertialNav::correct_with_gps跟 AP_InertialNav::correct_with_baro，也就是使用了 GPS跟气压计，分别由 check_gps跟 check_baro函数调用。我们不妨先来看看气压计：
void AP_InertialNav::check_baro()
{
uint32_t baro_update_time;
// calculate time since last baro reading (in ms)
baro_update_time = _baro.get_last_update();
if( baro_update_time != _baro_last_update ) {
const float dt = (float)(baro_update_time - _baro_last_update) * 0.001f; // in seconds
// call correction method
correct_with_baro(_baro.get_altitude()*100, dt);
_baro_last_update = baro_update_time;
}
}
// correct_with_baro - modifies accelerometer offsets using barometer. dt is time since last baro reading
void AP_InertialNav::correct_with_baro(float baro_alt,float dt)
{
static uint8_t first_reads = 0;
// discard samples where dt is too large
if( dt > 0.5f ) {
return;
}
// discard first 10 reads but perform some initialisation
if( first_reads <= 10 ) {
set_altitude(baro_alt);
first_reads++;
}
// 3rd order samples (i.e. position from baro) are delayed by 150ms (15 iterations at 100hz)
// so we should calculate error using historical estimates
float hist_position_base_z;
if( _hist_position_estimate_z.is_full() ) {
hist_position_base_z = _hist_position_estimate_z.front();
}else{
hist_position_base_z = _position_base.z;
}
// calculate error in position from baro with our estimate
_position_error.z = baro_alt - (hist_position_base_z + _position_correction.z);
}
我们很容易从这段代码中看出 _position_error.z的计算是气压计高度跟上一次高度相减。而其它的主要是判断操作，对时间进行判断当然主要是为了确定数据已经被更新，而 first_reads从其使用来看明显是用于初始化过程的，当它的值为 10的时候就不必管它了。而 _hist_position_estimate_z部分实际上是一个循环队列，一个在对尾插入数据，另一个则在队头去数据。不过对于 PX4的这个做法我倒是有些迷惑，如果是求均值那自然很好理解，但这里并没有，这里的做法只是使得数据被延迟 SIZE个周期，其实就是循环队列一圈。而该函数的调用频率是 400Hz，所以算下来的话就是 37.5 ms。不过，考虑到气压计是很慢的一个设备 baro_alt在 10个周期之内应该是同一个值，那么这种做法可能起到了缓冲作用，即把剧烈的变化给减缓了。当然，这种用法我可从来都没有使用过。
那么我们再来看看 GPS。
void AP_InertialNav::check_gps()
{
const uint32_t now = hal.scheduler->millis();
// compare gps time to previous reading
const AP_GPS &gps = _ahrs.get_gps();
if(gps.last_fix_time_ms() != _gps_last_time ) {
// call position correction method
correct_with_gps(now, gps.location().lng, gps.location().lat);
// record gps time and system time of this update
_gps_last_time = gps.last_fix_time_ms();
}else{
// if GPS updates stop arriving degrade position error to 10% over 2 seconds (assumes 100hz update rate)
if (now - _gps_last_update > AP_INTERTIALNAV_GPS_TIMEOUT_MS) {
_position_error.x *= 0.9886;
_position_error.y *= 0.9886;
// increment error count
if (_flags.ignore_error == 0 && _error_count < 255 && _xy_enabled) {
_error_count++;
}
}
}
}
// correct_with_gps - modifies accelerometer offsets using gps
void AP_InertialNav::correct_with_gps(uint32_t now, int32_t lon, int32_t lat)
{
float dt,x,y;
float hist_position_base_x, hist_position_base_y;
// calculate time since last gps reading
dt = (float)(now - _gps_last_update) * 0.001f;
// update last gps update time
_gps_last_update = now;
// discard samples where dt is too large
if( dt > 1.0f || dt == 0 || !_xy_enabled) {
return;
}
// calculate distance from base location
x = (float)(lat - _ahrs.get_home().lat) * LATLON_TO_CM;
y = (float)(lon - _ahrs.get_home().lng) * _lon_to_cm_scaling;
// sanity check the gps position. Relies on the main code calling GPS_Glitch::check_position() immediatley after a GPS update
if (_glitch_detector.glitching()) {
// failed sanity check so degrate position_error to 10% over 2 seconds (assumes 5hz update rate)
_position_error.x *= 0.7943;
_position_error.y *= 0.7943;
}else{
// if our internal glitching flag (from previous iteration) is true we have just recovered from a glitch
// reset the inertial nav position and velocity to gps values
if (_flags.gps_glitching) {
set_position_xy(x,y);
set_velocity_xy(_ahrs.get_gps().velocity().x * 100.0f,
_ahrs.get_gps().velocity().y * 100.0f);
_position_error.x = 0;
_position_error.y = 0;
}else{
// ublox gps positions are delayed by 400ms
// we store historical position at 10hz so 4 iterations ago
if( _hist_position_estimate_x.is_full()) {
hist_position_base_x = _hist_position_estimate_x.front();
hist_position_base_y = _hist_position_estimate_y.front();
}else{
hist_position_base_x = _position_base.x;
hist_position_base_y = _position_base.y;
}
// calculate error in position from gps with our historical estimate
_position_error.x = x - (hist_position_base_x + _position_correction.x);
_position_error.y = y - (hist_position_base_y + _position_correction.y);
}
}
// update our internal record of glitching flag so that we can notice a change
_flags.gps_glitching = _glitch_detector.glitching();
}
GPS的处理过程跟气压计是相似的，只是在数据处理上稍微多一些步骤。关于 xy的计算，我们看到是以起点作为坐标原点，那么我们就不难想象，后面我们所用到的坐标都是相对于起点的。但是我们看到，在上面的计算过程中对 GPS获取的数据做了转换，即 xy的单位是 "cm"，所以转换之后我们不必再考虑经纬度。
可能我们立马会想到，如果没有 GPS怎么办？答案是这个时候 error为 0，即由 _flags.gps_glitching控制。但其实我们也不难想象，在没有 GPS的时候我们只能够定高而不能够定点。
./ardupilot/libraries/AP_InertialNav/AP_InertialNav.cpp:196: if (_flags.gps_glitching) {
./ardupilot/libraries/AP_InertialNav/AP_InertialNav.cpp:220: _flags.gps_glitching = _glitch_detector.glitching();
./ardupilot/libraries/AP_Notify/Buzzer.cpp:133: if (_flags.gps_glitching != AP_Notify::flags.gps_glitching) {
./ardupilot/libraries/AP_Notify/Buzzer.cpp:134: _flags.gps_glitching = AP_Notify::flags.gps_glitching;
./ardupilot/libraries/AP_Notify/Buzzer.cpp:135: if (_flags.gps_glitching) {
bitcraze@bitcraze-vm:~/apm$
bitcraze@bitcraze-vm:~/apm$ grep -nr "_flags.gps_glitching" ./ardupilot/libraries/AP_InertialNav
./ardupilot/libraries/AP_InertialNav/AP_InertialNav.cpp:196: if (_flags.gps_glitching) {
./ardupilot/libraries/AP_InertialNav/AP_InertialNav.cpp:220: _flags.gps_glitching = _glitch_detector.glitching();
bitcraze@bitcraze-vm:~/apm$
bitcraze@bitcraze-vm:~/apm$ grep -nr "_glitch_detector" ./ardupilot/libraries/AP_InertialNav
./ardupilot/libraries/AP_InertialNav/AP_InertialNav.h:45: _glitch_detector(gps_glitch),
./ardupilot/libraries/AP_InertialNav/AP_InertialNav.h:306: GPS_Glitch& _glitch_detector; // GPS Glitch detector
./ardupilot/libraries/AP_InertialNav/AP_InertialNav.cpp:189: if (_glitch_detector.glitching()) {
./ardupilot/libraries/AP_InertialNav/AP_InertialNav.cpp:220: _flags.gps_glitching = _glitch_detector.glitching();
bitcraze@bitcraze-vm:~/apm$
bitcraze@bitcraze-vm:~/apm$ grep -nr glitching ./ardupilot/libraries/AP_GPS/
./ardupilot/libraries/AP_GPS/AP_GPS_Glitch.h:27: // check_position - checks latest gps position against last know position, velocity and maximum acceleration and updates glitching flag
./ardupilot/libraries/AP_GPS/AP_GPS_Glitch.h:36: // glitching - returns true if we are experiencing a glitch
./ardupilot/libraries/AP_GPS/AP_GPS_Glitch.h:37: bool glitching()const { return _flags.glitching; }
./ardupilot/libraries/AP_GPS/AP_GPS_Glitch.h:53: uint8_t glitching : 1; // 1 if we are experiencing a gps glitch
./ardupilot/libraries/AP_GPS/AP_GPS_Glitch.cpp:63: _flags.glitching = true;
./ardupilot/libraries/AP_GPS/AP_GPS_Glitch.cpp:77: _flags.glitching = false;
./ardupilot/libraries/AP_GPS/AP_GPS_Glitch.cpp:115: // update glitching flag
./ardupilot/libraries/AP_GPS/AP_GPS_Glitch.cpp:116: _flags.glitching = !all_ok;
bitcraze@bitcraze-vm:~/apm$
// check_position - returns true if gps position is acceptable, false if not
void GPS_Glitch::check_position()
{
uint32_t now = hal.scheduler->millis(); // current system time
float sane_dt; // time since last sane gps reading
float accel_based_distance; // movement based on max acceleration
Location curr_pos; // our current position estimate
Location gps_pos; // gps reported position
float distance_cm; // distance from gps to current position estimate in cm
bool all_ok; // true if the new gps position passes sanity checks
// exit immediately if we don't have gps lock
if (_gps.status() < AP_GPS::GPS_OK_FIX_3D) {
_flags.glitching = true;
return;
}
所以我们就应该清楚，在 GPS无效的情况下 _flags.gps_glitching值为 true，即 error为 0。其实我们仔细分析 correct_with_gps函数的时候，我们会发现，如果 GPS持续无效将执行 "// sanity"分支，即以 0.7943进行递减。但由于此时 error已经是 0，所以其值依然是 0。