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

上一篇:PX4 FMU [12] RC_Channel [转载]


下一篇:PX4 FMU [5] Loop