PX4 FMU [7] rgbled ［转载］
PX4 FMU [7] rgbled
PX4 FMU [7] rgbled
-------- 转载请注明出处
-------- 更多笔记请访问我的博客：merafour.blog.163.com
-------- 2014-12-15.冷月追风
-------- email：merafour@163.com
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, (unsignedlong)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, (unsignedlong)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 <red> <green> <blue>");
}
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 <ToshibaLED_PX4.h>
./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:boolToshibaLED_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:boolToshibaLED_PX4::hw_set_rgb(uint8_t red, uint8_t green, uint8_t blue)
./ardupilot/libraries/AP_Notify/ToshibaLED_PX4.cpp:61:voidToshibaLED_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 <red> <green> <blue>");
}
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<RGBLED *>(arg);
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:#defineRGBLED_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" __EXPORT int commander_main(int argc, char *argv[]);
./PX4Firmware/src/modules/commander/commander.cpp:246:intcommander_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 <ToshibaLED_PX4.h>
./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:boolToshibaLED_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:boolToshibaLED_PX4::hw_set_rgb(uint8_t red, uint8_t green, uint8_t blue)
./ardupilot/libraries/AP_Notify/ToshibaLED_PX4.cpp:61:voidToshibaLED_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 <AP_Notify.h> // Notify library
./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<uint8_t,3> last, next;
};
从定义中我们看到， 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, (unsignedlong)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
