Ardupilot字符型设备rgbled学习

目录

摘要

**本文主要学习pixhawk的原理图，对整个原理图进行分类学习。

1.主控芯片的学习

pixhawk的主控芯片分为主处理器和协处理器两个，分别是STM32F427VIT6、STM32F103C8T6,分别是100引脚和48引脚，主处理器：32位CPU，主频168 MHz ，256 KB RAM,2 MB Flash;独立供电32位STM32F103故障保护协处理器,主要处理安全相关，遥控器信号等。

2.LED端口

pixhawkv1上面总共有六个LED,其中有两个电源LED指示灯，三个状态指示灯，还有一个RGB灯。
1. 实物对应图

其中：左边的两个LED灯，是FMU的led灯，
其中PWR表示FMU上的5V电源指示灯，是绿色的，
B/E表示FMU的BOOTLOADER运行指示灯，是红色的。
右边的三个led灯，是IO的LED,PWR 表示IO芯片电源5V指示灯,绿色LED
B/E表示BOOTLOADER运行指示灯，红色LED,主要用来作为初始化指示灯。
ACT表示一切正常，蓝色LED灯。具体可以看下图的英文注释；


最后一个灯是RGB灯，主要运来指示飞控的运行模式，状态。
2. 原理图连接

3字符型设备基础知识（以rgbled为例）

1.概述
Linux的外设可以分为3类：字符设备（character device）、块设备（block device）字符、网络接口（network interface）,Nuttx和linux的设计思路基本一样，因此我们可以通过对比来学习，本节重点以RGBLED的实现流程来分析字符设备的驱动架构。
字符设备是能够像字节流（比如文件）一样被访问的设备，就是说对他的读写是以字节为单位的，如键盘、鼠标、以及一些传感器设备都是字符设备。
Nuttx采用VFS，和linux一样的设计思路，即“一切设备皆文件”，对设备的操作就如同对文件的操作，Nuttx下的设备驱动就是实现这种对文件操作的接口，设备驱动屏蔽了对设备本身的访问的复杂性。通过VFS对设备的抽象，呈现给用户简单的标准接口，如open(), read(), write()等。
2. 应用程序与驱动程序的关系

(1)应用程序使用库提供的open函数打开代表LED设备文件；
(2)库根据open函数传入的参数执行”swi”指令，这条指令会引起CPU异常，进入内核；
(3)内核的异常处理函数根据这些参数找到相应的驱动程序，返回一个文件句柄给库，进而返回给应用程序，这个就是字符设备的主设备号，或者称为fd；
(4)应用程序得到文件句柄后，使用库提供的write或ioctl函数发出控制命令。
(5)库根据write或ioctl函数传入的参数执行“swi”指令，这条指令会引起CPU异常，进入内核
(6)内核的异常处理函数根据这些参数调用驱动程序的相关函数实现点亮LED

3. 驱动程序开发步骤
(1)查看原理图、数据手册，了解设备的操作方法
(2)在内核中找到相近的驱动程序，以它为模版进行开发，有时候需要自己全部写
(3)实现驱动程序的初始化：比如向内核注册这个驱动程序，这样应用程序传入文件名字时，内核才能找到相应的驱动程序。
(4)设计所要实现的操作，比如open、close、read、ioctl
(5)实现中断服务函数(中断并不是每个设备驱动所必须的)
(6)编译该驱动程序到内核中，或者用insmod命令加载
(7)测试驱动程序

4.字符设备中重要数据结构和函数

设备驱动中的重要数据结构file_operations是联系VFS标准文件操作接口和设备驱动对设备具体操作的重要一环，这个数据结构中是一些函数指针，这些函数与VFS标准文件操作的接口一一对应，用户对文件的操作，最终通过file_operation结构体中对应功能的函数实现。
(1)最重要的结构体

struct file_operations
{
  /* The device driver open method differs from the mountpoint open method */

  int     (*open)(FAR struct file *filp);

  /* The following methods must be identical in signature and position because
   * the struct file_operations and struct mountp_operations are treated like
   * unions.
   */

  int     (*close)(FAR struct file *filp);
  ssize_t (*read)(FAR struct file *filp, FAR char *buffer, size_t buflen);
  ssize_t (*write)(FAR struct file *filp, FAR const char *buffer, size_t buflen);
  off_t   (*seek)(FAR struct file *filp, off_t offset, int whence);
  int     (*ioctl)(FAR struct file *filp, int cmd, unsigned long arg);
#ifndef CONFIG_DISABLE_POLL
  int     (*poll)(FAR struct file *filp, struct pollfd *fds, bool setup);
#endif

  /* The two structures need not be common after this point */
};

(2)采用重要的结构体定义了一个这种结构体类型的CDev::fops对象

/**
 * Character device indirection table.
 *
 * Every cdev we register gets the same function table; we use the private data
 * field in the inode to store the instance pointer.
 *
 * Note that we use the GNU extension syntax here because we don't get designated
 * initialisers in gcc 4.6.
 */
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,
};

(3)实现了file_operation中open=cdev_open,close=cdev_close等等函数的对应
那么file_operations实现的open,close,write,ioctl函数在哪里？

1.open()函数
open打开设备是操作设备必须的第一步操作，在应用中通过调用open()，系统返回一个整形的非零整数，称这个非零整数位文件描述符fd。打开设备之后对文件的一切操作就可以通过fd来完成。设备驱动中对应的open()函数的实现是非必须的，如果要实现一些对设备的初始化等工作，可以在设备驱动中的open()函数中实现。

应用中open()以设备的节点路径和操作权限为参数，操作进入VFS，调用fs_open.c中的open()函数，通过设备路径找到对应的inode节点，在进程的文件描述符链表中寻找并分配空闲可用的描述符fd和文件file，最后调用设备节点inode中的文件操作file_operation中的函数open()。应用程序调用成功时，返回本次分配的文件描述符fd，发生错误时，返回-1，错误码记录在errno中。

int open(const char *path, int oflags, ...)
{
  FAR struct filelist *list;
  FAR struct inode    *inode;
  FAR const char      *relpath = NULL;
#if defined(CONFIG_FILE_MODE) || !defined(CONFIG_DISABLE_MOUNTPOINT)
  mode_t               mode = 0666;
#endif
  int                  ret;
  int                  fd;

  /* Get the thread-specific file list */

  list = sched_getfiles();
  if (!list)
    {
      ret = EMFILE;
      goto errout;
    }

#ifdef CONFIG_FILE_MODE
#  ifdef CONFIG_CPP_HAVE_WARNING
#    warning "File creation not implemented"
#  endif

  /* If the file is opened for creation, then get the mode bits */

  if (oflags & (O_WRONLY|O_CREAT) != 0)
    {
      va_list ap;
      va_start(ap, oflags);
      mode = va_arg(ap, mode_t);
      va_end(ap);
    }
#endif

  /* Get an inode for this file */

  inode = inode_find(path, &relpath);
  if (!inode)
    {
      /* "O_CREAT is not set and the named file does not exist.  Or, a
       * directory component in pathname does not exist or is a dangling
       * symbolic link."
       */

      ret = ENOENT;
      goto errout;
    }

  /* Verify that the inode is valid and either a "normal" or a mountpoint.  We
   * specifically exclude block drivers.
   */

#ifndef CONFIG_DISABLE_MOUNTPOINT
  if ((!INODE_IS_DRIVER(inode) && !INODE_IS_MOUNTPT(inode)) || !inode->u.i_ops)
#else
  if (!INODE_IS_DRIVER(inode) || !inode->u.i_ops)
#endif
    {
      ret = ENXIO;
      goto errout_with_inode;
    }

  /* Make sure that the inode supports the requested access */

  ret = inode_checkflags(inode, oflags);
  if (ret < 0)
    {
      ret = -ret;
      goto errout_with_inode;
    }

  /* Associate the inode with a file structure */

  fd = files_allocate(inode, oflags, 0, 0);
  if (fd < 0)
    {
      ret = EMFILE;
      goto errout_with_inode;
    }

  /* Perform the driver open operation.  NOTE that the open method may be
   * called many times.  The driver/mountpoint logic should handled this
   * because it may also be closed that many times.
   */

  ret = OK;
  if (inode->u.i_ops->open)
    {
#ifndef CONFIG_DISABLE_MOUNTPOINT
      if (INODE_IS_MOUNTPT(inode))
        {
          ret = inode->u.i_mops->open((FAR struct file*)&list->fl_files[fd],
                                      relpath, oflags, mode);
        }
      else
#endif
        {
          ret = inode->u.i_ops->open((FAR struct file*)&list->fl_files[fd]);
        }
    }

  if (ret < 0)
    {
      ret = -ret;
      goto errout_with_fd;
    }

  return fd;

 errout_with_fd:
  files_release(fd);
 errout_with_inode:
  inode_release(inode);
 errout:
  set_errno(ret);
  return ERROR;
}

(2)close()函数
与打开设备文件对应的是关闭设备文件。应用程序中调用close()，执行fs_close.c中的close()函数，调用文件操作file_operation中的close()函数，最后释放文件描述符fd和文件。应用程序调用close()调用成功时，返回0，发生错误时，返回-1， 错误码记录在errno中。

int close(int fd)
{
  int err;
#if CONFIG_NFILE_DESCRIPTORS > 0
  int ret;

  /* Did we get a valid file descriptor? */

  if ((unsigned int)fd >= CONFIG_NFILE_DESCRIPTORS)
#endif
    {
      /* Close a socket descriptor */

#if defined(CONFIG_NET) && CONFIG_NSOCKET_DESCRIPTORS > 0
      if ((unsigned int)fd < (CONFIG_NFILE_DESCRIPTORS+CONFIG_NSOCKET_DESCRIPTORS))
        {
          return net_close(fd);
        }
      else
#endif
        {
          err = EBADF;
          goto errout;
        }
    }

#if CONFIG_NFILE_DESCRIPTORS > 0
  /* Close the driver or mountpoint.  NOTES: (1) there is no
   * exclusion mechanism here , the driver or mountpoint must be
   * able to handle concurrent operations internally, (2) The driver
   * may have been opened numerous times (for different file
   * descriptors) and must also handle being closed numerous times.
   * (3) for the case of the mountpoint, we depend on the close
   * methods bing identical in signature and position in the operations
   * vtable.
   */

  ret = files_close(fd);
  if (ret < 0)
    {
      /* An error occurred while closing the driver */

      err = -ret;
      goto errout;
    }
  return OK;

#endif

errout:
  set_errno(err);
  return ERROR;
}

(3)read()函数
应用程序调用read()函数，执行fs_read.c中read()函数，调用文件操作file_operation中的read()函数，从设备中读取数据。file_operation中的read函数含有三个参数，第一个是文件file指针，第二个是传输数据buffer，第三个是期望读取到的字节数。应用程序读取成功时，该函数返回真实读取到的字节数，如果发生错误，返回错误，错误码记录在errno中。

ssize_t read(int fd, FAR void *buf, size_t nbytes)
{
  /* Did we get a valid file descriptor? */

#if CONFIG_NFILE_DESCRIPTORS > 0
  if ((unsigned int)fd >= CONFIG_NFILE_DESCRIPTORS)
#endif
    {
      /* No.. If networking is enabled, read() is the same as recv() with
       * the flags parameter set to zero.
       */

#if def

(4)write()函数
应用程序中调用write()函数，执行fs_write.c中write()函数，调用文件操作 file_operation中的write()函数，往设备中写入数据。file_operation中的write含有三个参数，第一个是文件file指针，第二是传输数据buffer，第三个是期望写入的字节数。应用程序写入成功时，该函数返回真实写入的字节数，如果发生错误，返回-1， 错误码记录在errno中。

ssize_t write(int fd, FAR const void *buf, size_t nbytes)
{
  /* Did we get a valid file descriptor? */

#if CONFIG_NFILE_DESCRIPTORS > 0
  if ((unsigned int)fd >= CONFIG_NFILE_DESCRIPTORS)
#endif
    {
      /* Write to a socket descriptor is equivalent to send with flags == 0 */

#if defined(CONFIG_NET_TCP) && CONFIG_NSOCKET_DESCRIPTORS > 0
      return send(fd, buf, nbytes, 0);
#else
      set_errno(EBADF);
      return ERROR;
#endif
    }

  /* The descriptor is in the right range to be a file descriptor... write to the file */

#if CONFIG_NFILE_DESCRIPTORS > 0
  return file_write(fd, buf, nbytes);
#endif
}

(5) seek()函数

应用程序中调用lseek()函数，对应的VFS中的函数为fs_lseek.c中的lseek()函数，lseek()调用文件操作file_operation中的seek()，调整对文件的读写位置。它带有三个参数，第一个参数是文件file指针，第二个参数是设置的文件位置相对偏移，可正可负。第三个参数是设置位置的起始点，可选择文件头、文件当前位置或者文件尾。应用程序调用lseek()成功时，返回设置后的读写位置点，发生错误时，返回-1， 错误码记录在errno中。

off_t lseek(int fd, off_t offset, int whence)
{
  FAR struct filelist *list;
  FAR struct file     *filep;
  FAR struct inode    *inode;
  int                  err;

  /* Did we get a valid file descriptor? */

  if ((unsigned int)fd >= CONFIG_NFILE_DESCRIPTORS)
    {
      err = EBADF;
      goto errout;
    }

  /* Get the thread-specific file list */

  list = sched_getfiles();
  if (!list)
    {
      err = EMFILE;
      goto errout;
    }

  /* Is a driver registered? */

  filep = &list->fl_files[fd];
  inode =  filep->f_inode;

  if (inode && inode->u.i_ops)
    {
      /* Does it support the seek method */

      if (inode->u.i_ops->seek)
        {
          /* Yes, then let it perform the seek */

          err = (int)inode->u.i_ops->seek(filep, offset, whence);
          if (err < 0)
            {
              err = -err;
              goto errout;
            }
         }
      else
        {
          /* No... there are a couple of default actions we can take */

          switch (whence)
            {
              case SEEK_CUR:
                offset += filep->f_pos;

              case SEEK_SET:
                if (offset >= 0)
                  {
                    filep->f_pos = offset; /* Might be beyond the end-of-file */
                    break;
                  }
                else
                  {
                    err = EINVAL;
                    goto errout;
                  }
                break;

              case SEEK_END:
                err = ENOSYS;
                goto errout;

              default:
                err = EINVAL;
                goto errout;
            }
        }
    }

  return filep->f_pos;

errout:
  set_errno(err);
  return (off_t)ERROR;
}

#endif

(6) ioctl()函数

应用程序中调用ioctl()函数，执行fs_ioctl.c中的ioctl()函数，调用文件操作file_operation中的ioctl()函数。ioctl()用于执行设备特定的命令，如设置设备的属性，配置设备的寄存器等。file_operation()中的ioctl()带有三个参数，第一个是文件file指针，第二个参数是控制命令，第三个是命令参数，如果参数为指针，可以作为输入输出参数使用。ioctl()调用成功时，返回非负数，发生错误是，返回-1，错误码记录在errno中。

int ioctl(int fd, int req, unsigned long arg)
{
  int err;
#if CONFIG_NFILE_DESCRIPTORS > 0
  FAR struct filelist *list;
  FAR struct file     *this_file;
  FAR struct inode    *inode;
  int                  ret = OK;

  /* Did we get a valid file descriptor? */

  if ((unsigned int)fd >= CONFIG_NFILE_DESCRIPTORS)
#endif
    {
      /* Perform the socket ioctl */

#if defined(CONFIG_NET) && CONFIG_NSOCKET_DESCRIPTORS > 0
      if ((unsigned int)fd < (CONFIG_NFILE_DESCRIPTORS+CONFIG_NSOCKET_DESCRIPTORS))
        {
          return netdev_ioctl(fd, req, arg);
        }
      else
#endif
        {
          err = EBADF;
          goto errout;
        }
    }

#if CONFIG_NFILE_DESCRIPTORS > 0
  /* Get the thread-specific file list */

  list = sched_getfiles();
  if (!list)
    {
      err = EMFILE;
      goto errout;
    }

  /* Is a driver registered? Does it support the ioctl method? */

  this_file = &list->fl_files[fd];
  inode     = this_file->f_inode;

  if (inode && inode->u.i_ops && inode->u.i_ops->ioctl)
    {
      /* Yes, then let it perform the ioctl */

      ret = (int)inode->u.i_ops->ioctl(this_file, req, arg);
      if (ret < 0)
        {
          err = -ret;
          goto errout;
        }
    }

  return ret;
#endif

errout:
  set_errno(err);
  return ERROR;
}

(7) poll()函数

应用程序中使用poll()查询指定一组文件是否可读或者可写。VFS中执行fs_poll.c中的poll()函数。首先初始化信号量，用于实现定时。其次调用file_operation中的poll()函数查询文件是否可以读写，如果有文件可以读写，返回应用程序。否则，睡眠等待。如果睡眠时间到，再次查询文件是否可以读写，最后返回。
如果睡眠时间设定为0，那么第一次调用file_operation中的poll()后线程直接返回，无需等待。如果睡眠时间为有限值，那么线程等到睡眠睡眠时间到或者文件可读、写，或者信号量被信号中断，则再一次调用file_operation中的poll()查询一次，然后返回结果。如果睡眠时间设定为负数，那么线程将会永久睡眠，直到文件可读、写或者线程被信号中断。
file_operation的poll()函数设计中，如果文件可读、写，1）修改对应的pollfd中的返回事件标志为对应的事件，2）释放信号量。
应用程序调用poll()，第一个参数使pollfd数组指针，第二个为查询的文件数目，第三个为等待时间。如果poll()成功，返回正数，表明可读写的文件个数。返回0表明超时，返回-1表明发生错误，错误码记录在errno中。

int poll(FAR struct pollfd *fds, nfds_t nfds, int timeout)
{
  WDOG_ID wdog;
  sem_t sem;
  int count = 0;
  int ret;

  sem_init(&sem, 0, 0);
  ret = poll_setup(fds, nfds, &sem);
  if (ret >= 0)
    {
      if (timeout >= 0)
        {
          /* Wait for the poll event with a timeout.  Note that the
           * millisecond timeout has to be converted to system clock
           * ticks for wd_start
           */

          wdog = wd_create();
          wd_start(wdog,  MSEC2TICK(timeout), poll_timeout, 1, (uint32_t)&sem);
          poll_semtake(&sem);
          wd_delete(wdog);
        }
      else
        {
          /* Wait for the poll event with no timeout */

          poll_semtake(&sem);
        }

      /* Teardown the poll operation and get the count of events */

      ret = poll_teardown(fds, nfds, &count);
    }

  sem_destroy(&sem);

  /* Check for errors */

  if (ret < 0)
    {
      set_errno(-ret);
      return ERROR;
    }

  return count;
}

#endif /* CONFIG_DISABLE_POLL */

(4)inode_ops_u联合体

union inode_ops_u
{
  FAR const struct file_operations    *i_ops;  /* Driver operations for inode */
#ifndef CONFIG_DISABLE_MOUNTPOUNT
  FAR const struct block_operations   *i_bops; /* Block driver operations */
  FAR const struct mountpt_operations *i_mops; /* Operations on a mountpoint */
#endif
};

这里标注下后面会用到
(5)inode结构体

struct inode
{
  FAR struct inode *i_peer;       /* Pointer to same level inode */
  FAR struct inode *i_child;      /* Pointer to lower level inode */
  int16_t           i_crefs;      /* References to inode */
  uint16_t          i_flags;      /* Flags for inode */
  union inode_ops_u u;            /* Inode operations */
#ifdef CONFIG_FILE_MODE
  mode_t            i_mode;       /* Access mode flags */
#endif
  FAR void         *i_private;    /* Per inode driver private data */
  char              i_name[1];    /* Name of inode (variable) */
};

这里要注意的是 *i_private后面会用到
(6)File结构体
file结构体定义

struct file
{
  int               f_oflags; /* Open mode flags */
  off_t             f_pos;    /* File position */
  FAR struct inode *f_inode;  /* Driver interface */
  void             *f_priv;   /* Per file driver private data */
};

这里注意 FAR struct inode *f_inode; / Driver interface /
同时使用file结构体定义typedef FAR struct file file_t;
file_struct结构体

struct file_struct
{
  int                fs_filedes;   /* File descriptor associated with stream */
#if CONFIG_STDIO_BUFFER_SIZE > 0
  sem_t              fs_sem;       /* For thread safety */
  pid_t              fs_holder;    /* Holder of sem */
  int                fs_counts;    /* Number of times sem is held */
  FAR unsigned char *fs_bufstart;  /* Pointer to start of buffer */
  FAR unsigned char *fs_bufend;    /* Pointer to 1 past end of buffer */
  FAR unsigned char *fs_bufpos;    /* Current position in buffer */
  FAR unsigned char *fs_bufread;   /* Pointer to 1 past last buffered read char. */
#endif
  uint16_t           fs_oflags;    /* Open mode flags */
  uint8_t            fs_flags;     /* Stream flags */
#if CONFIG_NUNGET_CHARS > 0
  uint8_t            fs_nungotten; /* The number of characters buffered for ungetc */
  unsigned char      fs_ungotten[CONFIG_NUNGET_CHARS];
#endif
};

5.代码实现流程

类之间的关系：

1.

2.

3.

6.代码分析

（1）rgbled初始化
notify.init(true);

void AP_Notify::init(bool enable_external_leds)
{

// Notify devices for PX4 boards
#if CONFIG_HAL_BOARD == HAL_BOARD_PX4
    #if CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_PX4_V3 // Has enough memory for Oreo LEDs
        _devices[0] = new AP_BoardLED();
        _devices[1] = new ToshibaLED_I2C();
        _devices[2] = new ToneAlarm_PX4();
        _devices[3] = new Display();

        // Oreo LED enable/disable by NTF_OREO_THEME parameter
        if (_oreo_theme) {
            _devices[4] = new OreoLED_PX4(_oreo_theme);
        }

    #elif CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_PX4_V4 // Has its own LED board
        _devices[0] = new PixRacerLED();
        _devices[1] = new ToshibaLED_I2C();
        _devices[2] = new ToneAlarm_PX4();
        _devices[3] = new Display();

    #else   // All other px4 boards use standard devices.
        _devices[0] = new AP_BoardLED();
        _devices[1] = new ToshibaLED_I2C();
        _devices[2] = new ToneAlarm_PX4();
        _devices[3] = new Display();
    #endif

// Notify devices for VRBRAIN boards
#elif CONFIG_HAL_BOARD == HAL_BOARD_VRBRAIN  
    #if CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_VRBRAIN_V45 // Uses px4 LED board
        _devices[0] = new AP_BoardLED();
        _devices[1] = new ToshibaLED_I2C();
        _devices[2] = new ToneAlarm_PX4();
        _devices[3] = new ExternalLED();
    #else
        _devices[0] = new VRBoard_LED();
        _devices[1] = new ToshibaLED_I2C();
        _devices[2] = new ToneAlarm_PX4();
        _devices[3] = new ExternalLED();
    #endif

// Notify devices for linux boards    
#elif CONFIG_HAL_BOARD == HAL_BOARD_LINUX
    #if CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_NAVIO
        _devices[0] = new NavioLED_I2C();
        _devices[1] = new ToshibaLED_I2C();

    #elif CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_NAVIO2
        _devices[0] = new DiscreteRGBLed(4, 27, 6, false);
        _devices[1] = new ToshibaLED_I2C();

    #elif CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_BBBMINI
        _devices[0] = new AP_BoardLED();
        _devices[1] = new Buzzer();
        _devices[2] = new Display();

    #elif CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_BLUE
        _devices[0] = new AP_BoardLED();
        _devices[1] = new Display();

    #elif CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_RASPILOT
        _devices[0] = new ToshibaLED_I2C();
        _devices[1] = new ToneAlarm_Linux();

    #elif CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_MINLURE
        _devices[0] = new RCOutputRGBLedOff(15, 13, 14, 255);

    #elif CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_ERLEBRAIN2 || \
      CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_PXFMINI
        _devices[0] = new AP_BoardLED();

    #elif CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_BH
        _devices[0] = new AP_BoardLED();
        _devices[1] = new RCOutputRGBLed(HAL_RCOUT_RGBLED_RED, HAL_RCOUT_RGBLED_GREEN, HAL_RCOUT_RGBLED_BLUE);

    #elif CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_DISCO
        _devices[0] = new DiscoLED();
        _devices[1] = new ToneAlarm_Linux();

    #else
        _devices[0] = new AP_BoardLED();
        _devices[1] = new ToshibaLED_I2C();
        _devices[2] = new ToneAlarm_Linux();
    #endif

#else
    _devices[0] = new AP_BoardLED();
    _devices[1] = new ToshibaLED_I2C();
#endif

    // clear all flags and events
    memset(&AP_Notify::flags, 0, sizeof(AP_Notify::flags));
    memset(&AP_Notify::events, 0, sizeof(AP_Notify::events));

    // clear flight mode string and text buffer
    memset(_flight_mode_str, 0, sizeof(_flight_mode_str));
    memset(_send_text, 0, sizeof(_send_text));
    _send_text_updated_millis = 0;

    AP_Notify::flags.external_leds = enable_external_leds;

    for (uint8_t i = 0; i < CONFIG_NOTIFY_DEVICES_COUNT; i++)
    {
        if (_devices[i] != nullptr)
        {
            _devices[i]->pNotify = this;
            _devices[i]->init(); //调用初始化函数
        }
    }
}

我们使用的是_devices[1] = new ToshibaLED_I2C();

/************************************************************************************************************************************
*函数原型：bool RGBLed::init()
*函数功能：rgbled初始化
*修改日期：2018-7-10
*备   注：
*************************************************************************************************************************************/
bool RGBLed::init()
{
    _healthy = hw_init();
    return _healthy;
}

/************************************************************************************************************************************
*函数原型：bool ToshibaLED_I2C::hw_init()
*函数功能：rgbled初始化
*修改日期：2018-7-10
*备   注：
*************************************************************************************************************************************/
bool ToshibaLED_I2C::hw_init()
{
    // first look for led on external bus
    _dev = std::move(hal.i2c_mgr->get_device(TOSHIBA_LED_I2C_BUS_INTERNAL/*TOSHIBA_LED_I2C_BUS_EXTERNAL*/, TOSHIBA_LED_I2C_ADDR));
    if (!_dev || !_dev->get_semaphore()->take(HAL_SEMAPHORE_BLOCK_FOREVER)) 
    {
        return false;
    }

    // enable the led
    bool ret = _dev->write_register(TOSHIBA_LED_ENABLE, 0x03);

    // on failure try the internal bus
    if (!ret) {
        // give back external bus semaphore
        _dev->get_semaphore()->give();
        // get internal I2C bus driver
        _dev = std::move(hal.i2c_mgr->get_device(TOSHIBA_LED_I2C_BUS_EXTERNAL/*TOSHIBA_LED_I2C_BUS_INTERNAL*/, TOSHIBA_LED_I2C_ADDR));
        if (!_dev || !_dev->get_semaphore()->take(HAL_SEMAPHORE_BLOCK_FOREVER)) {
            return false;
        }
        ret = _dev->write_register(TOSHIBA_LED_ENABLE, 0x03);
    }

    // update the red, green and blue values to zero
    uint8_t val[4] = { TOSHIBA_LED_PWM0, _led_off, _led_off, _led_off };
    ret &= _dev->transfer(val, sizeof(val), nullptr, 0);

    // give back i2c semaphore
    _dev->get_semaphore()->give();

    _dev->register_periodic_callback(20000, FUNCTOR_BIND_MEMBER(&ToshibaLED_I2C::_timer, void));

    return ret;
}

void ToshibaLED_I2C::_timer(void)
{
    if (!_need_update) {
        return;
    }
    _need_update = false;

    /* 4-bit for each color */
    uint8_t val[4] = { TOSHIBA_LED_PWM0, (uint8_t)(rgb.b >> 4),
                       (uint8_t)(rgb.g / 16), (uint8_t)(rgb.r / 16) };

    _dev->transfer(val, sizeof(val), nullptr, 0);
}

---

(2)rgbled更新代码
1》》SCHED_TASK(update_notify, 50, 90), //LED指示灯通知函数
2》》更新

void Copter::update_notify()
{
    notify.update();
}

3》》代码分析

void AP_Notify::update(void)
{
    for (uint8_t i = 0; i < CONFIG_NOTIFY_DEVICES_COUNT; i++)
    {
        if (_devices[i] != nullptr)
        {
            _devices[i]->update();
        }
    }

    //reset the events
    memset(&AP_Notify::events, 0, sizeof(AP_Notify::events));
}

4》》我们使用的是iic_rgb

void RGBLed::update()
{
    //如果没有使能，我们立即返回----- return immediately if not enabled
    if (!_healthy) 
    {
        return;
    }
    if (!pNotify->_rgb_led_override) 
    {
        update_colours(); //更新颜色
        set_rgb(_red_des, _green_des, _blue_des); //发送设置颜色
    } else 
    {
        update_override();
    }
}

---

void RGBLed::update_colours(void)
{
    uint8_t brightness = _led_bright;

    switch (pNotify->_rgb_led_brightness) {
    case RGB_LED_OFF:
        brightness = _led_off; //亮度关闭
        break;
    case RGB_LED_LOW:          //亮度灰暗
        brightness = _led_dim;
        break;
    case RGB_LED_MEDIUM:
        brightness = _led_medium; //亮度中等
        break;
    case RGB_LED_HIGH:            //亮度非常亮
        brightness = _led_bright;
        break;
    }

    //比较慢的频率--------slow rate from 50Hz to 10hz
    counter++;
    if (counter < 5)//延迟100ms才执行代码
    {
        return;
    }

    //开始复位计算------reset counter
    counter = 0;

    //记录程序运行----------move forward one step
    step++;
    if (step >= 10) {
        step = 0;
    }

    // 使用USB，灯的颜色会变暗--------use dim light when connected through USB
    if (hal.gpio->usb_connected() && brightness > _led_dim) //连接使用USB，rgbled颜色会变暗
    {
        brightness = _led_dim;//颜色变暗
    }

    //初始化模式--------------------initialising pattern
    if (AP_Notify::flags.initialising) //初始化时,显示红蓝交替
    {
        if (step & 1)  /灯/奇数运行时，显示红
        {
            // odd steps display red light
            _red_des = brightness;
            _blue_des = _led_off;
            _green_des = _led_off;
        } else  //蓝灯偶数运行时选择
        {
            // even display blue light
            _red_des = _led_off;
            _blue_des = brightness;
            _green_des = _led_off;
        }

        //运行后退出-------exit so no other status modify this pattern
        return;
    }

    // 记录用于植保的AB点信息
    if(AP_Notify::flags.zigzag_record >1) //AP_Notify::flags.zigzag_record=16,记录A,
    //AP_Notify::flags.zigzag_record=81,记录B点信息
    {
        // 
        bool yellow = ((AP_Notify::flags.zigzag_record%2) == 0)?false:true; //=0，记录A,等于1，记录B点
        switch(step) 
        {
        case 0:
        case 1:
        case 2:
        case 3:
        case 4:
            if(yellow) //记录A点闪烁黄灯
            {
                // yellow on
                _red_des = brightness;
                _blue_des = _led_off;
                _green_des = brightness;
            }
            else //记录B点闪烁蓝灯
            {
                // blue on
                _red_des = _led_off;
                _blue_des = brightness;
                _green_des = _led_off;
            }
            break;
        case 5:
        case 6:
        case 7:
        case 8:
            // 亮一会，灭一会，实现闪烁
            _red_des = _led_off;
            _blue_des = _led_off;
            _green_des = _led_off;
            break;
        case 9:
            if(yellow)  //黄灯持续闪烁4次
            {
                AP_Notify::flags.zigzag_record /= 3;
            }
            else  //蓝灯闪烁四次
            {
                AP_Notify::flags.zigzag_record /= 2;
            }
            break;
        }

        return;
    }

    //地磁校准-----------------------for compass calibration
    if(AP_Notify::flags.compass_cal_status != 0) //开始校准
    {
        switch(AP_Notify::flags.compass_cal_status) //获取校准状态
        {
        case 1:
            // 首先亮粉色灯，采数据集Z+
            _red_des = brightness;
            _blue_des = brightness;
            _green_des = _led_off;
            break;
        case 2:
            //然后亮黄色灯，采集y+数据
            _red_des = brightness;
            _blue_des = _led_off;
            _green_des = brightness;
            break;
        case 3:
            // 亮青色，采集X+数据
            _red_des = _led_off;
            _blue_des = brightness;
            _green_des = brightness;
            break;
        case 4:
            //校准失败亮红
            _red_des = brightness;
            _blue_des = _led_off;
            _green_des = _led_off;
            break;
        default:
            break;
        }
        return;
    }

    //保存修改和进行电调校准save trim and esc calibration pattern
    if (AP_Notify::flags.save_trim || AP_Notify::flags.esc_calibration) 
    {
        switch(step) 
        {
            case 0:
            case 3:
            case 6:
                //红灯-------red on
                _red_des = brightness;
                _blue_des = _led_off;
                _green_des = _led_off;
                break;

            case 1:
            case 4:
            case 7:
                //蓝灯--------blue on
                _red_des = _led_off;
                _blue_des = brightness;
                _green_des = _led_off;
                break;

            case 2:
            case 5:
            case 8:
                //绿灯--------green on
                _red_des = _led_off;
                _blue_des = _led_off;
                _green_des = brightness;
                break;

            case 9:
                //关闭所有灯-------all off
                _red_des = _led_off;
                _blue_des = _led_off;
                _green_des = _led_off;
                break;
        }
        //退出模式------exit so no other status modify this pattern
        return;
    }
    //下面是失败指示灯
    //遥控器和电池故障闪黄灯------------radio and battery failsafe patter: flash yellow
    //Gps出现闪烁黄灯和蓝灯-------------PSgps failsafe pattern : flashing yellow and blue
    //ekf检测失败黄灯和蓝灯-------------ekf_bad pattern : flashing yellow and red
    if (AP_Notify::flags.failsafe_radio || AP_Notify::flags.failsafe_battery ||
            AP_Notify::flags.ekf_bad || AP_Notify::flags.gps_glitching || AP_Notify::flags.leak_detected) {
        switch(step) {
            case 0:
            case 1:
            case 2:
            case 3:
            case 4:
                //闪烁黄灯------------------yellow on
                _red_des = brightness;
                _blue_des = _led_off;
                _green_des = brightness;
                break;
            case 5:
            case 6:
            case 7:
            case 8:
            case 9:
                if (AP_Notify::flags.leak_detected)  //检测失败闪烁白色灯
                {
                    // purple if leak detected
                    _red_des = brightness;
                    _blue_des = brightness;
                    _green_des = brightness;
                } else if (AP_Notify::flags.ekf_bad) {  //红绿
                    // red on if ekf bad
                    _red_des = brightness;
                    _blue_des = _led_off;
                    _green_des = _led_off;
                } else if (AP_Notify::flags.gps_glitching)  //黄，蓝
                {
                    // blue on gps glitch
                    _red_des = _led_off;
                    _blue_des = brightness;
                    _green_des = _led_off;
                }else
                {
                    //遥控器故障或者电池故障--------all off for radio or battery failsafe
                    _red_des = _led_off;
                    _blue_des = _led_off;
                    _green_des = _led_off;
                }
                break;
        }
        //更改模式，立即退出------- exit so no other status modify this pattern
        return;
    }

    // 遥控器解锁一定是绿色或者蓝灯-------------solid green or blue if armed
    if (AP_Notify::flags.armed) 
    {
        // solid green if armed with GPS 3d lock
        if (AP_Notify::flags.gps_status >= AP_GPS::GPS_OK_FIX_3D)  //如果是GPS 3d定位了，一定闪烁绿灯
        {
            _red_des = _led_off;
            _blue_des = _led_off;
            _green_des = brightness;
        }else
        {
            //如果解锁在没有GPS的模式下解锁，亮蓝灯------------------------solid blue if armed with no GPS lock
            _red_des = _led_off;
            _blue_des = brightness;
            _green_des = _led_off;
        }
        return;
    }
    else //没有解锁
    {
        //如果解锁检测失败双闪黄灯--------double flash yellow if failing pre-arm checks
        if (!AP_Notify::flags.pre_arm_check) 
        {
            switch(step) {
                case 0:
                case 1:
                case 4:
                case 5:
                    //黄灯----- yellow on
                    _red_des = brightness;
                    _blue_des = _led_off;
                    _green_des = brightness;
                    break;
                case 2:
                case 3:
                case 6:
                case 7:
                case 8:
                case 9:
                    // all off
                    _red_des = _led_off;
                    _blue_des = _led_off;
                    _green_des = _led_off;
                    break;
            }
        }else{
            //在3D定位下，没有解锁，快速闪烁绿灯-------fast flashing green if disarmed with GPS 3D lock and DGPS
            //慢速闪烁绿灯-------slow flashing green if disarmed with GPS 3d lock (and no DGPS)
            //蓝检灯闪烁，没有解锁或者GPS没有定位，或者gps检测执行失败------flashing blue if disarmed with no gps lock or gps pre_arm checks have failed
            bool fast_green = AP_Notify::flags.gps_status >= AP_GPS::GPS_OK_FIX_3D_DGPS && AP_Notify::flags.pre_arm_gps_check;
            switch(step) {
                case 0:
                    if (fast_green) {
                        _green_des = brightness;
                    }
                    break;
                case 1:
                    if (fast_green) {
                        _green_des = _led_off;
                    }
                    break;
                case 2:
                    if (fast_green) {
                        _green_des = brightness;
                    }
                    break;
                case 3:
                    if (fast_green) {
                        _green_des = _led_off;
                    }
                    break;
                case 4:
                    _red_des = _led_off;
                    if (AP_Notify::flags.gps_status >= AP_GPS::GPS_OK_FIX_3D && AP_Notify::flags.pre_arm_gps_check) 
                    {
                        // flashing green if disarmed with GPS 3d lock
                        _blue_des = _led_off;
                        _green_des = brightness;
                    }else
                    {
                        // flashing blue if disarmed with no gps lock
                        _blue_des = brightness;
                        _green_des = _led_off;
                    }
                    break;
                case 5:
                    if (fast_green) {
                        _green_des = _led_off;
                    }
                    break;

                case 6:
                    if (fast_green) {
                        _green_des = brightness;
                    }
                    break;

                case 7:
                    if (fast_green) {
                        _green_des = _led_off;
                    }
                    break;
                case 8:
                    if (fast_green) {
                        _green_des = brightness;
                    }
                    break;
                case 9:
                    // all off
                    _red_des = _led_off;
                    _blue_des = _led_off;
                    _green_des = _led_off;
                    break;
            }
        }
    }
}