devbox
IT小白
这个屌丝很懒，什么也没留下！
关注作者
热门标签
热门文章
当前位置:   article > 正文

pixhawk 光流--位置估计--姿态估计--位置控制--姿态控制_motorspeed = map(abs(tiltangle), 0, 90, 0, 500);

作者：IT小白 | 2024-04-26 03:55:39

赞

踩

motorspeed = map(abs(tiltangle), 0, 90, 0, 500);

本文是边分析边写的，顺便就记录下了分析的思路，并不是按照教材那种思路介绍性的，而是按照程序员分析程序的思路来的。所以读者跟着看有些地方看了意义不大，但是这种程序分析的思路还是可以借鉴的，如果有大神看到了本文，有更好的见解，欢迎指教。

前面的是从APM代码角度看的，后面是从原生码角度看的。这blog写的我自己看了都想吐，翻下去都是代码，就最后一张图人性化一点。

温馨提示：可以从后面点看，程序中有注释。

先从ardupilot/libraries/AP_OpticalFlow/examples/AP_OpticalFlow_test/ AP_OpticalFlow_test.cpp看起

  1. voidsetup()
  2. {
  3.     hal.console->println("OpticalFlowlibrary test ver 1.6");
  4.  
  5.     hal.scheduler->delay(1000);
  6.  
  7.     // flowSensor initialization
  8.     optflow.init();
  9.  
  10.     if (!optflow.healthy()) {
  11.         hal.console->print("Failed toinitialise PX4Flow ");
  12.     }
  13.  
  14.     hal.scheduler->delay(1000);只需要看
  15. }
  16. voidloop()
  17. {
  18.     hal.console->println("this onlytests compilation succeeds");
  19.  
  20.     hal.scheduler->delay(5000);
  21. }

因此optflow.init();再进行判断if (!optflow.healthy())，任务就完成了

跳转到optflow.init();

  1. voidOpticalFlow::init(void)
  2. {
  3.     if (backend != NULL) {
  4.         backend->init();
  5.     } else {
  6.         _enabled = 0;
  7.     }
  8. }

跳转到init();

  1. classOpticalFlow_backend
  2. {
  3.     friend class OpticalFlow;
  4.  
  5. public:
  6.     // constructor
  7.     OpticalFlow_backend(OpticalFlow&_frontend) : frontend(_frontend) {}
  8.  
  9.     // init - initialise sensor
  10.     virtual void init() = 0;
  11.  
  12.     // read latest values from sensor and fillin x,y and totals.
  13.     virtual void update() = 0;
  14.  
  15. protected:
  16.     // access to frontend
  17.     OpticalFlow &frontend;
  18.  
  19.     // update the frontend
  20.     void _update_frontend(const structOpticalFlow::OpticalFlow_state &state);
  21.  
  22.     // get the flow scaling parameters
  23.     Vector2f _flowScaler(void) const { returnVector2f(frontend._flowScalerX, frontend._flowScalerY); }
  24.  
  25.     // get the yaw angle in radians
  26.     float _yawAngleRad(void) const { returnradians(float(frontend._yawAngle_cd) * 0.01f); }
  27. };

看到了吧，virtualvoid init() = 0;就是这个虚函数

怎么找到它的实例化呢？

Ctrl+H全局搜索public OpticalFlow_backend


路径是：ardupilot/libraries/AP_OpticalFlow/ AP_OpticalFlow_PX4.h

  1. 路径是：ardupilot/libraries/AP_OpticalFlow/ AP_OpticalFlow_PX4.h
  2. class AP_OpticalFlow_PX4 : public OpticalFlow_backend
  3. {
  4. public:
  5.     /// constructor
  6.     AP_OpticalFlow_PX4(OpticalFlow &_frontend);
  7.  
  8.     // init - initialise the sensor
  9.     void init();
  10.  
  11.     // update - read latest values from sensor and fill in x,y and totals.
  12.     void update(void);
  13.  
  14. private:
  15.     int         _fd;                // file descriptor for sensor
  16.     uint64_t    _last_timestamp;    // time of last update (used to avoid processing old reports)
  17. };
  18. void init();就是实例化，继续搜索AP_OpticalFlow_PX4:: init

void init();就是实例化，继续搜索AP_OpticalFlow_PX4::init


本体就是

  1. voidAP_OpticalFlow_PX4::init(void)
  2. {
  3.    _fd = open(PX4FLOW0_DEVICE_PATH, O_RDONLY);//关于sd卡文件的read
  4.  
  5.    // check for failure to open device
  6.    if (_fd == -1) {
  7.        return;
  8.     }
  9.  
  10.    // change to 10Hz update
  11.    if (ioctl(_fd, SENSORIOCSPOLLRATE, 10) != 0) {
  12.        hal.console->printf("Unable to set flow rate to10Hz\n");       
  13.     }
  14. }

慢慢查看，先看_fd = open(PX4FLOW0_DEVICE_PATH, O_RDONLY);源码如下

路径是ardupilot/modules/PX4NuttX/nuttx/fs/fs_open.c

  1. /****************************************************************************
  2.  *Name: open
  3.  *
  4.  *Description:
  5.  *   Standard 'open' interface
  6.  *
  7.  ****************************************************************************/
  8. int open(const char *path, int oflags, ...)
  9. {
  10.   FARstruct filelist *list;
  11.   FARstruct inode    *inode;
  12.   FARconst char      *relpath = NULL;
  13. #if defined(CONFIG_FILE_MODE) ||!defined(CONFIG_DISABLE_MOUNTPOINT)
  14.  mode_t               mode = 0666;
  15. #endif
  16.  int                  ret;
  17.  int                  fd;
  18.   /*Get the thread-specific file list */得到具体线程列表
  19.  list = sched_getfiles();
  20.   if(!list)
  21.     {
  22.      ret = EMFILE;
  23.      goto errout;
  24.     }
  25. #ifdef CONFIG_FILE_MODE
  26. # ifdef CONFIG_CPP_HAVE_WARNING
  27. #   warning "File creation not implemented"
  28. #  endif
  29.   /*If the file is opened for creation, then get the mode bits */
  30.   if(oflags & (O_WRONLY|O_CREAT) != 0)
  31.     {
  32.      va_list ap;
  33.      va_start(ap, oflags);
  34.      mode = va_arg(ap, mode_t);
  35.      va_end(ap);
  36.     }
  37. #endif
  38.   /*Get an inode for this file */得到索引节点
  39.  inode = inode_find(path, &relpath);
  40.   if(!inode)
  41.     {
  42.      /* "O_CREAT is not set and the named file does not exist.  Or, a
  43.       * directory component in pathname does not exist or is a dangling
  44.       * symbolic link."
  45.       */
  46.      ret = ENOENT;
  47.      goto errout;
  48.     }
  49.   /*Verify that the inode is valid and either a "normal" or amountpoint.  We
  50.    *specifically exclude block drivers.
  51.    */
  52. #ifndef CONFIG_DISABLE_MOUNTPOINT
  53.   if((!INODE_IS_DRIVER(inode) && !INODE_IS_MOUNTPT(inode)) ||!inode->u.i_ops)
  54. #else
  55.   if(!INODE_IS_DRIVER(inode) || !inode->u.i_ops)
  56. #endif
  57.     {
  58.      ret = ENXIO;
  59.      goto errout_with_inode;
  60.     }
  61.   /*Make sure that the inode supports the requested access */
  62.   ret= inode_checkflags(inode, oflags);
  63.   if(ret < 0)
  64.     {
  65.      ret = -ret;
  66.      goto errout_with_inode;
  67.     }
  68.   /*Associate the inode with a file structure */
  69.   fd= files_allocate(inode, oflags, 0, 0);
  70.   if(fd < 0)
  71.     {
  72.      ret = EMFILE;
  73.      goto errout_with_inode;
  74.     }
  75.   /*Perform the driver open operation.  NOTEthat the open method may be
  76.    *called many times.  The driver/mountpointlogic should handled this
  77.    *because it may also be closed that many times.
  78.    */
  79.   ret= OK;
  80.   if(inode->u.i_ops->open)
  81.     {
  82. #ifndef CONFIG_DISABLE_MOUNTPOINT
  83.      if (INODE_IS_MOUNTPT(inode))
  84.        {
  85.          ret = inode->u.i_mops->open((FAR structfile*)&list->fl_files[fd],
  86.                                       relpath,oflags, mode);
  87.        }
  88.      else
  89. #endif
  90.        {
  91.          ret = inode->u.i_ops->open((FAR structfile*)&list->fl_files[fd]);
  92.        }
  93.     }
  94.   if(ret < 0)
  95.     {
  96.      ret = -ret;
  97.      goto errout_with_fd;
  98.     }
  99.  return fd;
  100.  errout_with_fd:
  101.  files_release(fd);
  102.  errout_with_inode:
  103.  inode_release(inode);
  104.  errout:
  105.  set_errno(ret);
  106.  return ERROR;
  107. }
  108. 跳转到list = sched_getfiles();
  109. 路径是ardupilot/modules/PX4NuttX/nuttx/sched/sched_getfiles.c
  110. /************************************************************************
  111.  *Name: sched_getfiles
  112.  *
  113.  *Description:
  114.  *  Return a pointer to the file list for this thread
  115.  *
  116.  *Parameters:
  117.  *   None
  118.  *
  119.  *Return Value:
  120.  *   Apointer to the errno.
  121.  *
  122.  *Assumptions:
  123.  *
  124.  ************************************************************************/
  125. #if CONFIG_NFILE_DESCRIPTORS > 0
  126. FAR struct filelist *sched_getfiles(void)
  127. {
  128.   FARstruct tcb_s *rtcb = (FAR struct tcb_s*)g_readytorun.head;
  129.   FARstruct task_group_s *group = rtcb->group;
  130.   /*The group may be NULL under certain conditions. For example, if
  131.    *debug output is attempted from the IDLE thead before the group has
  132.    *been allocated.  I have only seen thiscase when memory management
  133.    *debug is enabled.
  134.    */
  135.   if(group)
  136.     {
  137.      return &group->tg_filelist;
  138.     }
  139.   /*Higher level logic must handle the NULL gracefully */
  140.  return NULL;
  141. }
  142. #endif /* CONFIG_NFILE_DESCRIPTORS */
  143. 跳转到inode = inode_find(path, &relpath);
  144. 路径是ardupilot/modules/PX4NuttX/nuttx/fs/fs_inodefine.c
  145. /****************************************************************************
  146.  *Name: inode_find
  147.  *
  148.  *Description:
  149.  *   Thisis called from the open() logic to get a reference to the inode
  150.  *  associated with a path.
  151.  *
  152.  ****************************************************************************/
  153. //得到一个inode与路径相关的引用
  154. FAR struct inode *inode_find(FAR const char*path, FAR const char **relpath)
  155. {
  156.   FARstruct inode *node;
  157.   if(!*path || path[0] != '/')
  158.     {
  159.      return NULL;
  160.     }
  161.   /*Find the node matching the path.  Iffound, increment the count of
  162.    *references on the node.
  163.    */
  164.  inode_semtake();
  165.  node = inode_search(&path, (FAR struct inode**)NULL, (FAR struct inode**)NULL,relpath);
  166.   if(node)
  167.     {
  168.      node->i_crefs++;
  169.     }
  170.  inode_semgive();
  171.  return node;
  172. }

看到不大懂，姑且认为是关于sd卡文件的read

#definePX4FLOW0_DEVICE_PATH "/dev/px4flow0"路径ardupilot/modules/PX4Firmware/src/drivers/drv_px4flow.h

  1. void AP_OpticalFlow_PX4::init(void)
  2. {
  3.    _fd = open(PX4FLOW0_DEVICE_PATH, O_RDONLY);
  4.    // check for failure to open device
  5.    if (_fd == -1) {
  6.        return;
  7.     }
  8.    // change to 10Hz update
  9.    if (ioctl(_fd, SENSORIOCSPOLLRATE, 10) != 0) {
  10.        hal.console->printf("Unable to set flow rate to10Hz\n");       
  11.     }
  12. }

继续往下看ioctl(_fd, SENSORIOCSPOLLRATE, 10);这句话的目的是控制光流跟新频率为10Hz

分别看ioctl的输入

_fd = open(PX4FLOW0_DEVICE_PATH, O_RDONLY);_fd用来判断是否有这个路径并读到了东西

SENSORIOCSPOLLRATE设置传感器读取的时间间隔

路径ardupilot/modules/PX4Firmware/src/drivers/drv_sensor.h

#define SENSORIOCSPOLLRATE       _SENSORIOC(0)

#define _SENSORIOC(_n)                   (_PX4_IOC(_SENSORIOCBASE,_n))

#define _SENSORIOCBASE                (0x2000)ardupilot/modules/PX4Firmware/src/drivers/drv_sensor.h

#define _PX4_IOC(x,y) _IOC(x,y)    ardupilot/modules/PX4Firmware/src/platforms/px4_defines.h

#define_IOC(type,nr)   ((type)|(nr))

也就是说SENSORIOCSPOLLRATE等价于_PX4_IOC(0x2000,0)等价于(0x2000)|(0)

ardupilot/modules/PX4Firmware/Build/px4fmu-v2_APM.build/nuttx-export/include/nuttx/fs/ioctl.h

与上面的宏定义相比

#define_PX4FLOWIOCBASE                       (0x7700)

#define__PX4FLOWIOC(_n)             (_IOC(_PX4FLOWIOCBASE,_n))

 

另外，在ardupilot/modules/PX4Firmware/src/drivers/drv_px4flow.h中找到“px4flow drivers also implement the generic sensordriver interfaces from drv_sensor.h”，表明光流的驱动在通用传感器驱动文件中ardupilot/modules/PX4Firmware/src/drivers/drv_sensor.h

跳转到ioctl(_fd,SENSORIOCSPOLLRATE, 10)

  1. /****************************************************************************
  2.  *Name: ioctl
  3.  *
  4.  *Description:
  5.  *  Perform device specific operations.
  6.  *
  7.  *Parameters:
  8.  *  fd       File/socket descriptor ofdevice
  9.  *  req      The ioctl command
  10.  *  arg      The argument of the ioctlcmd
  11.  *
  12.  *Return:
  13.  *  >=0 on success (positive non-zero values are cmd-specific)
  14.  *   -1on failure withi errno set properly:
  15.  *
  16.  *  EBADF
  17.  *    'fd' is not a valid descriptor.
  18.  *  EFAULT
  19.  *    'arg' references an inaccessible memory area.
  20.  *  EINVAL
  21.  *    'cmd' or 'arg' is not valid.
  22.  *  ENOTTY
  23.  *    'fd' is not associated with a character special device.
  24.  *  ENOTTY
  25.  *     The specified request does not apply to the kind of object that the
  26.  *     descriptor 'fd' references.
  27.  *
  28.  ****************************************************************************/
  29. int ioctl(int fd, int req, unsigned longarg)
  30. {
  31.   interr;
  32. #if CONFIG_NFILE_DESCRIPTORS > 0
  33.   FARstruct filelist *list;
  34.   FARstruct file     *this_file;
  35.   FARstruct inode    *inode;
  36.   int                  ret = OK;
  37.   /*Did we get a valid file descriptor? */
  38.   if((unsigned int)fd >= CONFIG_NFILE_DESCRIPTORS)
  39. #endif
  40.     {
  41.       /* Perform the socket ioctl */
  42. #if defined(CONFIG_NET) &&CONFIG_NSOCKET_DESCRIPTORS > 0
  43.      if ((unsigned int)fd <(CONFIG_NFILE_DESCRIPTORS+CONFIG_NSOCKET_DESCRIPTORS))
  44.        {
  45.          return netdev_ioctl(fd, req, arg);
  46.        }
  47.      else
  48. #endif
  49.        {
  50.          err = EBADF;
  51.          goto errout;
  52.        }
  53.     }
  54. #if CONFIG_NFILE_DESCRIPTORS > 0
  55.   /*Get the thread-specific file list */
  56.  list = sched_getfiles();
  57.   if(!list)
  58.     {
  59.      err = EMFILE;
  60.      goto errout;
  61.     }
  62.   /*Is a driver registered? Does it support the ioctl method? */
  63.  this_file = &list->fl_files[fd];
  64.  inode     = this_file->f_inode;
  65.   if(inode && inode->u.i_ops && inode->u.i_ops->ioctl)
  66.     {
  67.      /* Yes, then let it perform the ioctl */
  68.       ret = (int)inode->u.i_ops->ioctl(this_file,req, arg);
  69.      if (ret < 0)
  70.        {
  71.          err = -ret;
  72.          goto errout;
  73.        }
  74.     }
  75.  return ret;
  76. #endif
  77. errout:
  78.  set_errno(err);
  79.  return ERROR;
  80. }

这句话是这个函数的核心

/* Yes, then let it perform the ioctl */

ret =(int)inode->u.i_ops->ioctl(this_file, req, arg);

跳转至ardupilot/modules/PX4Firmware/Build/px4fmu-v2_APM.build/nuttx-export/include/nuttx/fs/fs.h

int    (*ioctl)(FAR struct file *filp, int cmd, unsigned long arg);

无意中跑到ardupilot/libraries/AP_HAL_PX4/GPIO.cpp中，Ctrl+点击ioctl

可以搜索到

现在可以总结关于光流的程序有哪些部分了：

ardupilot/libraries/AP_OpticalFlow文件夹

ardupilot/modules/PX4Firmware/src/drivers/drv_px4flow.h

ardupilot/modules/PX4Firmware/src/drivers/px4flow文件夹

顺便再ardupilot/modules/PX4Firmware/src/drivers/px4flow/px4flow.cpp里面找到了

extern "C"__EXPORT int px4flow_main(int argc, char *argv[]);

也就是程序入口，久违的感动

现在有2条思路：一是继续跳转至ioctl实例化，继续看如何执行的；二是顺着main来看，先还是按照老思路example来

  1. int
  2. PX4FLOW::ioctl(structfile *filp, int cmd, unsigned long arg)
  3. {
  4.          switch(cmd) {
  5.          caseSENSORIOCSPOLLRATE: {
  6.                             switch(arg) {
  7.                             /*switching to manual polling */
  8.                             caseSENSOR_POLLRATE_MANUAL:
  9.                                      stop();
  10.                                      _measure_ticks= 0;
  11.                                      returnOK;
  12.                             /*external signalling (DRDY) not supported */
  13.                             case SENSOR_POLLRATE_EXTERNAL:
  14.                             /*zero would be bad */
  15.                             case0:
  16.                                      return-EINVAL;
  17.                             /*set default/max polling rate */
  18.                             caseSENSOR_POLLRATE_MAX:
  19.                             caseSENSOR_POLLRATE_DEFAULT: {
  20.                                                /*do we need to start internal polling? */
  21.                                                boolwant_start = (_measure_ticks == 0);
  22.                                                /*set interval for next measurement to minimum legal value */
  23.                                                _measure_ticks= USEC2TICK(PX4FLOW_CONVERSION_INTERVAL);
  24.                                                /*if we need to start the poll state machine, do it */
  25.                                                if(want_start) {
  26.                                                         start();
  27.                                                }
  28.                                                returnOK;
  29.                                      }
  30.                             /*adjust to a legal polling interval in Hz */
  31.                             default:{
  32.                                                /*do we need to start internal polling? */
  33.                                                boolwant_start = (_measure_ticks == 0);
  34.                                                /*convert hz to tick interval via microseconds */
  35.                                                unsignedticks = USEC2TICK(1000000 / arg);
  36.                                                /*check against maximum rate */
  37.                                                if(ticks < USEC2TICK(PX4FLOW_CONVERSION_INTERVAL)) {
  38.                                                         return-EINVAL;
  39.                                                }
  40.                                                /*update interval for next measurement */
  41.                                                _measure_ticks= ticks;
  42.                                                /*if we need to start the poll state machine, do it */
  43.                                                if(want_start) {
  44.                                                         start();
  45.                                                }
  46.                                                returnOK;
  47.                                      }
  48.                             }
  49.                    }
  50.          caseSENSORIOCGPOLLRATE:
  51.                    if(_measure_ticks == 0) {
  52.                             returnSENSOR_POLLRATE_MANUAL;
  53.                    }
  54.                    return(1000 / _measure_ticks);
  55.          caseSENSORIOCSQUEUEDEPTH: {
  56.                             /*lower bound is mandatory, upper bound is a sanity check */
  57.                             if((arg < 1) || (arg > 100)) {
  58.                                      return-EINVAL;
  59.                             }
  60.                             irqstate_t flags = irqsave();
  61.                             if(!_reports->resize(arg)) {
  62.                                      irqrestore(flags);
  63.                                      return-ENOMEM;
  64.                             }
  65.                             irqrestore(flags);
  66.                             returnOK;
  67.                    }
  68.          caseSENSORIOCGQUEUEDEPTH:
  69.                    return_reports->size();
  70.          caseSENSORIOCSROTATION:
  71.                    _sensor_rotation= (enum Rotation)arg;
  72.                    returnOK;
  73.          caseSENSORIOCGROTATION:
  74.                    return_sensor_rotation;
  75.          caseSENSORIOCRESET:
  76.                    /*XXX implement this */
  77.                    return-EINVAL;
  78.          default:
  79.                    /*give it to the superclass */
  80.                    returnI2C::ioctl(filp, cmd, arg);
  81.          }
  82. }

这个函数很好理解，根据传入的参数对号入座，传入的是SENSORIOCSPOLLRATE，那么进入

  1. case SENSORIOCSPOLLRATE: {
  2.                             switch(arg) {
  3.                             /*switching to manual polling */
  4.                             caseSENSOR_POLLRATE_MANUAL:
  5.                                      stop();
  6.                                      _measure_ticks= 0;
  7.                                      returnOK;
  8.                             /*external signalling (DRDY) not supported */
  9.                             case SENSOR_POLLRATE_EXTERNAL:
  10.                             /*zero would be bad */
  11.                             case0:
  12.                                      return-EINVAL;
  13.                             /*set default/max polling rate */
  14.                             caseSENSOR_POLLRATE_MAX:
  15.                             caseSENSOR_POLLRATE_DEFAULT: {
  16.                                                /*do we need to start internal polling? */
  17.                                                boolwant_start = (_measure_ticks == 0);
  18.                                                /*set interval for next measurement to minimum legal value */
  19.                                                _measure_ticks= USEC2TICK(PX4FLOW_CONVERSION_INTERVAL);
  20.                                                /*if we need to start the poll state machine, do it */
  21.                                                if(want_start) {
  22.                                                         start();
  23.                                                }
  24.                                                returnOK;
  25.                                      }
  26.                             /*adjust to a legal polling interval in Hz */
  27.                             default:{
  28.                                                /*do we need to start internal polling? */
  29.                                                boolwant_start = (_measure_ticks == 0);
  30.                                                /*convert hz to tick interval via microseconds */
  31.                                                unsignedticks = USEC2TICK(1000000 / arg);
  32.                                                /*check against maximum rate */
  33.                                                if(ticks < USEC2TICK(PX4FLOW_CONVERSION_INTERVAL)) {
  34.                                                         return-EINVAL;
  35.                                                }
  36.                                                /*update interval for next measurement */
  37.                                                _measure_ticks= ticks;
  38.                                                /* if we need to start thepoll state machine, do it */
  39.                                                if(want_start) {
  40.                                                         start();
  41.                                                }
  42.                                                returnOK;
  43.                                      }

Ok，下一个参数是10，那么进入default

看注释能明白default是用来调整合适的间隔

里面需要重点看的是

unsigned ticks =USEC2TICK(1000000 / arg);// 看注释能明白应该是处理时间间隔的

start();//轮询状态机

先看unsigned ticks = USEC2TICK(1000000 / arg);

跳转至ardupilot/modules/PX4Firmware/Build/px4fmu-v2_APM.build/nuttx-export/include/nuttx/clock.h

#define USEC2TICK(usec)      (((usec)+(USEC_PER_TICK/2))/USEC_PER_TICK) /* Rounds */

这里并没有寄存器的相关操作，只是把用户所定的时间进行了一些转换，方便单片机之后定时操作

那么跳转至start();

  1. void
  2. PX4FLOW::start()
  3. {
  4.          /*reset the report ring and state machine *///复位报告环和状态机
  5.          _collect_phase= false;
  6.          _reports->flush();
  7.          /*schedule a cycle to start things *///安排一个周期开始一些事情
  8.          work_queue(HPWORK,&_work, (worker_t)&PX4FLOW::cycle_trampoline, this, 1);
  9.          /*notify about state change */
  10.          structsubsystem_info_s info = {
  11.                    true,
  12.                    true,
  13.                    true,
  14.                    subsystem_info_s::SUBSYSTEM_TYPE_OPTICALFLOW
  15.          };
  16.          staticorb_advert_t pub = nullptr;
  17.          if(pub != nullptr) {
  18.                    orb_publish(ORB_ID(subsystem_info),pub, &info);
  19.          }else {
  20.                    pub= orb_advertise(ORB_ID(subsystem_info), &info);
  21.          }
  22. }

看函数名字的意思是启动光流

看来最关键的是work_queue(HPWORK, &_work, (worker_t)&PX4FLOW::cycle_trampoline,this, 1)再后面就是发布和公告子系统信息，跳转至work_queue(HPWORK, &_work,(worker_t)&PX4FLOW::cycle_trampoline, this, 1);


  1. /****************************************************************************
  2.  *Name: work_queue
  3.  *
  4.  *Description:
  5.  *  Queue work to be performed at a later time.  All queued work will be
  6.  *  performed on the worker thread of of execution (not the caller's).
  7.  *
  8.  *   Thework structure is allocated by caller, but completely managed by
  9.  *   thework queue logic.  The caller shouldnever modify the contents of
  10.  *   thework queue structure; the caller should not call work_queue()
  11.  *  again until either (1) the previous work has been performed and removed
  12.  *   fromthe queue, or (2) work_cancel() has been called to cancel the work
  13.  *   andremove it from the work queue.
  14.  *
  15.  *Input parameters:
  16.  *  qid    - The work queue ID (index)
  17.  *  work   - The work structure toqueue
  18.  *  worker - The worker callback to be invoked.  The callback will invoked
  19.  *           on the worker thread of execution.
  20.  *  arg    - The argument that will bepassed to the workder callback when
  21.  *           int is invoked.
  22.  *  delay  - Delay (in clock ticks)from the time queue until the worker
  23.  *           is invoked. Zero means to perform the work immediately.
  24.  *
  25.  *Returned Value:
  26.  *   Zeroon success, a negated errno on failure
  27.  *
  28.  ****************************************************************************/
  29. 注释翻译：
  30. 在稍后的时间将要执行的队列的工作。 所有排队工作将在工作线程（不是调用的程序）上进行。
  31. 工作结构由调用者分配，but，由工作队列逻辑完全管理。调用者不能修改工作队列结构的内容，也不能再次调用work_queue()，除非任务执行完毕或者任务被取消
  32. 输入参数：
  33. qid    -工作队列的ID
  34. work  -队列的工作结构
  35. worker -工作线程被执行，worker callback 会被调用
  36. arg    -参数会传递给worker callback
  37. delay  -延时多久再执行这个任务，若为0，则直接执行
  38. 返回值：
  39. 0，则成功执行；错误号，则失败
  40. int work_queue(int qid, FAR struct work_s*work, worker_t worker,
  41.                FAR void *arg, uint32_t delay)
  42. {
  43.   FARstruct wqueue_s *wqueue = &g_work[qid];
  44.  irqstate_t flags;
  45.  DEBUGASSERT(work != NULL && (unsigned)qid < NWORKERS);//应该是调试用的debug
  46.   /* First, initialize the work structure */
  47.   work->worker = worker;           /* Work callback */
  48.   work->arg    = arg;              /* Callback argument */
  49.   work->delay  = delay;            /* Delay until work performed */
  50.   /*Now, time-tag that entry and put it in the work queue.  This must be
  51.    *done with interrupts disabled.  Thispermits this function to be called
  52.    *from with task logic or interrupt handlers.
  53.    */
  54.   flags        = irqsave();
  55.   work->qtime  = clock_systimer(); /* Time work queued */
  56.  dq_addlast((FAR dq_entry_t *)work, &wqueue->q);
  57.  kill(wqueue->pid, SIGWORK);     /* Wake up the worker thread */
  58.  irqrestore(flags);
  59.  return OK;
  60. }

在看一下调用的函数work_queue(HPWORK, &_work,(worker_t)&PX4FLOW::cycle_trampoline, this, 1);

int work_queue(int qid, FAR struct work_s*work, worker_t worker, FAR void *arg, uint32_t delay)

这个应该是比较关键的函数，一点点看

  1. FAR struct wqueue_s *wqueue =&g_work[qid];
  2. /* This structure defines the state on onework queue.  Thisstructure is
  3.  * used internally by the OS and worker queuelogic and should not be
  4.  * accessed by application logic.
  5.  */
  6. 注释翻译：
  7. 这个结构体定义了一个工作队列的状态。这种结构是由系统和worker队列逻辑在内部使用，不应被应用程序逻辑访问。
  8. struct wqueue_s
  9. {
  10.  pid_t             pid; /* The taskID of the worker thread *///worker线程中的任务ID
  11.  struct dq_queue_s q;   /* Thequeue of pending work *///挂起的工作队列
  12. };
  13. typedef int16_t      pid_t;
  14. struct dq_queue_s
  15. {
  16.   FARdq_entry_t *head;//头
  17.   FARdq_entry_t *tail;//尾
  18. };

也就是说

  FARstruct wqueue_s *wqueue = &g_work[qid];

 irqstate_t flags;

这2句是声明数据类型

  /*First, initialize the work structure *///初始化work结构体

 work->worker = worker;          /* Work callback */

 work->arg    = arg;              /* Callback argument */

 work->delay  = delay;            /* Delay until work performed */

work的定义struct work_s work;

work_s的定义

  1. /* Defines one entry in the workqueue.  The user only needs thisstructure
  2.  * inorder to declare instances of the work structure.  Handling of all
  3.  *fields is performed by the work APIs
  4.  */
  5. 注释翻译：定义一个工作队列的入口。使用前声明这个结构，再实例化。所有处理都是有API来完成的
  6. struct work_s
  7. {
  8.   structdq_entry_s dq;  /* Implements a doublylinked list */
  9.  worker_t  worker;      /* Work callback */
  10.   FARvoid *arg;         /* Callback argument*/
  11.  uint32_t  qtime;       /* Time work queued */
  12.  uint32_t  delay;       /* Delay until work performed */
  13. };
  14.  work->worker = worker;          /* Work callback */
  15.  work->arg    = arg;              /* Callback argument */
  16.  work->delay  = delay;            /* Delay until work performed */
  17. /* Now, time-tag that entry and put it inthe work queue.  This must be
  18.    *done with interrupts disabled.  Thispermits this function to be called
  19.    *from with task logic or interrupt handlers.
  20.    */
  21. 注释翻译：现在，时间标记该条目，并把它的工作队列。这个需要在中断失能的情况下完成。允许这个函数从任务逻辑或中断处理程序调用。
  22. flags       = irqsave();/* Save the current primask state& disable IRQs */保存当前primask的状态，并失能中断
  23. work->qtime  = clock_systimer(); /* Time work queued */

跳转至irqsave();

  1. static inline irqstate_t irqsave(void)
  2. {
  3. #ifdef CONFIG_ARMV7M_USEBASEPRI
  4.  uint8_t basepri = getbasepri();
  5.  setbasepri(NVIC_SYSH_DISABLE_PRIORITY);
  6.  return (irqstate_t)basepri;
  7. #else
  8.  unsigned short primask;
  9.   /*Return the current value of primask register and set
  10.    *bit 0 of the primask register to disable interrupts
  11.    */
  12.  __asm__ __volatile__
  13.     (
  14.     "\tmrs    %0, primask\n"
  15.     "\tcpsid  i\n"
  16.     : "=r" (primask)
  17.     :
  18.     : "memory");
  19.  return primask;
  20. #endif
  21. }

PRIMASK位：只允许NMI和hard  fault异常，其他中断/ 异常都被屏蔽(当前CPU优先级=0)。(stm32的一个中断控制寄存器的位)

跳转至clock_systimer()

/* Access to raw system clock*****************/访问原始系统时钟

/* Direct access to the systemtimer/counter is supported only if (1) the

 *system timer counter is available (i.e., we are notconfigured to use

 * ahardware periodic timer), and (2) the execution environment has direct

 *access to kernel global data

 */

注释翻译：下面2种情况可以直接访问系统时钟/计数器，(1)系统时钟是可得的(这就是说，我们没有配置硬件周期定时)，(2)执行环境可以直接访问内核全局数据

extern volatile uint32_t g_system_timer

#define clock_systimer() g_system_timer

这个意思是从其他地方传来内核全局数据g_system_timer赋给work->qtime

关于g_system_timer的地方，只有

/****************************************************************************

 * Name: clock_timer

 *

 * Description:

 *   Thisfunction must be called once every time the real time clock

 *  interrupt occurs.  The interval ofthis clock interrupt must be

 *  MSEC_PER_TICK

 *

 ****************************************************************************/

void clock_timer(void)

{

 /* Increment the per-tick system counter */

 g_system_timer++;

}

就是每一次时钟中断g_system_timer++一次，另g_system_timer初始化为0

 

dq_addlast((FAR dq_entry_t *)work,&wqueue->q);

跳转至dq_addlast()//dq_addlast adds 'node' to the end of 'queue'  dq_addlast增加'节点'到'队列'的末端

输入参数就是之前声明的work结构体和wqueue结构体

struct dq_queue_s

{

 FAR dq_entry_t *head;

 FAR dq_entry_t *tail;

};

 

kill(wqueue->pid,SIGWORK);      /* Wake up theworker thread */唤醒工作进程

跳转至kill()

  1. /************************************************************************
  2.  * Name: kill
  3.  *
  4.  * Description:
  5.  *   Thekill() system call can be used to send any signal to any task.
  6.  *
  7.  *  Limitation: Sending of signals to 'process groups' is not
  8.  *  supported in NuttX
  9.  *
  10.  * Parameters:
  11.  *   pid- The id of the task to receive the signal. The POSIX kill
  12.  *    specification encodes process group information as zero and
  13.  *    negative pid values.  Onlypositive, non-zero values of pid are
  14.  *    supported by this implementation.
  15.  *  signo - The signal number to send. If signo is zero, no signal is
  16.  *    sent, but all error checking is performed.
  17.  *
  18.  * Returned Value:
  19.  *    Onsuccess (at least one signal was sent), zero is returned.  On
  20.  *   error, -1 is returned, and errno is set appropriately:
  21.  *
  22.  *   EINVAL An invalid signal was specified.
  23.  *   EPERM  The process does not havepermission to send the
  24.  *          signal to any of the target processes.
  25.  *   ESRCH  The pid or process groupdoes not exist.
  26.  *   ENOSYS Do not support sending signals to process groups.
  27.  *
  28.  * Assumptions:
  29.  *
  30.  ************************************************************************/
  31. 注释翻译：
  32. 这个kill()系统调用可用于任何信号发送到任何任务。
  33. 限制：信号发送到“进程组”在NuttX不支持
  34. 参数：
  35. pid -任务的ID用来接收信号。如果pid的值是0或负的，POSIX的kill规范会编码进程组信息
  36.     只有正的pid才会被执行
  37. signo  -需发送的信号数量。如果signo为0，就是没有信号发送，但是所有错误检查会被执行
  38. 返回值：
  39. 0，  成功；-1，发生错误，并且会返回错误码：
  40. EINVAL   无效的信号指定。
  41. EPERM   进程没有权限将信号发送到任何目标进程。
  42. ESRCH    PID或进程组不存在。
  43. ENOSYS  不支持发送的信号来处理组。
  44. int kill(pid_t pid, int signo)
  45. {
  46. #ifdef CONFIG_SCHED_HAVE_PARENT
  47.  FAR struct tcb_s *rtcb = (FAR struct tcb_s *)g_readytorun.head;
  48. #endif
  49.  siginfo_t info;
  50.  int ret;
  51.  /* We do not support sending signals to process groups */
  52.  if (pid <= 0)
  53.    {
  54.       ret = -ENOSYS;
  55.       goto errout;
  56.    }
  57.  /* Make sure that the signal is valid */
  58.  if (!GOOD_SIGNO(signo))
  59.    {
  60.       ret = -EINVAL;
  61.       goto errout;
  62. }
  63. //至此都是保证pid大于0
  64.  /* Keep things stationary through the following */
  65.  sched_lock();
  66.  /* Create the siginfo structure */创建siginfo结构体
  67.  info.si_signo           = signo;
  68.  info.si_code            = SI_USER;
  69.  info.si_value.sival_ptr = NULL;
  70. #ifdef CONFIG_SCHED_HAVE_PARENT
  71.  info.si_pid             =rtcb->pid;
  72.  info.si_status          = OK;
  73. #endif
  74.  /* Send the signal */
  75.  ret = sig_dispatch(pid, &info);
  76.  sched_unlock();
  77.  if (ret < 0)
  78.    {
  79.       goto errout;
  80.    }
  81.  return OK;
  82. errout:
  83.  set_errno(-ret);
  84.  return ERROR;
  85. }

sched_lock();/* Keep thingsstationary through the following */通过这个让事情变得平稳，

sched_unlock();//也就是一个保护作用，里面的内容不让其他程序触碰

重点就是info结构体赋值，赋值的内容就是之前的任务的相关信息，然后就是ret =sig_dispatch(pid,&info);把任务发出去（这一层一层包的- -!）

跳转至sig_dispatch()

  1. /****************************************************************************
  2.  * Name: sig_dispatch
  3.  *
  4.  * Description:
  5.  *   Thisis the front-end for sig_tcbdispatch that should be typically
  6.  *   beused to dispatch a signal.  If HAVE_GROUP_MEMBERS is defined,
  7.  *   thenfunction will follow the group signal delivery algorthrims:
  8.  *
  9.  *   Thisfront-end does the following things before calling
  10.  *  sig_tcbdispatch.
  11.  *
  12.  *    With HAVE_GROUP_MEMBERS defined:
  13.  *     -Get the TCB associated with the pid.
  14.  *     -If the TCB was found, get the group from the TCB.
  15.  *     - If the PID has already exited, lookup thegroup that that was
  16.  *      started by this task.
  17.  *     -Use the group to pick the TCB to receive the signal
  18.  *     -Call sig_tcbdispatch with the TCB
  19.  *
  20.  *    With HAVE_GROUP_MEMBERS *not* defined
  21.  *     -Get the TCB associated with the pid.
  22.  *     -Call sig_tcbdispatch with the TCB
  23.  *
  24.  * Returned Value:
  25.  *  Returns 0 (OK) on success or a negated errno value on failure.
  26.  *
  27.  ****************************************************************************/
  28. 注释翻译：(TCB是线程控制块的意思)
  29. 这个函数是用作派遣任务
  30. 如果HAVE_GROUP_MEMBERS被定义了，那么要完成
  31. 获取与PID相关的TCB
  32. 如果TCB被发现，请从TCB里获取组
  33. 如果PID已经退出，查找被此任务开始的组
  34. 使用组pick TCB来接收信号
  35. 用TCB来调用sig_tcbdispatch
  36. 如果HAVE_GROUP_MEMBERS没有被定义，那么
  37. 获取与PID相关的TCB
  38.        用TCB来调用sig_tcbdispatch
  39. int sig_dispatch(pid_t pid, FARsiginfo_t *info)
  40. {
  41. #ifdef HAVE_GROUP_MEMBERS
  42.  FAR struct tcb_s *stcb;
  43.  FAR struct task_group_s *group;
  44.  /* Get the TCB associated with the pid */
  45.  stcb = sched_gettcb(pid);//获取一个任务ID，这个函数会返回一个指针指向相应的TCB
  46.  if (stcb)
  47.    {
  48.       /* The task/thread associated with thisPID is still active.  Get its
  49.        * task group.
  50.        */
  51. //与PID相关的任务/线程还是激活的，得到它的任务组
  52.       group = stcb->group;
  53.    }
  54.  else
  55.    {
  56.       /* The task/thread associated with thisPID has exited.  In the normal
  57.        * usage model, the PID should correspondto the PID of the task that
  58.        * created the task group.  Try looking it up.
  59.        */
  60.       group = group_findbypid(pid);
  61.    }
  62.   /* Didwe locate the group? */
  63.  if (group)
  64.    {
  65.       /* Yes.. call group_signal() to send thesignal to the correct group
  66.        * member.
  67.        */
  68.       return group_signal(group, info);
  69.    }
  70.  else
  71.    {
  72.       return -ESRCH;
  73.    }
  74. #else
  75.  FAR struct tcb_s *stcb;
  76.  /* Get the TCB associated with the pid */
  77.  stcb = sched_gettcb(pid);
  78.  if (!stcb)
  79.    {
  80.      return -ESRCH;
  81.    }
  82.  return sig_tcbdispatch(stcb, info);
  83. #endif
  84. }

ret = sig_dispatch(pid,&info);

快到内核了，东西越来越多，一点点看吧

stcb = sched_gettcb(pid); /* Get the TCB associated with the pid */获取pid相关的TCB

跳转至sched_gettcb()

  1. /****************************************************************************
  2.  * Name: sched_gettcb
  3.  *
  4.  * Description:
  5.  *  Given a task ID, this function will return
  6.  *   thea pointer to the corresponding TCB (or NULL if there
  7.  *   isno such task ID).
  8.  *
  9.  ****************************************************************************/
  10. 注释翻译：获取一个任务ID，这个函数会返回一个指针指向相应的TCB
  11. FAR struct tcb_s*sched_gettcb(pid_t pid)
  12. {
  13.  FAR struct tcb_s *ret = NULL;
  14.  int hash_ndx;
  15.  /* Verify that the PID is within range */
  16.  if (pid >= 0 )
  17.    {
  18.       /* Get the hash_ndx associated with thepid */
  19.       hash_ndx = PIDHASH(pid);
  20.       /* Verify that the correct TCB was found.*/
  21.       if (pid == g_pidhash[hash_ndx].pid)
  22.         {
  23.           /* Return the TCB associated withthis pid (if any) */
  24.           ret = g_pidhash[hash_ndx].tcb;
  25.         }
  26.    }
  27.  /* Return the TCB. */
  28.  return ret;
  29. }
  30. 现在应该到了任务分配层了，应该也算内核了吧，下一层就是物理驱动层
  31. 细细看来，跳转至hash_ndx =PIDHASH(pid);
  32.  
  33. group = stcb->group;看看这个group是什么东西
  34. #ifdef HAVE_TASK_GROUP
  35.  FAR struct task_group_s *group;       /* Pointer to shared task group data  */
  36. #endif
  37. struct task_group_s
  38. {
  39. #ifdef HAVE_GROUP_MEMBERS
  40.  struct task_group_s *flink;      /* Supports a singly linked list            */
  41.  gid_t tg_gid;                    /* The ID of this task group                */
  42.  gid_t tg_pgid;                   /* The ID of the parent task group          */
  43. #endif
  44. #if!defined(CONFIG_DISABLE_PTHREAD) && defined(CONFIG_SCHED_HAVE_PARENT)
  45.  pid_t tg_task;                   /* The ID of the task within the group      */
  46. #endif
  47.  uint8_t tg_flags;                /* See GROUP_FLAG_* definitions             */
  48.  /* Group membership ***********************************************************/
  49.  uint8_t    tg_nmembers;           /* Number of members in thegroup           */
  50. #ifdef HAVE_GROUP_MEMBERS
  51.  uint8_t    tg_mxmembers;          /* Number of members inallocation          */
  52.  FAR pid_t *tg_members;           /* Members of the group                     */
  53. #endif
  54.  /* atexit/on_exit support ****************************************************/
  55. #if defined(CONFIG_SCHED_ATEXIT)&& !defined(CONFIG_SCHED_ONEXIT)
  56. # ifdefined(CONFIG_SCHED_ATEXIT_MAX) && CONFIG_SCHED_ATEXIT_MAX > 1
  57.  atexitfunc_t tg_atexitfunc[CONFIG_SCHED_ATEXIT_MAX];
  58. # else
  59.  atexitfunc_t tg_atexitfunc;      /* Called when exit is called.             */
  60. # endif
  61. #endif
  62. #ifdef CONFIG_SCHED_ONEXIT
  63. # ifdefined(CONFIG_SCHED_ONEXIT_MAX) && CONFIG_SCHED_ONEXIT_MAX > 1
  64.  onexitfunc_t tg_onexitfunc[CONFIG_SCHED_ONEXIT_MAX];
  65.  FAR void *tg_onexitarg[CONFIG_SCHED_ONEXIT_MAX];
  66. # else
  67.  onexitfunc_t tg_onexitfunc;      /* Called when exit is called.             */
  68.  FAR void *tg_onexitarg;          /* The argument passed to the function     */
  69. # endif
  70. #endif
  71.  /* Child exit status **********************************************************/
  72. #ifdefined(CONFIG_SCHED_HAVE_PARENT) && defined(CONFIG_SCHED_CHILD_STATUS)
  73.  FAR struct child_status_s *tg_children; /* Head of a list of childstatus     */
  74. #endif
  75.  /* waitpid support************************************************************/
  76.   /*Simple mechanism used only when there is no support for SIGCHLD            */
  77. #if defined(CONFIG_SCHED_WAITPID)&& !defined(CONFIG_SCHED_HAVE_PARENT)
  78.  sem_t tg_exitsem;                /* Support for waitpid                      */
  79.  int *tg_statloc;                  /* Location to return exitstatus           */
  80. #endif
  81.  /* Pthreads *******************************************************************/
  82. #ifndef CONFIG_DISABLE_PTHREAD
  83.                                     /* Pthreadjoin Info:                       */
  84.  sem_t tg_joinsem;                /*   Mutually exclusive access tojoin data */
  85.  FAR struct join_s *tg_joinhead;  /*   Head of a list of joindata            */
  86.  FAR struct join_s *tg_jointail;  /*   Tail of a list of joindata            */
  87.  uint8_t tg_nkeys;                /* Number pthread keys allocated            */
  88. #endif
  89.  /* POSIX Signal Control Fields ************************************************/
  90. #ifndef CONFIG_DISABLE_SIGNALS
  91.  sq_queue_t sigpendingq;          /* List of pending signals                  */
  92. #endif
  93.  /* Environment variables ******************************************************/
  94. #ifndef CONFIG_DISABLE_ENVIRON
  95.  size_t     tg_envsize;            /* Size of environment stringallocation    */
  96.  FAR char  *tg_envp;               /* Allocated environmentstrings            */
  97. #endif
  98.  /* PIC data space and address environments************************************/
  99.  /* Logically the PIC data space belongs here (see struct dspace_s).  The
  100.   * current logic needs review: There are differences in the away that the
  101.   * life of the PIC data is managed.
  102.   */
  103.  /* File descriptors ***********************************************************/
  104. #if CONFIG_NFILE_DESCRIPTORS > 0
  105.  struct filelist tg_filelist;     /* Maps file descriptor to file             */
  106. #endif
  107.  /* FILE streams***************************************************************/
  108.  /* In a flat, single-heap build. The stream list is allocated with this
  109.   * structure.  But kernel mode witha kernel allocator, it must be separately
  110.   * allocated using a user-space allocator.
  111.   */
  112. #if CONFIG_NFILE_STREAMS > 0
  113. #if defined(CONFIG_NUTTX_KERNEL)&& defined(CONFIG_MM_KERNEL_HEAP)
  114.  FAR struct streamlist *tg_streamlist;
  115. #else
  116.  struct streamlist tg_streamlist; /* Holds C buffered I/O info                */
  117. #endif
  118. #endif
  119.  /* Sockets ********************************************************************/
  120. #if CONFIG_NSOCKET_DESCRIPTORS >0
  121.  struct socketlist tg_socketlist; /* Maps socket descriptor to socket        */
  122. #endif
  123.  /* POSIX Named Message Queue Fields *******************************************/
  124. #ifndef CONFIG_DISABLE_MQUEUE
  125.  sq_queue_t tg_msgdesq;           /* List of opened message queues           */
  126. #endif
  127. };
  128. 好吧，还以为是个什么结构体呢，好长一段啊！！

这些都是不需要改动的地方，太多太杂了，先放着，重点看光流读取并且如何用于控制的部分，这些部分需要根据实际应用改动。

那么第二条思路开始，去main里面看看

(应该是这样前面看到属于ardupilot/libraries/AP_OpticalFlow文件夹里面的，属于APM的那套，应该有个虚拟层把硬件层抽象了，最后根据具体硬件来选择对应的硬件驱动；接下来要看的属于ardupilot/modules/PX4Firmware/src/drivers/px4flow文件夹里面的，属于PX4原生码)

extern "C" __EXPORT int px4flow_main(int argc, char *argv[]);

  1. int
  2. px4flow_main(int argc, char *argv[])
  3. {
  4. 	/*
  5. 	 * Start/load the driver.
  6. 	 */
  7. 	if (!strcmp(argv[1], "start")) {
  8. 		return px4flow::start();
  9. 	}
  10. 	/*
  11. 	 * Stop the driver
  12. 	 */
  13. 	if (!strcmp(argv[1], "stop")) {
  14. 		px4flow::stop();
  15. 	}
  16. 	/*
  17. 	 * Test the driver/device.
  18. 	 */
  19. 	if (!strcmp(argv[1], "test")) {
  20. 		px4flow::test();
  21. 	}
  22. 	/*
  23. 	 * Reset the driver.
  24. 	 */
  25. 	if (!strcmp(argv[1], "reset")) {
  26. 		px4flow::reset();
  27. 	}
  28. 	/*
  29. 	 * Print driver information.
  30. 	 */
  31. 	if (!strcmp(argv[1], "info") || !strcmp(argv[1], "status")) {
  32. 		px4flow::info();
  33. 	}
  34. 	errx(1, "unrecognized command, try 'start', 'test', 'reset' or 'info'");
  35. }
跳转至start() 

  1. /**
  2.  * Start the driver.
  3.  */
  4. int
  5. start()
  6. {
  7. 	int fd;
  8.  
  9. 	/* entry check: */
  10. 	if (start_in_progress) {
  11. 		warnx("start already in progress");
  12. 		return 1;
  13. 	}
  14.  
  15. 	start_in_progress = true;
  16.  
  17. 	if (g_dev != nullptr) {
  18. 		start_in_progress = false;
  19. 		warnx("already started");
  20. 		return 1;
  21. 	}
  22.  
  23. 	warnx("scanning I2C buses for device..");
  24.  
  25. 	int retry_nr = 0;
  26.  
  27. 	while (1) {
  28. 		const int busses_to_try[] = {
  29. 			PX4_I2C_BUS_EXPANSION,
  30. #ifdef PX4_I2C_BUS_ESC
  31. 			PX4_I2C_BUS_ESC,
  32. #endif
  33. #ifdef PX4_I2C_BUS_ONBOARD
  34. 			PX4_I2C_BUS_ONBOARD,
  35. #endif
  36. 			-1
  37. 		};
  38.  
  39. 		const int *cur_bus = busses_to_try;
  40.  
  41. 		while (*cur_bus != -1) {
  42. 			/* create the driver */
  43. 			/* warnx("trying bus %d", *cur_bus); */
  44. 			g_dev = new PX4FLOW(*cur_bus);
  45.  
  46. 			if (g_dev == nullptr) {
  47. 				/* this is a fatal error */
  48. 				break;
  49. 			}
  50.  
  51. 			/* init the driver: */
  52. 			if (OK == g_dev->init()) {
  53. 				/* success! */
  54. 				break;
  55. 			}
  56.  
  57. 			/* destroy it again because it failed. */
  58. 			delete g_dev;
  59. 			g_dev = nullptr;
  60.  
  61. 			/* try next! */
  62. 			cur_bus++;
  63. 		}
  64.  
  65. 		/* check whether we found it: */
  66. 		if (*cur_bus != -1) {
  67.  
  68. 			/* check for failure: */
  69. 			if (g_dev == nullptr) {
  70. 				break;
  71. 			}
  72.  
  73. 			/* set the poll rate to default, starts automatic data collection */
  74. 			fd = open(PX4FLOW0_DEVICE_PATH, O_RDONLY);
  75.  
  76. 			if (fd < 0) {
  77. 				break;
  78. 			}
  79.  
  80. 			if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_MAX) < 0) {
  81. 				break;
  82. 			}
  83.  
  84. 			/* success! */
  85. 			start_in_progress = false;
  86. 			return 0;
  87. 		}
  88.  
  89. 		if (retry_nr < START_RETRY_COUNT) {
  90. 			/* lets not be too verbose */
  91. 			// warnx("PX4FLOW not found on I2C busses. Retrying in %d ms. Giving up in %d retries.", START_RETRY_TIMEOUT, START_RETRY_COUNT - retry_nr);
  92. 			usleep(START_RETRY_TIMEOUT * 1000);
  93. 			retry_nr++;
  94.  
  95. 		} else {
  96. 			break;
  97. 		}
  98. 	}
  99.  
  100. 	if (g_dev != nullptr) {
  101. 		delete g_dev;
  102. 		g_dev = nullptr;
  103. 	}
  104.  
  105. 	start_in_progress = false;
  106. 	return 1;
  107. }

之后又会深入到底层，暂时先放着

读数据在test()里面

  1. /**
  2.  * Perform some basic functional tests on the driver;
  3.  * make sure we can collect data from the sensor in polled
  4.  * and automatic modes.
  5.  */
  6. void
  7. test()
  8. {
  9. 	struct optical_flow_s report;
  10. 	ssize_t sz;
  11. 	int ret;
  12.  
  13. 	int fd = open(PX4FLOW0_DEVICE_PATH, O_RDONLY);
  14.  
  15. 	if (fd < 0) {
  16. 		err(1, "%s open failed (try 'px4flow start' if the driver is not running", PX4FLOW0_DEVICE_PATH);
  17. 	}
  18.  
  19.  
  20. 	/* do a simple demand read */
  21. 	sz = read(fd, &report, sizeof(report));
  22.  
  23. 	if (sz != sizeof(report)) {
  24. 		warnx("immediate read failed");
  25. 	}
  26.  
  27. 	warnx("single read");
  28. 	warnx("pixel_flow_x_integral: %i", f_integral.pixel_flow_x_integral);
  29. 	warnx("pixel_flow_y_integral: %i", f_integral.pixel_flow_y_integral);
  30. 	warnx("framecount_integral: %u",
  31. 	      f_integral.frame_count_since_last_readout);
  32.  
  33. 	/* start the sensor polling at 10Hz */
  34. 	if (OK != ioctl(fd, SENSORIOCSPOLLRATE, 10)) {
  35. 		errx(1, "failed to set 10Hz poll rate");
  36. 	}
  37.  
  38. 	/* read the sensor 5x and report each value */
  39. 	for (unsigned i = 0; i < 10; i++) {
  40. 		struct pollfd fds;
  41.  
  42. 		/* wait for data to be ready */
  43. 		fds.fd = fd;
  44. 		fds.events = POLLIN;
  45. 		ret = poll(&fds, 1, 2000);
  46.  
  47. 		if (ret != 1) {
  48. 			errx(1, "timed out waiting for sensor data");
  49. 		}
  50.  
  51. 		/* now go get it */
  52. 		sz = read(fd, &report, sizeof(report));
  53.  
  54. 		if (sz != sizeof(report)) {
  55. 			err(1, "periodic read failed");
  56. 		}
  57.  
  58. 		warnx("periodic read %u", i);
  59.  
  60. 		warnx("framecount_total: %u", f.frame_count);
  61. 		warnx("framecount_integral: %u",
  62. 		      f_integral.frame_count_since_last_readout);
  63. 		warnx("pixel_flow_x_integral: %i", f_integral.pixel_flow_x_integral);
  64. 		warnx("pixel_flow_y_integral: %i", f_integral.pixel_flow_y_integral);
  65. 		warnx("gyro_x_rate_integral: %i", f_integral.gyro_x_rate_integral);
  66. 		warnx("gyro_y_rate_integral: %i", f_integral.gyro_y_rate_integral);
  67. 		warnx("gyro_z_rate_integral: %i", f_integral.gyro_z_rate_integral);
  68. 		warnx("integration_timespan [us]: %u", f_integral.integration_timespan);
  69. 		warnx("ground_distance: %0.2f m",
  70. 		      (double) f_integral.ground_distance / 1000);
  71. 		warnx("time since last sonar update [us]: %i",
  72. 		      f_integral.sonar_timestamp);
  73. 		warnx("quality integration average : %i", f_integral.qual);
  74. 		warnx("quality : %i", f.qual);
  75.  
  76.  
  77. 	}
  78.  
  79. 	errx(0, "PASS");
  80. }
看注释能明白sz = read(fd, &report, sizeof(report));是读的操作

ardupilot/modules/PX4Nuttx/nuttx/fs/fs_read.c这个路径应该是系统的路径，已经进了系统吗？系统是什么概念啊？

  1. ssize_t read(int fd, FAR void *buf, size_t nbytes)
  2. {
  3.   /* Did we get a valid file descriptor? */
  4.  
  5. #if CONFIG_NFILE_DESCRIPTORS > 0
  6.   if ((unsigned int)fd >= CONFIG_NFILE_DESCRIPTORS)
  7. #endif
  8.     {
  9.       /* No.. If networking is enabled, read() is the same as recv() with
  10.        * the flags parameter set to zero.
  11.        */
  12.  
  13. #if defined(CONFIG_NET) && CONFIG_NSOCKET_DESCRIPTORS > 0
  14.       return recv(fd, buf, nbytes, 0);
  15. #else
  16.       /* No networking... it is a bad descriptor in any event */
  17.  
  18.       set_errno(EBADF);
  19.       return ERROR;
  20. #endif
  21.     }
  22.  
  23.   /* The descriptor is in a valid range to file descriptor... do the read */
  24.  
  25. #if CONFIG_NFILE_DESCRIPTORS > 0
  26.   return file_read(fd, buf, nbytes);
  27. #endif
  28. }
看注释能明白return file_read(fd, buf, nbytes);是读操作 

  1. /****************************************************************************
  2.  * Private Functions
  3.  ****************************************************************************/
  4.  
  5. #if CONFIG_NFILE_DESCRIPTORS > 0
  6. static inline ssize_t file_read(int fd, FAR void *buf, size_t nbytes)
  7. {
  8.   FAR struct filelist *list;
  9.   int ret = -EBADF;
  10.  
  11.   /* Get the thread-specific file list */
  12.  
  13.   list = sched_getfiles();
  14.   if (!list)
  15.     {
  16.       /* Failed to get the file list */
  17.  
  18.       ret = -EMFILE;
  19.     }
  20.  
  21.   /* Were we given a valid file descriptor? */
  22.  
  23.   else if ((unsigned int)fd < CONFIG_NFILE_DESCRIPTORS)
  24.     {
  25.       FAR struct file *this_file = &list->fl_files[fd];
  26.       FAR struct inode *inode    = this_file->f_inode;
  27.  
  28.       /* Yes.. Was this file opened for read access? */
  29.  
  30.       if ((this_file->f_oflags & O_RDOK) == 0)
  31.         {
  32.           /* No.. File is not read-able */
  33.  
  34.           ret = -EACCES;
  35.         }
  36.  
  37.       /* Is a driver or mountpoint registered? If so, does it support
  38.        * the read method?
  39.        */
  40.  
  41.       else if (inode && inode->u.i_ops && inode->u.i_ops->read)
  42.         {
  43.           /* Yes.. then let it perform the read.  NOTE that for the case
  44.            * of the mountpoint, we depend on the read methods bing
  45.            * identical in signature and position in the operations vtable.
  46.            */
  47.  
  48.           ret = (int)inode->u.i_ops->read(this_file, (char*)buf, (size_t)nbytes);
  49.         }
  50.     }
  51.  
  52.   /* If an error occurred, set errno and return -1 (ERROR) */
  53.  
  54.   if (ret < 0)
  55.     {
  56.       set_errno(-ret);
  57.       return ERROR;
  58.     }
  59.  
  60.   /* Otherwise, return the number of bytes read */
  61.  
  62.   return ret;
  63. }
  64. #endif
跟踪buf可知ret = (int)inode->u.i_ops->read(this_file, (char*)buf, (size_t)nbytes);是读操作

于是进入了“文件操作”

  1. struct file_operations
  2. {
  3.   /* The device driver open method differs from the mountpoint open method */
  4.  
  5.   int     (*open)(FAR struct file *filp);
  6.  
  7.   /* The following methods must be identical in signature and position because
  8.    * the struct file_operations and struct mountp_operations are treated like
  9.    * unions.
  10.    */
  11.  
  12.   int     (*close)(FAR struct file *filp);
  13.   ssize_t (*read)(FAR struct file *filp, FAR char *buffer, size_t buflen);
  14.   ssize_t (*write)(FAR struct file *filp, FAR const char *buffer, size_t buflen);
  15.   off_t   (*seek)(FAR struct file *filp, off_t offset, int whence);
  16.   int     (*ioctl)(FAR struct file *filp, int cmd, unsigned long arg);
  17. #ifndef CONFIG_DISABLE_POLL
  18.   int     (*poll)(FAR struct file *filp, struct pollfd *fds, bool setup);
  19. #endif
  20.  
  21.   /* The two structures need not be common after this point */
  22. };
怎么继续找下去呢？C++好麻烦啊

看注释This structure is provided by devices when they are registered with the system.  It is used to call back to perform device specific operations.
这种结构由设备当它们与系统中注册提供。 它是用来回调来执行设备的特定操作。
就是说首先要注册，再根据具体的设备(控制芯片)选用具体的操作。

ardupilot/modules/PX4Firmware/src/drivers/px4flow.cpp中找到了相应的read函数

  1. ssize_t
  2. PX4FLOW::read(struct file *filp, char *buffer, size_t buflen)
  3. {
  4. 	unsigned count = buflen / sizeof(struct optical_flow_s);
  5. 	struct optical_flow_s *rbuf = reinterpret_cast<struct optical_flow_s *>(buffer);
  6. 	int ret = 0;
  7.  
  8. 	/* buffer must be large enough */
  9. 	if (count < 1) {
  10. 		return -ENOSPC;
  11. 	}
  12.  
  13. 	/* if automatic measurement is enabled */
  14. 	if (_measure_ticks > 0) {
  15.  
  16. 		/*
  17. 		 * While there is space in the caller's buffer, and reports, copy them.
  18. 		 * Note that we may be pre-empted by the workq thread while we are doing this;
  19. 		 * we are careful to avoid racing with them.
  20. 		 */
  21. 		while (count--) {
  22. 			if (_reports->get(rbuf)) {
  23. 				ret += sizeof(*rbuf);
  24. 				rbuf++;
  25. 			}
  26. 		}
  27.  
  28. 		/* if there was no data, warn the caller */
  29. 		return ret ? ret : -EAGAIN;
  30. 	}
  31.  
  32. 	/* manual measurement - run one conversion */
  33. 	do {
  34. 		_reports->flush();
  35.  
  36. 		/* trigger a measurement */
  37. 		if (OK != measure()) {
  38. 			ret = -EIO;
  39. 			break;
  40. 		}
  41.  
  42. 		/* run the collection phase */
  43. 		if (OK != collect()) {
  44. 			ret = -EIO;
  45. 			break;
  46. 		}
  47.  
  48. 		/* state machine will have generated a report, copy it out */
  49. 		if (_reports->get(rbuf)) {
  50. 			ret = sizeof(*rbuf);
  51. 		}
  52.  
  53. 	} while (0);
  54.  
  55. 	return ret;
  56. }
get(rbuf)  (read函数中) 

  1. bool
  2. RingBuffer::get(void *val, size_t val_size)
  3. {
  4. 	if (_tail != _head) {
  5. 		unsigned candidate;
  6. 		unsigned next;
  7.  
  8. 		if ((val_size == 0) || (val_size > _item_size)) {
  9. 			val_size = _item_size;
  10. 		}
  11.  
  12. 		do {
  13. 			/* decide which element we think we're going to read */
  14. 			candidate = _tail;
  15.  
  16. 			/* and what the corresponding next index will be */
  17. 			next = _next(candidate);
  18.  
  19. 			/* go ahead and read from this index */
  20. 			if (val != NULL) {
  21. 				memcpy(val, &_buf[candidate * _item_size], val_size);
  22. 			}
  23.  
  24. 			/* if the tail pointer didn't change, we got our item */
  25. 		} while (!__PX4_SBCAP(&_tail, candidate, next));
  26.  
  27. 		return true;
  28.  
  29. 	} else {
  30. 		return false;
  31. 	}
  32. }
字面意思的从缓冲环中得到数据，并存到rbuf中
继续看 measure()和 collect()(read函数中) 
  1. int
  2. PX4FLOW::measure()
  3. {
  4. 	int ret;
  5.  
  6. 	/*
  7. 	 * Send the command to begin a measurement.
  8. 	 */
  9. 	uint8_t cmd = PX4FLOW_REG;
  10. 	ret = transfer(&cmd, 1, nullptr, 0);
  11.  
  12. 	if (OK != ret) {
  13. 		perf_count(_comms_errors);
  14. 		DEVICE_DEBUG("i2c::transfer returned %d", ret);
  15. 		return ret;
  16. 	}
  17.  
  18. 	ret = OK;
  19.  
  20. 	return ret;
  21. }
看到 PX4FLOW_REG 
  1. /* PX4FLOW Registers addresses */
  2. #define PX4FLOW_REG			0x16	///< Measure Register 22

终于和硬件对应起来了

根据DEVICE_DEBUG("i2c::transfer returned %d", ret);可以得知使用的transfer为i2c::transfer 

Ctrl+H全局搜索i2c::transfer 

i2c函数很多，综合输入参数和DEVICE_DEBUG("i2c::transfer returned %d", ret);信息，可知上图中的i2c::transfer就是我们要找的函数

  1. int
  2. I2C::transfer(const uint8_t *send, unsigned send_len, uint8_t *recv, unsigned recv_len)
  3. {
  4. 	struct i2c_msg_s msgv[2];
  5. 	unsigned msgs;
  6. 	int ret;
  7. 	unsigned retry_count = 0;
  8.  
  9. 	do {
  10. 		//	DEVICE_DEBUG("transfer out %p/%u  in %p/%u", send, send_len, recv, recv_len);
  11.  
  12. 		msgs = 0;
  13.  
  14. 		if (send_len > 0) {
  15. 			msgv[msgs].addr = _address;
  16. 			msgv[msgs].flags = 0;
  17. 			msgv[msgs].buffer = const_cast<uint8_t *>(send);
  18. 			msgv[msgs].length = send_len;
  19. 			msgs++;
  20. 		}
  21.  
  22. 		if (recv_len > 0) {
  23. 			msgv[msgs].addr = _address;
  24. 			msgv[msgs].flags = I2C_M_READ;
  25. 			msgv[msgs].buffer = recv;
  26. 			msgv[msgs].length = recv_len;
  27. 			msgs++;
  28. 		}
  29.  
  30. 		if (msgs == 0) {
  31. 			return -EINVAL;
  32. 		}
  33.  
  34. 		ret = I2C_TRANSFER(_dev, &msgv[0], msgs);
  35.  
  36. 		/* success */
  37. 		if (ret == OK) {
  38. 			break;
  39. 		}
  40.  
  41. 		/* if we have already retried once, or we are going to give up, then reset the bus */
  42. 		if ((retry_count >= 1) || (retry_count >= _retries)) {
  43. 			up_i2creset(_dev);
  44. 		}
  45.  
  46. 	} while (retry_count++ < _retries);
  47.  
  48. 	return ret;
  49.  
  50. }

struct i2c_msg_s msgv[2];

  1. struct i2c_msg_s
  2. {
  3.   uint16_t  addr;                  /* Slave address */
  4.   uint16_t  flags;                 /* See I2C_M_* definitions */
  5.   uint8_t  *buffer;
  6.   int       length;
  7. };
ret = I2C_TRANSFER(_dev, &msgv[0], msgs);找I2C_TRANSFER函数

#define I2C_TRANSFER(d,m,c) ((d)->ops->transfer(d,m,c))，找transfer函数

已无能为力了，找不下去了。好吧到此断了，找不到硬件层了，数据还没读回来

继续看collect()

  1. int
  2. PX4FLOW::collect()
  3. {
  4. 	int ret = -EIO;
  5.  
  6. 	/* read from the sensor */
  7. 	uint8_t val[I2C_FRAME_SIZE + I2C_INTEGRAL_FRAME_SIZE] = { 0 };
  8.  
  9. 	perf_begin(_sample_perf);
  10.  
  11. 	if (PX4FLOW_REG == 0x00) {
  12. 		ret = transfer(nullptr, 0, &val[0], I2C_FRAME_SIZE + I2C_INTEGRAL_FRAME_SIZE);
  13. 	}
  14.  
  15. 	if (PX4FLOW_REG == 0x16) {
  16. 		ret = transfer(nullptr, 0, &val[0], I2C_INTEGRAL_FRAME_SIZE);
  17. 	}
  18.  
  19. 	if (ret < 0) {
  20. 		DEVICE_DEBUG("error reading from sensor: %d", ret);
  21. 		perf_count(_comms_errors);
  22. 		perf_end(_sample_perf);
  23. 		return ret;
  24. 	}
  25.  
  26. 	if (PX4FLOW_REG == 0) {
  27. 		memcpy(&f, val, I2C_FRAME_SIZE);
  28. 		memcpy(&f_integral, &(val[I2C_FRAME_SIZE]), I2C_INTEGRAL_FRAME_SIZE);
  29. 	}
  30.  
  31. 	if (PX4FLOW_REG == 0x16) {
  32. 		memcpy(&f_integral, val, I2C_INTEGRAL_FRAME_SIZE);
  33. 	}
  34.  
  35.  
  36. 	struct optical_flow_s report;
  37.  
  38. 	report.timestamp = hrt_absolute_time();
  39.  
  40. 	report.pixel_flow_x_integral = static_cast<float>(f_integral.pixel_flow_x_integral) / 10000.0f;//convert to radians
  41.  
  42. 	report.pixel_flow_y_integral = static_cast<float>(f_integral.pixel_flow_y_integral) / 10000.0f;//convert to radians
  43.  
  44. 	report.frame_count_since_last_readout = f_integral.frame_count_since_last_readout;
  45.  
  46. 	report.ground_distance_m = static_cast<float>(f_integral.ground_distance) / 1000.0f;//convert to meters
  47.  
  48. 	report.quality = f_integral.qual; //0:bad ; 255 max quality
  49.  
  50. 	report.gyro_x_rate_integral = static_cast<float>(f_integral.gyro_x_rate_integral) / 10000.0f; //convert to radians
  51.  
  52. 	report.gyro_y_rate_integral = static_cast<float>(f_integral.gyro_y_rate_integral) / 10000.0f; //convert to radians
  53.  
  54. 	report.gyro_z_rate_integral = static_cast<float>(f_integral.gyro_z_rate_integral) / 10000.0f; //convert to radians
  55.  
  56. 	report.integration_timespan = f_integral.integration_timespan; //microseconds
  57.  
  58. 	report.time_since_last_sonar_update = f_integral.sonar_timestamp;//microseconds
  59.  
  60. 	report.gyro_temperature = f_integral.gyro_temperature;//Temperature * 100 in centi-degrees Celsius
  61.  
  62. 	report.sensor_id = 0;
  63.  
  64. 	/* rotate measurements according to parameter */根据参数测量旋转
  65. 	float zeroval = 0.0f;
  66.  
  67. 	rotate_3f(_sensor_rotation, report.pixel_flow_x_integral, report.pixel_flow_y_integral, zeroval);
  68.  
  69. 	rotate_3f(_sensor_rotation, report.gyro_x_rate_integral, report.gyro_y_rate_integral, report.gyro_z_rate_integral);
  70.  
  71. 	if (_px4flow_topic == nullptr) {
  72. 		_px4flow_topic = orb_advertise(ORB_ID(optical_flow), &report);
  73.  
  74. 	} else {
  75. 		/* publish it */
  76. 		orb_publish(ORB_ID(optical_flow), _px4flow_topic, &report);
  77. 	}
  78.  
  79. 	/* publish to the distance_sensor topic as well */将distance_sensor主题发布出去
  80. 	struct distance_sensor_s distance_report;
  81. 	distance_report.timestamp = report.timestamp;
  82. 	distance_report.min_distance = PX4FLOW_MIN_DISTANCE;
  83. 	distance_report.max_distance = PX4FLOW_MAX_DISTANCE;
  84. 	distance_report.current_distance = report.ground_distance_m;
  85. 	distance_report.covariance = 0.0f;
  86. 	distance_report.type = distance_sensor_s::MAV_DISTANCE_SENSOR_ULTRASOUND;
  87. 	/* TODO: the ID needs to be properly set */
  88. 	distance_report.id = 0;
  89. 	distance_report.orientation = 8;
  90.  
  91. 	orb_publish(ORB_ID(distance_sensor), _distance_sensor_topic, &distance_report);
  92.  
  93. 	/* post a report to the ring */发布到环的报告
  94. 	if (_reports->force(&report)) {
  95. 		perf_count(_buffer_overflows);
  96. 	}
  97.  
  98. 	/* notify anyone waiting for data */通知别人等待数据
  99. 	poll_notify(POLLIN);
  100.  
  101. 	ret = OK;
  102.  
  103. 	perf_end(_sample_perf);
  104. 	return ret;
  105. }

注意看

  1. 	if (PX4FLOW_REG == 0x00) {
  2. 		ret = transfer(nullptr, 0, &val[0], I2C_FRAME_SIZE + I2C_INTEGRAL_FRAME_SIZE);
  3. 	}
  4.  
  5. 	if (PX4FLOW_REG == 0x16) {
  6. 		ret = transfer(nullptr, 0, &val[0], I2C_INTEGRAL_FRAME_SIZE);
  7. 	}

这个是把数据读回

意思就是measure()是写指令操作，collect()是读传回来的数据，有一点奇怪PX4FLOW_REG什么时候会赋值为0x00
memcpy(&f, val, I2C_FRAME_SIZE);
memcpy(&f_integral, &(val[I2C_FRAME_SIZE]), I2C_INTEGRAL_FRAME_SIZE);

这2句是讲读取的数据存到f和f_integral结构体中

  1. struct i2c_frame f;
  2. struct i2c_integral_frame f_integral;
  3. typedef  struct i2c_frame {
  4. 	uint16_t frame_count;
  5. 	int16_t pixel_flow_x_sum;
  6. 	int16_t pixel_flow_y_sum;
  7. 	int16_t flow_comp_m_x;
  8. 	int16_t flow_comp_m_y;
  9. 	int16_t qual;
  10. 	int16_t gyro_x_rate;
  11. 	int16_t gyro_y_rate;
  12. 	int16_t gyro_z_rate;
  13. 	uint8_t gyro_range;
  14. 	uint8_t sonar_timestamp;
  15. 	int16_t ground_distance;
  16. } i2c_frame;
  17.  
  18. #define I2C_FRAME_SIZE (sizeof(i2c_frame))
  19.  
  20.  
  21. typedef struct i2c_integral_frame {
  22. 	uint16_t frame_count_since_last_readout;
  23. 	int16_t pixel_flow_x_integral;
  24. 	int16_t pixel_flow_y_integral;
  25. 	int16_t gyro_x_rate_integral;
  26. 	int16_t gyro_y_rate_integral;
  27. 	int16_t gyro_z_rate_integral;
  28. 	uint32_t integration_timespan;
  29. 	uint32_t sonar_timestamp;
  30. 	uint16_t ground_distance;
  31. 	int16_t gyro_temperature;
  32. 	uint8_t qual;
  33. } i2c_integral_frame;
接着optical_flow_s report用来存储处理后的数据，之后控制用的数据就是来自这里

再换个地方，看read数据之后如何用于控制的

全局搜索ORB_ID(optical_flow)，看哪个地方订阅了光流消息


有2个地方orb_copy(ORB_ID(optical_flow), optical_flow_sub, &flow);，再根据rc.mc_apps

默认采用的是

姿态估计 Attitude_estimator_q  

位置估计 position_estimator_inav  
姿态控制 mc_att_control  

位置控制 mc_pos_control  

EKF2暂时先不考虑

接下来要看position_estimator_inav _main.cpp(从常识也知道光流是用来获取空间xy方向的位置信息，所以之后肯定是用于位置估计上)

那么先宏观上分析下这个 position_estimator_inav _main.cpp 

  1. #include <px4_posix.h>
  2. #include <unistd.h>
  3. #include <stdlib.h>
  4. #include <stdio.h>
  5. #include <stdbool.h>
  6. #include <fcntl.h>
  7. #include <string.h>
  8. #include <px4_config.h>
  9. #include <math.h>
  10. #include <float.h>
  11. #include <uORB/uORB.h>
  12. #include <uORB/topics/parameter_update.h>
  13. #include <uORB/topics/actuator_controls.h>
  14. #include <uORB/topics/actuator_controls_0.h>
  15. #include <uORB/topics/actuator_controls_1.h>
  16. #include <uORB/topics/actuator_controls_2.h>
  17. #include <uORB/topics/actuator_controls_3.h>
  18. #include <uORB/topics/actuator_armed.h>
  19. #include <uORB/topics/sensor_combined.h>
  20. #include <uORB/topics/vehicle_attitude.h>
  21. #include <uORB/topics/vehicle_local_position.h>
  22. #include <uORB/topics/vehicle_global_position.h>
  23. #include <uORB/topics/vehicle_gps_position.h>
  24. #include <uORB/topics/vision_position_estimate.h>
  25. #include <uORB/topics/att_pos_mocap.h>
  26. #include <uORB/topics/optical_flow.h>
  27. #include <uORB/topics/distance_sensor.h>
  28. #include <poll.h>
  29. #include <systemlib/err.h>
  30. #include <systemlib/mavlink_log.h>
  31. #include <geo/geo.h>
  32. #include <systemlib/systemlib.h>
  33. #include <drivers/drv_hrt.h>
  34. #include <platforms/px4_defines.h>
  35.  
  36. #include <terrain_estimation/terrain_estimator.h>
  37. #include "position_estimator_inav_params.h"
  38. #include "inertial_filter.h"
添加了这么多头文件，大概知道会用于控制、传感器融合、位置定位、gps也用上了、光流、惯性导航滤波等 

extern "C" __EXPORT int position_estimator_inav_main(int argc, char *argv[]);
开始工作了，进入 main

这里顺便猜想下，每个这样的功能函数或者文件，都有extern "C" __EXPORT int position_estimator_inav_main(int argc, char *argv[]);，是不是在哪个地方直接调用了，所以启动了该项功能，是不是在rcS中呢？估计有个总的流程，分别调用启动这些功能“函数”

  1. /****************************************************************************
  2.  * main
  3.  ****************************************************************************/
  4. int position_estimator_inav_thread_main(int argc, char *argv[])
  5. {
  6. /**************************类似于初始化或者声明******************************/
  7. 	orb_advert_t mavlink_log_pub = nullptr;
  8.  
  9. 	float x_est[2] = { 0.0f, 0.0f };	// pos, vel
  10. 	float y_est[2] = { 0.0f, 0.0f };	// pos, vel
  11. 	float z_est[2] = { 0.0f, 0.0f };	// pos, vel
  12.  
  13. 	float est_buf[EST_BUF_SIZE][3][2];	// estimated position buffer
  14. 	float R_buf[EST_BUF_SIZE][3][3];	// rotation matrix buffer
  15. 	float R_gps[3][3];					// rotation matrix for GPS correction moment
  16. 	memset(est_buf, 0, sizeof(est_buf));
  17. 	memset(R_buf, 0, sizeof(R_buf));
  18. 	memset(R_gps, 0, sizeof(R_gps));
  19. 	int buf_ptr = 0;
  20.  
  21. 	static const float min_eph_epv = 2.0f;	// min EPH/EPV, used for weight calculation
  22. 	static const float max_eph_epv = 20.0f;	// max EPH/EPV acceptable for estimation
  23.  
  24. 	float eph = max_eph_epv;
  25. 	float epv = 1.0f;
  26.  
  27. 	float eph_flow = 1.0f;
  28.  
  29. 	float eph_vision = 0.2f;
  30. 	float epv_vision = 0.2f;
  31.  
  32. 	float eph_mocap = 0.05f;
  33. 	float epv_mocap = 0.05f;
  34.  
  35. 	float x_est_prev[2], y_est_prev[2], z_est_prev[2];
  36. 	memset(x_est_prev, 0, sizeof(x_est_prev));
  37. 	memset(y_est_prev, 0, sizeof(y_est_prev));
  38. 	memset(z_est_prev, 0, sizeof(z_est_prev));
  39.  
  40. 	int baro_init_cnt = 0;
  41. 	int baro_init_num = 200;
  42. 	float baro_offset = 0.0f;		// baro offset for reference altitude, initialized on start, then adjusted
  43.  
  44. 	hrt_abstime accel_timestamp = 0;
  45. 	hrt_abstime baro_timestamp = 0;
  46.  
  47. 	bool ref_inited = false;
  48. 	hrt_abstime ref_init_start = 0;
  49. 	const hrt_abstime ref_init_delay = 1000000;	// wait for 1s after 3D fix
  50. 	struct map_projection_reference_s ref;
  51. 	memset(&ref, 0, sizeof(ref));
  52.  
  53. 	uint16_t accel_updates = 0;
  54. 	uint16_t baro_updates = 0;
  55. 	uint16_t gps_updates = 0;
  56. 	uint16_t attitude_updates = 0;
  57. 	uint16_t flow_updates = 0;
  58. 	uint16_t vision_updates = 0;
  59. 	uint16_t mocap_updates = 0;
  60.  
  61. 	hrt_abstime updates_counter_start = hrt_absolute_time();
  62. 	hrt_abstime pub_last = hrt_absolute_time();
  63.  
  64. 	hrt_abstime t_prev = 0;
  65.  
  66. 	/* store error when sensor updates, but correct on each time step to avoid jumps in estimated value */
  67. 	float acc[] = { 0.0f, 0.0f, 0.0f };	// N E D
  68. 	float acc_bias[] = { 0.0f, 0.0f, 0.0f };	// body frame
  69. 	float corr_baro = 0.0f;		// D
  70. 	float corr_gps[3][2] = {
  71. 		{ 0.0f, 0.0f },		// N (pos, vel)
  72. 		{ 0.0f, 0.0f },		// E (pos, vel)
  73. 		{ 0.0f, 0.0f },		// D (pos, vel)
  74. 	};
  75. 	float w_gps_xy = 1.0f;
  76. 	float w_gps_z = 1.0f;
  77.  
  78. 	float corr_vision[3][2] = {
  79. 		{ 0.0f, 0.0f },		// N (pos, vel)
  80. 		{ 0.0f, 0.0f },		// E (pos, vel)
  81. 		{ 0.0f, 0.0f },		// D (pos, vel)
  82. 	};
  83.  
  84. 	float corr_mocap[3][1] = {
  85. 		{ 0.0f },		// N (pos)
  86. 		{ 0.0f },		// E (pos)
  87. 		{ 0.0f },		// D (pos)
  88. 	};
  89. 	const int mocap_heading = 2;
  90.  
  91. 	float dist_ground = 0.0f;		//variables for lidar altitude estimation
  92. 	float corr_lidar = 0.0f;
  93. 	float lidar_offset = 0.0f;
  94. 	int lidar_offset_count = 0;
  95. 	bool lidar_first = true;
  96. 	bool use_lidar = false;
  97. 	bool use_lidar_prev = false;
  98.  
  99. 	float corr_flow[] = { 0.0f, 0.0f };	// N E
  100. 	float w_flow = 0.0f;
  101.  
  102. 	hrt_abstime lidar_time = 0;			// time of last lidar measurement (not filtered)
  103. 	hrt_abstime lidar_valid_time = 0;	// time of last lidar measurement used for correction (filtered)
  104.  
  105. 	int n_flow = 0;
  106. 	float gyro_offset_filtered[] = { 0.0f, 0.0f, 0.0f };
  107. 	float flow_gyrospeed[] = { 0.0f, 0.0f, 0.0f };
  108. 	float flow_gyrospeed_filtered[] = { 0.0f, 0.0f, 0.0f };
  109. 	float att_gyrospeed_filtered[] = { 0.0f, 0.0f, 0.0f };
  110. 	float yaw_comp[] = { 0.0f, 0.0f };
  111. 	hrt_abstime flow_time = 0;
  112. 	float flow_min_dist = 0.2f;
  113.  
  114. 	bool gps_valid = false;			// GPS is valid
  115. 	bool lidar_valid = false;		// lidar is valid
  116. 	bool flow_valid = false;		// flow is valid
  117. 	bool flow_accurate = false;		// flow should be accurate (this flag not updated if flow_valid == false)
  118. 	bool vision_valid = false;		// vision is valid
  119. 	bool mocap_valid = false;		// mocap is valid
  120.  
  121. 	/* declare and safely initialize all structs */
  122. 	struct actuator_controls_s actuator;
  123. 	memset(&actuator, 0, sizeof(actuator));
  124. 	struct actuator_armed_s armed;
  125. 	memset(&armed, 0, sizeof(armed));
  126. 	struct sensor_combined_s sensor;
  127. 	memset(&sensor, 0, sizeof(sensor));
  128. 	struct vehicle_gps_position_s gps;
  129. 	memset(&gps, 0, sizeof(gps));
  130. 	struct vehicle_attitude_s att;
  131. 	memset(&att, 0, sizeof(att));
  132. 	struct vehicle_local_position_s local_pos;
  133. 	memset(&local_pos, 0, sizeof(local_pos));
  134. 	struct optical_flow_s flow;
  135. 	memset(&flow, 0, sizeof(flow));
  136. 	struct vision_position_estimate_s vision;
  137. 	memset(&vision, 0, sizeof(vision));
  138. 	struct att_pos_mocap_s mocap;
  139. 	memset(&mocap, 0, sizeof(mocap));
  140. 	struct vehicle_global_position_s global_pos;
  141. 	memset(&global_pos, 0, sizeof(global_pos));
  142. 	struct distance_sensor_s lidar;
  143. 	memset(&lidar, 0, sizeof(lidar));
  144. 	struct vehicle_rates_setpoint_s rates_setpoint;
  145. 	memset(&rates_setpoint, 0, sizeof(rates_setpoint));
  146. /**************************订阅各种信息******************************/
  147. 	/* subscribe */
  148. 	int parameter_update_sub = orb_subscribe(ORB_ID(parameter_update));
  149. 	int actuator_sub = orb_subscribe(ORB_ID_VEHICLE_ATTITUDE_CONTROLS);
  150. 	int armed_sub = orb_subscribe(ORB_ID(actuator_armed));
  151. 	int sensor_combined_sub = orb_subscribe(ORB_ID(sensor_combined));
  152. 	int vehicle_attitude_sub = orb_subscribe(ORB_ID(vehicle_attitude));
  153. 	int optical_flow_sub = orb_subscribe(ORB_ID(optical_flow));
  154. 	int vehicle_gps_position_sub = orb_subscribe(ORB_ID(vehicle_gps_position));
  155. 	int vision_position_estimate_sub = orb_subscribe(ORB_ID(vision_position_estimate));
  156. 	int att_pos_mocap_sub = orb_subscribe(ORB_ID(att_pos_mocap));
  157. 	int distance_sensor_sub = orb_subscribe(ORB_ID(distance_sensor));
  158. 	int vehicle_rate_sp_sub = orb_subscribe(ORB_ID(vehicle_rates_setpoint));
  159. /**************************发布位置信息******************************/
  160. 	/* advertise */
  161. 	orb_advert_t vehicle_local_position_pub = orb_advertise(ORB_ID(vehicle_local_position), &local_pos);
  162. 	orb_advert_t vehicle_global_position_pub = NULL;
  163. /**************************类似于初始化params******************************/
  164. 	struct position_estimator_inav_params params;
  165. 	memset(¶ms, 0, sizeof(params));
  166. 	struct position_estimator_inav_param_handles pos_inav_param_handles;
  167. 	/* initialize parameter handles */
  168. 	inav_parameters_init(&pos_inav_param_handles);
  169.  
  170. 	/* first parameters read at start up */
  171. 	struct parameter_update_s param_update;
  172. 	orb_copy(ORB_ID(parameter_update), parameter_update_sub,
  173. 		 ¶m_update); /* read from param topic to clear updated flag */
  174. 	/* first parameters update */
  175. 	inav_parameters_update(&pos_inav_param_handles, ¶ms);
  176. /**************************sensor_combined******************************/
  177. 	px4_pollfd_struct_t fds_init[1] = {};
  178. 	fds_init[0].fd = sensor_combined_sub;
  179. 	fds_init[0].events = POLLIN;
  180. 气压计///
  181. 	/* wait for initial baro value */
  182. 	bool wait_baro = true;
  183. 	TerrainEstimator *terrain_estimator = new TerrainEstimator();
  184.  
  185. 	thread_running = true;
  186. 	hrt_abstime baro_wait_for_sample_time = hrt_absolute_time();
  187.  
  188. 	while (wait_baro && !thread_should_exit) {
  189. 		int ret = px4_poll(&fds_init[0], 1, 1000);
  190.  
  191. 		if (ret < 0) {
  192. 			/* poll error */
  193. 			mavlink_log_info(&mavlink_log_pub, "[inav] poll error on init");
  194. 		} else if (hrt_absolute_time() - baro_wait_for_sample_time > MAX_WAIT_FOR_BARO_SAMPLE) {
  195. 			wait_baro = false;
  196. 			mavlink_log_info(&mavlink_log_pub, "[inav] timed out waiting for a baro sample");
  197. 		}
  198. 		else if (ret > 0) {
  199. 			if (fds_init[0].revents & POLLIN) {
  200. 				orb_copy(ORB_ID(sensor_combined), sensor_combined_sub, &sensor);
  201.  
  202. 				if (wait_baro && sensor.baro_timestamp[0] != baro_timestamp) {
  203. 					baro_timestamp = sensor.baro_timestamp[0];
  204. 					baro_wait_for_sample_time = hrt_absolute_time();
  205.  
  206. 					/* mean calculation over several measurements */
  207. 					if (baro_init_cnt < baro_init_num) {
  208. 						if (PX4_ISFINITE(sensor.baro_alt_meter[0])) {
  209. 							baro_offset += sensor.baro_alt_meter[0];
  210. 							baro_init_cnt++;
  211. 						}
  212.  
  213. 					} else {
  214. 						wait_baro = false;
  215. 						baro_offset /= (float) baro_init_cnt;
  216. 						local_pos.z_valid = true;
  217. 						local_pos.v_z_valid = true;
  218. 					}
  219. 				}
  220. 			}
  221.  
  222. 		} else {
  223. 			PX4_WARN("INAV poll timeout");
  224. 		}
  225. 	}
  226. /**************************主循环******************************/
  227. 	/* main loop */
  228. 	px4_pollfd_struct_t fds[1];
  229. 	fds[0].fd = vehicle_attitude_sub;
  230. 	fds[0].events = POLLIN;
  231. 判断是否应该退出该线程///
  232. 	while (!thread_should_exit) {   //这个判断条件经常使用，到处都用过类似的
  233. 		int ret = px4_poll(fds, 1, 20); // wait maximal 20 ms = 50 Hz minimum rate
  234. 		hrt_abstime t = hrt_absolute_time();
  235.  
  236. 		if (ret < 0) {
  237. 			/* poll error */
  238. 			mavlink_log_info(&mavlink_log_pub, "[inav] poll error on init");
  239. 			continue;
  240.  
  241. 		} else if (ret > 0) {
  242. 			/* act on attitude updates */
  243.  
  244. 			/* vehicle attitude */
  245. 			orb_copy(ORB_ID(vehicle_attitude), vehicle_attitude_sub, &att);//姿态跟新
  246. 			attitude_updates++;
  247.  
  248. 			bool updated;
  249.  
  250. 			/* parameter update */
  251. 			orb_check(parameter_update_sub, &updated);
  252.  
  253. 			if (updated) {
  254. 				struct parameter_update_s update;
  255. 				orb_copy(ORB_ID(parameter_update), parameter_update_sub, &update);//parameter跟新
  256. 				inav_parameters_update(&pos_inav_param_handles, ¶ms);
  257. 			}
  258.  
  259. 			/* actuator */
  260. 			orb_check(actuator_sub, &updated);
  261.  
  262. 			if (updated) {
  263. 				orb_copy(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, actuator_sub, &actuator);//执行器数据跟新（这里并不是pwm）
  264. 			}
  265.  
  266. 			/* armed */
  267. 			orb_check(armed_sub, &updated);
  268.  
  269. 			if (updated) {
  270. 				orb_copy(ORB_ID(actuator_armed), armed_sub, &armed);//解锁数据跟新
  271. 			}
  272.  
  273. 			/* sensor combined */
  274. 			orb_check(sensor_combined_sub, &updated);
  275.  
  276. 			if (updated) {
  277. 				orb_copy(ORB_ID(sensor_combined), sensor_combined_sub, &sensor);//传感器融合数据跟新
  278.  
  279. 				if (sensor.accelerometer_timestamp[0] != accel_timestamp) {
  280. 					if (att.R_valid) {
  281. 						/* correct accel bias */
  282. 						sensor.accelerometer_m_s2[0] -= acc_bias[0];
  283. 						sensor.accelerometer_m_s2[1] -= acc_bias[1];
  284. 						sensor.accelerometer_m_s2[2] -= acc_bias[2];
  285.  
  286. 						/* transform acceleration vector from body frame to NED frame */
  287. 						for (int i = 0; i < 3; i++) {
  288. 							acc[i] = 0.0f;
  289.  
  290. 							for (int j = 0; j < 3; j++) {
  291. 								acc[i] += PX4_R(att.R, i, j) * sensor.accelerometer_m_s2[j];
  292. 							}
  293. 						}
  294.  
  295. 						acc[2] += CONSTANTS_ONE_G;
  296.  
  297. 					} else {
  298. 						memset(acc, 0, sizeof(acc));
  299. 					}
  300.  
  301. 					accel_timestamp = sensor.accelerometer_timestamp[0];
  302. 					accel_updates++;
  303. 				}
  304.  
  305. 				if (sensor.baro_timestamp[0] != baro_timestamp) {
  306. 					corr_baro = baro_offset - sensor.baro_alt_meter[0] - z_est[0];
  307. 					baro_timestamp = sensor.baro_timestamp[0];
  308. 					baro_updates++;
  309. 				}
  310. 			}
  311.  
  312.  
  313. 			/* lidar alt estimation */
  314. 			orb_check(distance_sensor_sub, &updated);
  315.  
  316. 			/* update lidar separately, needed by terrain estimator */
  317. 			if (updated) {
  318. 				orb_copy(ORB_ID(distance_sensor), distance_sensor_sub, &lidar);//高度数据跟新
  319. 				lidar.current_distance += params.lidar_calibration_offset;
  320. 			}
  321.  
  322. 			if (updated) { //check if altitude estimation for lidar is enabled and new sensor data
  323.  
  324. 				if (params.enable_lidar_alt_est && lidar.current_distance > lidar.min_distance && lidar.current_distance < lidar.max_distance
  325. 			    		&& (PX4_R(att.R, 2, 2) > 0.7f)) {
  326.  
  327. 					if (!use_lidar_prev && use_lidar) {
  328. 						lidar_first = true;
  329. 					}
  330.  
  331. 					use_lidar_prev = use_lidar;
  332.  
  333. 					lidar_time = t;
  334. 					dist_ground = lidar.current_distance * PX4_R(att.R, 2, 2); //vertical distance
  335.  
  336. 					if (lidar_first) {
  337. 						lidar_first = false;
  338. 						lidar_offset = dist_ground + z_est[0];
  339. 						mavlink_log_info(&mavlink_log_pub, "[inav] LIDAR: new ground offset");
  340. 						warnx("[inav] LIDAR: new ground offset");
  341. 					}
  342.  
  343. 					corr_lidar = lidar_offset - dist_ground - z_est[0];
  344.  
  345. 					if (fabsf(corr_lidar) > params.lidar_err) { //check for spike
  346. 						corr_lidar = 0;
  347. 						lidar_valid = false;
  348. 						lidar_offset_count++;
  349.  
  350. 						if (lidar_offset_count > 3) { //if consecutive bigger/smaller measurements -> new ground offset -> reinit
  351. 							lidar_first = true;
  352. 							lidar_offset_count = 0;
  353. 						}
  354.  
  355. 					} else {
  356. 						corr_lidar = lidar_offset - dist_ground - z_est[0];
  357. 						lidar_valid = true;
  358. 						lidar_offset_count = 0;
  359. 						lidar_valid_time = t;
  360. 					}
  361. 				} else {
  362. 					lidar_valid = false;
  363. 				}
  364. 			}
  365. /**************************************这里就是光流的处理*********************************/
  366. 			/* optical flow */
  367. 			orb_check(optical_flow_sub, &updated);
  368.  
  369. 			if (updated && lidar_valid) {
  370. 				orb_copy(ORB_ID(optical_flow), optical_flow_sub, &flow);//光流数据跟新
  371.  
  372. 				flow_time = t;
  373. 				float flow_q = flow.quality / 255.0f;
  374. 				float dist_bottom = lidar.current_distance;//高度信息
  375.  
  376. 				if (dist_bottom > flow_min_dist && flow_q > params.flow_q_min && PX4_R(att.R, 2, 2) > 0.7f) {
  377. 					/* distance to surface */
  378. 					//float flow_dist = dist_bottom / PX4_R(att.R, 2, 2); //use this if using sonar
  379. 					float flow_dist = dist_bottom; //use this if using lidar
  380.  
  381. 					/* check if flow if too large for accurate measurements */
  382. 					/* calculate estimated velocity in body frame */
  383. 					float body_v_est[2] = { 0.0f, 0.0f };
  384.  
  385. 					for (int i = 0; i < 2; i++) {
  386. 						body_v_est[i] = PX4_R(att.R, 0, i) * x_est[1] + PX4_R(att.R, 1, i) * y_est[1] + PX4_R(att.R, 2, i) * z_est[1];
  387. 					}
  388.  
  389. 					/* set this flag if flow should be accurate according to current velocity and attitude rate estimate */
  390. 					flow_accurate = fabsf(body_v_est[1] / flow_dist - att.rollspeed) < max_flow &&
  391. 							fabsf(body_v_est[0] / flow_dist + att.pitchspeed) < max_flow;
  392.  
  393. 					/*calculate offset of flow-gyro using already calibrated gyro from autopilot*/
  394. 					flow_gyrospeed[0] = flow.gyro_x_rate_integral / (float)flow.integration_timespan * 1000000.0f;
  395. 					flow_gyrospeed[1] = flow.gyro_y_rate_integral / (float)flow.integration_timespan * 1000000.0f;
  396. 					flow_gyrospeed[2] = flow.gyro_z_rate_integral / (float)flow.integration_timespan * 1000000.0f;
  397.  
  398. 					//moving average
  399. 					if (n_flow >= 100) {
  400. 						gyro_offset_filtered[0] = flow_gyrospeed_filtered[0] - att_gyrospeed_filtered[0];
  401. 						gyro_offset_filtered[1] = flow_gyrospeed_filtered[1] - att_gyrospeed_filtered[1];
  402. 						gyro_offset_filtered[2] = flow_gyrospeed_filtered[2] - att_gyrospeed_filtered[2];
  403. 						n_flow = 0;
  404. 						flow_gyrospeed_filtered[0] = 0.0f;
  405. 						flow_gyrospeed_filtered[1] = 0.0f;
  406. 						flow_gyrospeed_filtered[2] = 0.0f;
  407. 						att_gyrospeed_filtered[0] = 0.0f;
  408. 						att_gyrospeed_filtered[1] = 0.0f;
  409. 						att_gyrospeed_filtered[2] = 0.0f;
  410.  
  411. 					} else {
  412. 						flow_gyrospeed_filtered[0] = (flow_gyrospeed[0] + n_flow * flow_gyrospeed_filtered[0]) / (n_flow + 1);
  413. 						flow_gyrospeed_filtered[1] = (flow_gyrospeed[1] + n_flow * flow_gyrospeed_filtered[1]) / (n_flow + 1);
  414. 						flow_gyrospeed_filtered[2] = (flow_gyrospeed[2] + n_flow * flow_gyrospeed_filtered[2]) / (n_flow + 1);
  415. 						att_gyrospeed_filtered[0] = (att.pitchspeed + n_flow * att_gyrospeed_filtered[0]) / (n_flow + 1);
  416. 						att_gyrospeed_filtered[1] = (att.rollspeed + n_flow * att_gyrospeed_filtered[1]) / (n_flow + 1);
  417. 						att_gyrospeed_filtered[2] = (att.yawspeed + n_flow * att_gyrospeed_filtered[2]) / (n_flow + 1);
  418. 						n_flow++;
  419. 					}
  420.  
  421.  
  422. 					/*yaw compensation (flow sensor is not in center of rotation) -> params in QGC*/
  423. 					yaw_comp[0] = - params.flow_module_offset_y * (flow_gyrospeed[2] - gyro_offset_filtered[2]);
  424. 					yaw_comp[1] = params.flow_module_offset_x * (flow_gyrospeed[2] - gyro_offset_filtered[2]);
  425.  
  426. 					/* convert raw flow to angular flow (rad/s) */
  427. 					float flow_ang[2];
  428.  
  429. 					/* check for vehicle rates setpoint - it below threshold -> dont subtract -> better hover */
  430. 					orb_check(vehicle_rate_sp_sub, &updated);
  431. 					if (updated)
  432. 						orb_copy(ORB_ID(vehicle_rates_setpoint), vehicle_rate_sp_sub, &rates_setpoint);
  433.  
  434. 					double rate_threshold = 0.15f;
  435.  
  436. 					if (fabs(rates_setpoint.pitch) < rate_threshold) {
  437. 						//warnx("[inav] test ohne comp");
  438. 						flow_ang[0] = (flow.pixel_flow_x_integral / (float)flow.integration_timespan * 1000000.0f) * params.flow_k;//for now the flow has to be scaled (to small)
  439. 					}
  440. 					else {
  441. 						//warnx("[inav] test mit comp");
  442. 						//calculate flow [rad/s] and compensate for rotations (and offset of flow-gyro)
  443. 						flow_ang[0] = ((flow.pixel_flow_x_integral - flow.gyro_x_rate_integral) / (float)flow.integration_timespan * 1000000.0f
  444. 							       + gyro_offset_filtered[0]) * params.flow_k;//for now the flow has to be scaled (to small)
  445. 					}
  446.  
  447. 					if (fabs(rates_setpoint.roll) < rate_threshold) {
  448. 						flow_ang[1] = (flow.pixel_flow_y_integral / (float)flow.integration_timespan * 1000000.0f) * params.flow_k;//for now the flow has to be scaled (to small)
  449. 					}
  450. 					else {
  451. 						//calculate flow [rad/s] and compensate for rotations (and offset of flow-gyro)
  452. 						flow_ang[1] = ((flow.pixel_flow_y_integral - flow.gyro_y_rate_integral) / (float)flow.integration_timespan * 1000000.0f
  453. 							       + gyro_offset_filtered[1]) * params.flow_k;//for now the flow has to be scaled (to small)
  454. 					}
  455.  
  456. 					/* flow measurements vector */
  457. 					float flow_m[3];
  458. 					if (fabs(rates_setpoint.yaw) < rate_threshold) {
  459. 						flow_m[0] = -flow_ang[0] * flow_dist;
  460. 						flow_m[1] = -flow_ang[1] * flow_dist;
  461. 					} else {
  462. 						flow_m[0] = -flow_ang[0] * flow_dist - yaw_comp[0] * params.flow_k;
  463. 						flow_m[1] = -flow_ang[1] * flow_dist - yaw_comp[1] * params.flow_k;
  464. 					}
  465. 					flow_m[2] = z_est[1];
  466.  
  467. 					/* velocity in NED */
  468. 					float flow_v[2] = { 0.0f, 0.0f };
  469.  
  470. 					/* project measurements vector to NED basis, skip Z component */
  471. 					for (int i = 0; i < 2; i++) {
  472. 						for (int j = 0; j < 3; j++) {
  473. 							flow_v[i] += PX4_R(att.R, i, j) * flow_m[j];
  474. 						}
  475. 					}
  476.  
  477. 					/* velocity correction */
  478. 					corr_flow[0] = flow_v[0] - x_est[1];
  479. 					corr_flow[1] = flow_v[1] - y_est[1];
  480. 					/* adjust correction weight */
  481. 					float flow_q_weight = (flow_q - params.flow_q_min) / (1.0f - params.flow_q_min);
  482. 					w_flow = PX4_R(att.R, 2, 2) * flow_q_weight / fmaxf(1.0f, flow_dist);
  483.  
  484.  
  485. 					/* if flow is not accurate, reduce weight for it */
  486. 					// TODO make this more fuzzy
  487. 					if (!flow_accurate) {
  488. 						w_flow *= 0.05f;
  489. 					}
  490.  
  491. 					/* under ideal conditions, on 1m distance assume EPH = 10cm */
  492. 					eph_flow = 0.1f / w_flow;
  493.  
  494. 					flow_valid = true;
  495.  
  496. 				} else {
  497. 					w_flow = 0.0f;
  498. 					flow_valid = false;
  499. 				}
  500.  
  501. 				flow_updates++;
  502. 			}
  503. /**************************************光流结束*********************************/
  504. //视觉的处理
  505. 			/* check no vision circuit breaker is set */
  506. 			if (params.no_vision != CBRK_NO_VISION_KEY) {
  507. 				/* vehicle vision position */
  508. 				orb_check(vision_position_estimate_sub, &updated);
  509.  
  510. 				if (updated) {
  511. 					orb_copy(ORB_ID(vision_position_estimate), vision_position_estimate_sub, &vision);
  512.  
  513. 					static float last_vision_x = 0.0f;
  514. 					static float last_vision_y = 0.0f;
  515. 					static float last_vision_z = 0.0f;
  516.  
  517. 					/* reset position estimate on first vision update */
  518. 					if (!vision_valid) {
  519. 						x_est[0] = vision.x;
  520. 						x_est[1] = vision.vx;
  521. 						y_est[0] = vision.y;
  522. 						y_est[1] = vision.vy;
  523.  
  524. 						/* only reset the z estimate if the z weight parameter is not zero */
  525. 						if (params.w_z_vision_p > MIN_VALID_W) {
  526. 							z_est[0] = vision.z;
  527. 							z_est[1] = vision.vz;
  528. 						}
  529.  
  530. 						vision_valid = true;
  531.  
  532. 						last_vision_x = vision.x;
  533. 						last_vision_y = vision.y;
  534. 						last_vision_z = vision.z;
  535.  
  536. 						warnx("VISION estimate valid");
  537. 						mavlink_log_info(&mavlink_log_pub, "[inav] VISION estimate valid");
  538. 					}
  539.  
  540. 					/* calculate correction for position */
  541. 					corr_vision[0][0] = vision.x - x_est[0];
  542. 					corr_vision[1][0] = vision.y - y_est[0];
  543. 					corr_vision[2][0] = vision.z - z_est[0];
  544.  
  545. 					static hrt_abstime last_vision_time = 0;
  546.  
  547. 					float vision_dt = (vision.timestamp_boot - last_vision_time) / 1e6f;
  548. 					last_vision_time = vision.timestamp_boot;
  549.  
  550. 					if (vision_dt > 0.000001f && vision_dt < 0.2f) {
  551. 						vision.vx = (vision.x - last_vision_x) / vision_dt;
  552. 						vision.vy = (vision.y - last_vision_y) / vision_dt;
  553. 						vision.vz = (vision.z - last_vision_z) / vision_dt;
  554.  
  555. 						last_vision_x = vision.x;
  556. 						last_vision_y = vision.y;
  557. 						last_vision_z = vision.z;
  558.  
  559. 						/* calculate correction for velocity */
  560. 						corr_vision[0][1] = vision.vx - x_est[1];
  561. 						corr_vision[1][1] = vision.vy - y_est[1];
  562. 						corr_vision[2][1] = vision.vz - z_est[1];
  563.  
  564. 					} else {
  565. 						/* assume zero motion */
  566. 						corr_vision[0][1] = 0.0f - x_est[1];
  567. 						corr_vision[1][1] = 0.0f - y_est[1];
  568. 						corr_vision[2][1] = 0.0f - z_est[1];
  569. 					}
  570.  
  571. 					vision_updates++;
  572. 				}
  573. 			}
  574. ///mocap数据处理///
  575. 			/* vehicle mocap position */
  576. 			orb_check(att_pos_mocap_sub, &updated);
  577.  
  578. 			if (updated) {
  579. 				orb_copy(ORB_ID(att_pos_mocap), att_pos_mocap_sub, &mocap);
  580.  
  581. 				if (!params.disable_mocap) {
  582. 					/* reset position estimate on first mocap update */
  583. 					if (!mocap_valid) {
  584. 						x_est[0] = mocap.x;
  585. 						y_est[0] = mocap.y;
  586. 						z_est[0] = mocap.z;
  587.  
  588. 						mocap_valid = true;
  589.  
  590. 						warnx("MOCAP data valid");
  591. 						mavlink_log_info(&mavlink_log_pub, "[inav] MOCAP data valid");
  592. 					}
  593.  
  594. 					/* calculate correction for position */
  595. 					corr_mocap[0][0] = mocap.x - x_est[0];
  596. 					corr_mocap[1][0] = mocap.y - y_est[0];
  597. 					corr_mocap[2][0] = mocap.z - z_est[0];
  598.  
  599. 					mocap_updates++;
  600. 				}
  601. 			}
  602. //GPS数据处理///
  603. 			/* vehicle GPS position */
  604. 			orb_check(vehicle_gps_position_sub, &updated);
  605.  
  606. 			if (updated) {
  607. 				orb_copy(ORB_ID(vehicle_gps_position), vehicle_gps_position_sub, &gps);
  608.  
  609. 				bool reset_est = false;
  610.  
  611. 				/* hysteresis for GPS quality */
  612. 				if (gps_valid) {
  613. 					if (gps.eph > max_eph_epv || gps.epv > max_eph_epv || gps.fix_type < 3) {
  614. 						gps_valid = false;
  615. 						mavlink_log_info(&mavlink_log_pub, "[inav] GPS signal lost");
  616. 						warnx("[inav] GPS signal lost");
  617. 					}
  618.  
  619. 				} else {
  620. 					if (gps.eph < max_eph_epv * 0.7f && gps.epv < max_eph_epv * 0.7f && gps.fix_type >= 3) {
  621. 						gps_valid = true;
  622. 						reset_est = true;
  623. 						mavlink_log_info(&mavlink_log_pub, "[inav] GPS signal found");
  624. 						warnx("[inav] GPS signal found");
  625. 					}
  626. 				}
  627.  
  628. 				if (gps_valid) {
  629. 					double lat = gps.lat * 1e-7;
  630. 					double lon = gps.lon * 1e-7;
  631. 					float alt = gps.alt * 1e-3;
  632.  
  633. 					/* initialize reference position if needed */
  634. 					if (!ref_inited) {
  635. 						if (ref_init_start == 0) {
  636. 							ref_init_start = t;
  637.  
  638. 						} else if (t > ref_init_start + ref_init_delay) {
  639. 							ref_inited = true;
  640.  
  641. 							/* set position estimate to (0, 0, 0), use GPS velocity for XY */
  642. 							x_est[0] = 0.0f;
  643. 							x_est[1] = gps.vel_n_m_s;
  644. 							y_est[0] = 0.0f;
  645. 							y_est[1] = gps.vel_e_m_s;
  646.  
  647. 							local_pos.ref_lat = lat;
  648. 							local_pos.ref_lon = lon;
  649. 							local_pos.ref_alt = alt + z_est[0];
  650. 							local_pos.ref_timestamp = t;
  651.  
  652. 							/* initialize projection */
  653. 							map_projection_init(&ref, lat, lon);
  654. 							// XXX replace this print
  655. 							warnx("init ref: lat=%.7f, lon=%.7f, alt=%8.4f", (double)lat, (double)lon, (double)alt);
  656. 							mavlink_log_info(&mavlink_log_pub, "[inav] init ref: %.7f, %.7f, %8.4f", (double)lat, (double)lon, (double)alt);
  657. 						}
  658. 					}
  659.  
  660. 					if (ref_inited) {
  661. 						/* project GPS lat lon to plane */
  662. 						float gps_proj[2];
  663. 						map_projection_project(&ref, lat, lon, &(gps_proj[0]), &(gps_proj[1]));
  664.  
  665. 						/* reset position estimate when GPS becomes good */
  666. 						if (reset_est) {
  667. 							x_est[0] = gps_proj[0];
  668. 							x_est[1] = gps.vel_n_m_s;
  669. 							y_est[0] = gps_proj[1];
  670. 							y_est[1] = gps.vel_e_m_s;
  671. 						}
  672.  
  673. 						/* calculate index of estimated values in buffer */
  674. 						int est_i = buf_ptr - 1 - min(EST_BUF_SIZE - 1, max(0, (int)(params.delay_gps * 1000000.0f / PUB_INTERVAL)));
  675.  
  676. 						if (est_i < 0) {
  677. 							est_i += EST_BUF_SIZE;
  678. 						}
  679.  
  680. 						/* calculate correction for position */
  681. 						corr_gps[0][0] = gps_proj[0] - est_buf[est_i][0][0];
  682. 						corr_gps[1][0] = gps_proj[1] - est_buf[est_i][1][0];
  683. 						corr_gps[2][0] = local_pos.ref_alt - alt - est_buf[est_i][2][0];
  684.  
  685. 						/* calculate correction for velocity */
  686. 						if (gps.vel_ned_valid) {
  687. 							corr_gps[0][1] = gps.vel_n_m_s - est_buf[est_i][0][1];
  688. 							corr_gps[1][1] = gps.vel_e_m_s - est_buf[est_i][1][1];
  689. 							corr_gps[2][1] = gps.vel_d_m_s - est_buf[est_i][2][1];
  690.  
  691. 						} else {
  692. 							corr_gps[0][1] = 0.0f;
  693. 							corr_gps[1][1] = 0.0f;
  694. 							corr_gps[2][1] = 0.0f;
  695. 						}
  696.  
  697. 						/* save rotation matrix at this moment */
  698. 						memcpy(R_gps, R_buf[est_i], sizeof(R_gps));
  699.  
  700. 						w_gps_xy = min_eph_epv / fmaxf(min_eph_epv, gps.eph);
  701. 						w_gps_z = min_eph_epv / fmaxf(min_eph_epv, gps.epv);
  702. 					}
  703.  
  704. 				} else {
  705. 					/* no GPS lock */
  706. 					memset(corr_gps, 0, sizeof(corr_gps));
  707. 					ref_init_start = 0;
  708. 				}
  709.  
  710. 				gps_updates++;
  711. 			}
  712. 		}
  713. ///检查是否timeout//
  714. 		/* check for timeout on FLOW topic */
  715. 		if ((flow_valid || lidar_valid) && t > (flow_time + flow_topic_timeout)) {
  716. 			flow_valid = false;
  717. 			warnx("FLOW timeout");
  718. 			mavlink_log_info(&mavlink_log_pub, "[inav] FLOW timeout");
  719. 		}
  720.  
  721. 		/* check for timeout on GPS topic */
  722. 		if (gps_valid && (t > (gps.timestamp_position + gps_topic_timeout))) {
  723. 			gps_valid = false;
  724. 			warnx("GPS timeout");
  725. 			mavlink_log_info(&mavlink_log_pub, "[inav] GPS timeout");
  726. 		}
  727.  
  728. 		/* check for timeout on vision topic */
  729. 		if (vision_valid && (t > (vision.timestamp_boot + vision_topic_timeout))) {
  730. 			vision_valid = false;
  731. 			warnx("VISION timeout");
  732. 			mavlink_log_info(&mavlink_log_pub, "[inav] VISION timeout");
  733. 		}
  734.  
  735. 		/* check for timeout on mocap topic */
  736. 		if (mocap_valid && (t > (mocap.timestamp_boot + mocap_topic_timeout))) {
  737. 			mocap_valid = false;
  738. 			warnx("MOCAP timeout");
  739. 			mavlink_log_info(&mavlink_log_pub, "[inav] MOCAP timeout");
  740. 		}
  741.  
  742. 		/* check for lidar measurement timeout */
  743. 		if (lidar_valid && (t > (lidar_time + lidar_timeout))) {
  744. 			lidar_valid = false;
  745. 			warnx("LIDAR timeout");
  746. 			mavlink_log_info(&mavlink_log_pub, "[inav] LIDAR timeout");
  747. 		}
  748.  
  749. 		float dt = t_prev > 0 ? (t - t_prev) / 1000000.0f : 0.0f;
  750. 		dt = fmaxf(fminf(0.02, dt), 0.0002);		// constrain dt from 0.2 to 20 ms
  751. 		t_prev = t;
  752.  
  753. 		/* increase EPH/EPV on each step */
  754. 		if (eph < 0.000001f) { //get case where eph is 0 -> would stay 0
  755. 			eph = 0.001;
  756. 		}
  757.  
  758. 		if (eph < max_eph_epv) {
  759. 			eph *= 1.0f + dt;
  760. 		}
  761.  
  762. 		if (epv < 0.000001f) { //get case where epv is 0 -> would stay 0
  763. 			epv = 0.001;
  764. 		}
  765.  
  766. 		if (epv < max_eph_epv) {
  767. 			epv += 0.005f * dt;	// add 1m to EPV each 200s (baro drift)
  768. 		}
  769. //根据具体情况，将决策赋给标志位//
  770. 		/* use GPS if it's valid and reference position initialized */使用全球定位系统，如果它是有效的，并参考位置初始化
  771. 		bool use_gps_xy = ref_inited && gps_valid && params.w_xy_gps_p > MIN_VALID_W;
  772. 		bool use_gps_z = ref_inited && gps_valid && params.w_z_gps_p > MIN_VALID_W;
  773. 		/* use VISION if it's valid and has a valid weight parameter */使用VISION，如果它是有效的，并具有有效的权重参数
  774. 		bool use_vision_xy = vision_valid && params.w_xy_vision_p > MIN_VALID_W;
  775. 		bool use_vision_z = vision_valid && params.w_z_vision_p > MIN_VALID_W;
  776. 		/* use MOCAP if it's valid and has a valid weight parameter */使用MOCAP如果它是有效的，并具有有效的权重参数
  777. 		bool use_mocap = mocap_valid && params.w_mocap_p > MIN_VALID_W && params.att_ext_hdg_m == mocap_heading; //check if external heading is mocap
  778.  
  779. 		if (params.disable_mocap) { //disable mocap if fake gps is used
  780. 			use_mocap = false;
  781. 		}
  782.  
  783. 		/* use flow if it's valid and (accurate or no GPS available) */
  784. 		bool use_flow = flow_valid && (flow_accurate || !use_gps_xy);
  785.  
  786. 		/* use LIDAR if it's valid and lidar altitude estimation is enabled */
  787. 		use_lidar = lidar_valid && params.enable_lidar_alt_est;
  788.  
  789. 		bool can_estimate_xy = (eph < max_eph_epv) || use_gps_xy || use_flow || use_vision_xy || use_mocap;
  790.  
  791. 		bool dist_bottom_valid = (t < lidar_valid_time + lidar_valid_timeout);
  792.  
  793. 		float w_xy_gps_p = params.w_xy_gps_p * w_gps_xy;
  794. 		float w_xy_gps_v = params.w_xy_gps_v * w_gps_xy;
  795. 		float w_z_gps_p = params.w_z_gps_p * w_gps_z;
  796. 		float w_z_gps_v = params.w_z_gps_v * w_gps_z;
  797.  
  798. 		float w_xy_vision_p = params.w_xy_vision_p;
  799. 		float w_xy_vision_v = params.w_xy_vision_v;
  800. 		float w_z_vision_p = params.w_z_vision_p;
  801.  
  802. 		float w_mocap_p = params.w_mocap_p;
  803. //根据之前的决策，开始校准数据处理///
  804. 		/* reduce GPS weight if optical flow is good */
  805. 		if (use_flow && flow_accurate) {
  806. 			w_xy_gps_p *= params.w_gps_flow;
  807. 			w_xy_gps_v *= params.w_gps_flow;
  808. 		}
  809.  
  810. 		/* baro offset correction */
  811. 		if (use_gps_z) {
  812. 			float offs_corr = corr_gps[2][0] * w_z_gps_p * dt;
  813. 			baro_offset += offs_corr;
  814. 			corr_baro += offs_corr;
  815. 		}
  816.  
  817. 		/* accelerometer bias correction for GPS (use buffered rotation matrix) */
  818. 		float accel_bias_corr[3] = { 0.0f, 0.0f, 0.0f };
  819.  
  820. 		if (use_gps_xy) {
  821. 			accel_bias_corr[0] -= corr_gps[0][0] * w_xy_gps_p * w_xy_gps_p;
  822. 			accel_bias_corr[0] -= corr_gps[0][1] * w_xy_gps_v;
  823. 			accel_bias_corr[1] -= corr_gps[1][0] * w_xy_gps_p * w_xy_gps_p;
  824. 			accel_bias_corr[1] -= corr_gps[1][1] * w_xy_gps_v;
  825. 		}
  826.  
  827. 		if (use_gps_z) {
  828. 			accel_bias_corr[2] -= corr_gps[2][0] * w_z_gps_p * w_z_gps_p;
  829. 			accel_bias_corr[2] -= corr_gps[2][1] * w_z_gps_v;
  830. 		}
  831.  
  832. 		/* transform error vector from NED frame to body frame */
  833. 		for (int i = 0; i < 3; i++) {
  834. 			float c = 0.0f;
  835.  
  836. 			for (int j = 0; j < 3; j++) {
  837. 				c += R_gps[j][i] * accel_bias_corr[j];
  838. 			}
  839.  
  840. 			if (PX4_ISFINITE(c)) {
  841. 				acc_bias[i] += c * params.w_acc_bias * dt;
  842. 			}
  843. 		}
  844.  
  845. 		/* accelerometer bias correction for VISION (use buffered rotation matrix) */
  846. 		accel_bias_corr[0] = 0.0f;
  847. 		accel_bias_corr[1] = 0.0f;
  848. 		accel_bias_corr[2] = 0.0f;
  849.  
  850. 		if (use_vision_xy) {
  851. 			accel_bias_corr[0] -= corr_vision[0][0] * w_xy_vision_p * w_xy_vision_p;
  852. 			accel_bias_corr[0] -= corr_vision[0][1] * w_xy_vision_v;
  853. 			accel_bias_corr[1] -= corr_vision[1][0] * w_xy_vision_p * w_xy_vision_p;
  854. 			accel_bias_corr[1] -= corr_vision[1][1] * w_xy_vision_v;
  855. 		}
  856.  
  857. 		if (use_vision_z) {
  858. 			accel_bias_corr[2] -= corr_vision[2][0] * w_z_vision_p * w_z_vision_p;
  859. 		}
  860.  
  861. 		/* accelerometer bias correction for MOCAP (use buffered rotation matrix) */
  862. 		accel_bias_corr[0] = 0.0f;
  863. 		accel_bias_corr[1] = 0.0f;
  864. 		accel_bias_corr[2] = 0.0f;
  865.  
  866. 		if (use_mocap) {
  867. 			accel_bias_corr[0] -= corr_mocap[0][0] * w_mocap_p * w_mocap_p;
  868. 			accel_bias_corr[1] -= corr_mocap[1][0] * w_mocap_p * w_mocap_p;
  869. 			accel_bias_corr[2] -= corr_mocap[2][0] * w_mocap_p * w_mocap_p;
  870. 		}
  871.  
  872. 		/* transform error vector from NED frame to body frame */
  873. 		for (int i = 0; i < 3; i++) {
  874. 			float c = 0.0f;
  875.  
  876. 			for (int j = 0; j < 3; j++) {
  877. 				c += PX4_R(att.R, j, i) * accel_bias_corr[j];
  878. 			}
  879.  
  880. 			if (PX4_ISFINITE(c)) {
  881. 				acc_bias[i] += c * params.w_acc_bias * dt;
  882. 			}
  883. 		}
  884.  
  885. 		/* accelerometer bias correction for flow and baro (assume that there is no delay) */
  886. 		accel_bias_corr[0] = 0.0f;
  887. 		accel_bias_corr[1] = 0.0f;
  888. 		accel_bias_corr[2] = 0.0f;
  889.  
  890. 		if (use_flow) {
  891. 			accel_bias_corr[0] -= corr_flow[0] * params.w_xy_flow;
  892. 			accel_bias_corr[1] -= corr_flow[1] * params.w_xy_flow;
  893. 		}
  894.  
  895. 		if (use_lidar) {
  896. 			accel_bias_corr[2] -= corr_lidar * params.w_z_lidar * params.w_z_lidar;
  897. 		} else {
  898. 			accel_bias_corr[2] -= corr_baro * params.w_z_baro * params.w_z_baro;
  899. 		}
  900.  
  901. 		/* transform error vector from NED frame to body frame */
  902. 		for (int i = 0; i < 3; i++) {
  903. 			float c = 0.0f;
  904.  
  905. 			for (int j = 0; j < 3; j++) {
  906. 				c += PX4_R(att.R, j, i) * accel_bias_corr[j];
  907. 			}
  908.  
  909. 			if (PX4_ISFINITE(c)) {
  910. 				acc_bias[i] += c * params.w_acc_bias * dt;
  911. 			}
  912. 		}
  913. //滤波处理///
  914. 		/* inertial filter prediction for altitude */
  915. 		inertial_filter_predict(dt, z_est, acc[2]);
  916.  
  917. 		if (!(PX4_ISFINITE(z_est[0]) && PX4_ISFINITE(z_est[1]))) {
  918. 			write_debug_log("BAD ESTIMATE AFTER Z PREDICTION", dt, x_est, y_est, z_est, x_est_prev, y_est_prev, z_est_prev,
  919. 					acc, corr_gps, w_xy_gps_p, w_xy_gps_v, corr_mocap, w_mocap_p,
  920. 					corr_vision, w_xy_vision_p, w_z_vision_p, w_xy_vision_v);
  921. 			memcpy(z_est, z_est_prev, sizeof(z_est));
  922. 		}
  923.  
  924. 		/* inertial filter correction for altitude */
  925. 		if (use_lidar) {
  926. 			inertial_filter_correct(corr_lidar, dt, z_est, 0, params.w_z_lidar);
  927.  
  928. 		} else {
  929. 			inertial_filter_correct(corr_baro, dt, z_est, 0, params.w_z_baro);
  930. 		}
  931.  
  932. 		if (use_gps_z) {
  933. 			epv = fminf(epv, gps.epv);
  934.  
  935. 			inertial_filter_correct(corr_gps[2][0], dt, z_est, 0, w_z_gps_p);
  936. 			inertial_filter_correct(corr_gps[2][1], dt, z_est, 1, w_z_gps_v);
  937. 		}
  938.  
  939. 		if (use_vision_z) {
  940. 			epv = fminf(epv, epv_vision);
  941. 			inertial_filter_correct(corr_vision[2][0], dt, z_est, 0, w_z_vision_p);
  942. 		}
  943.  
  944. 		if (use_mocap) {
  945. 			epv = fminf(epv, epv_mocap);
  946. 			inertial_filter_correct(corr_mocap[2][0], dt, z_est, 0, w_mocap_p);
  947. 		}
  948.  
  949. 		if (!(PX4_ISFINITE(z_est[0]) && PX4_ISFINITE(z_est[1]))) {
  950. 			write_debug_log("BAD ESTIMATE AFTER Z CORRECTION", dt, x_est, y_est, z_est, x_est_prev, y_est_prev, z_est_prev,
  951. 					acc, corr_gps, w_xy_gps_p, w_xy_gps_v, corr_mocap, w_mocap_p,
  952. 					corr_vision, w_xy_vision_p, w_z_vision_p, w_xy_vision_v);
  953. 			memcpy(z_est, z_est_prev, sizeof(z_est));
  954. 			memset(corr_gps, 0, sizeof(corr_gps));
  955. 			memset(corr_vision, 0, sizeof(corr_vision));
  956. 			memset(corr_mocap, 0, sizeof(corr_mocap));
  957. 			corr_baro = 0;
  958.  
  959. 		} else {
  960. 			memcpy(z_est_prev, z_est, sizeof(z_est));
  961. 		}
  962.  
  963. 		if (can_estimate_xy) {
  964. 			/* inertial filter prediction for position */
  965. 			inertial_filter_predict(dt, x_est, acc[0]);
  966. 			inertial_filter_predict(dt, y_est, acc[1]);
  967.  
  968. 			if (!(PX4_ISFINITE(x_est[0]) && PX4_ISFINITE(x_est[1]) && PX4_ISFINITE(y_est[0]) && PX4_ISFINITE(y_est[1]))) {
  969. 				write_debug_log("BAD ESTIMATE AFTER PREDICTION", dt, x_est, y_est, z_est, x_est_prev, y_est_prev, z_est_prev,
  970. 						acc, corr_gps, w_xy_gps_p, w_xy_gps_v, corr_mocap, w_mocap_p,
  971. 						corr_vision, w_xy_vision_p, w_z_vision_p, w_xy_vision_v);
  972. 				memcpy(x_est, x_est_prev, sizeof(x_est));
  973. 				memcpy(y_est, y_est_prev, sizeof(y_est));
  974. 			}
  975.  
  976. 			/* inertial filter correction for position */
  977. 			if (use_flow) {
  978. 				eph = fminf(eph, eph_flow);
  979.  
  980. 				inertial_filter_correct(corr_flow[0], dt, x_est, 1, params.w_xy_flow * w_flow);
  981. 				inertial_filter_correct(corr_flow[1], dt, y_est, 1, params.w_xy_flow * w_flow);
  982. 			}
  983.  
  984. 			if (use_gps_xy) {
  985. 				eph = fminf(eph, gps.eph);
  986.  
  987. 				inertial_filter_correct(corr_gps[0][0], dt, x_est, 0, w_xy_gps_p);
  988. 				inertial_filter_correct(corr_gps[1][0], dt, y_est, 0, w_xy_gps_p);
  989.  
  990. 				if (gps.vel_ned_valid && t < gps.timestamp_velocity + gps_topic_timeout) {
  991. 					inertial_filter_correct(corr_gps[0][1], dt, x_est, 1, w_xy_gps_v);
  992. 					inertial_filter_correct(corr_gps[1][1], dt, y_est, 1, w_xy_gps_v);
  993. 				}
  994. 			}
  995.  
  996. 			if (use_vision_xy) {
  997. 				eph = fminf(eph, eph_vision);
  998.  
  999. 				inertial_filter_correct(corr_vision[0][0], dt, x_est, 0, w_xy_vision_p);
  1000. 				inertial_filter_correct(corr_vision[1][0], dt, y_est, 0, w_xy_vision_p);
  1001.  
  1002. 				if (w_xy_vision_v > MIN_VALID_W) {
  1003. 					inertial_filter_correct(corr_vision[0][1], dt, x_est, 1, w_xy_vision_v);
  1004. 					inertial_filter_correct(corr_vision[1][1], dt, y_est, 1, w_xy_vision_v);
  1005. 				}
  1006. 			}
  1007.  
  1008. 			if (use_mocap) {
  1009. 				eph = fminf(eph, eph_mocap);
  1010.  
  1011. 				inertial_filter_correct(corr_mocap[0][0], dt, x_est, 0, w_mocap_p);
  1012. 				inertial_filter_correct(corr_mocap[1][0], dt, y_est, 0, w_mocap_p);
  1013. 			}
  1014.  
  1015. 			if (!(PX4_ISFINITE(x_est[0]) && PX4_ISFINITE(x_est[1]) && PX4_ISFINITE(y_est[0]) && PX4_ISFINITE(y_est[1]))) {
  1016. 				write_debug_log("BAD ESTIMATE AFTER CORRECTION", dt, x_est, y_est, z_est, x_est_prev, y_est_prev, z_est_prev,
  1017. 						acc, corr_gps, w_xy_gps_p, w_xy_gps_v, corr_mocap, w_mocap_p,
  1018. 						corr_vision, w_xy_vision_p, w_z_vision_p, w_xy_vision_v);
  1019. 				memcpy(x_est, x_est_prev, sizeof(x_est));
  1020. 				memcpy(y_est, y_est_prev, sizeof(y_est));
  1021. 				memset(corr_gps, 0, sizeof(corr_gps));
  1022. 				memset(corr_vision, 0, sizeof(corr_vision));
  1023. 				memset(corr_mocap, 0, sizeof(corr_mocap));
  1024. 				memset(corr_flow, 0, sizeof(corr_flow));
  1025.  
  1026. 			} else {
  1027. 				memcpy(x_est_prev, x_est, sizeof(x_est));
  1028. 				memcpy(y_est_prev, y_est, sizeof(y_est));
  1029. 			}
  1030.  
  1031. 		} else {
  1032. 			/* gradually reset xy velocity estimates */
  1033. 			inertial_filter_correct(-x_est[1], dt, x_est, 1, params.w_xy_res_v);
  1034. 			inertial_filter_correct(-y_est[1], dt, y_est, 1, params.w_xy_res_v);
  1035. 		}
  1036. ///运行地形估计//
  1037. 		/* run terrain estimator */
  1038. 		terrain_estimator->predict(dt, &att, &sensor, &lidar);
  1039. 		terrain_estimator->measurement_update(hrt_absolute_time(), &gps, &lidar, &att);
  1040.  
  1041. 		if (inav_verbose_mode) {
  1042. 			/* print updates rate */
  1043. 			if (t > updates_counter_start + updates_counter_len) {
  1044. 				float updates_dt = (t - updates_counter_start) * 0.000001f;
  1045. 				warnx(
  1046. 					"updates rate: accelerometer = %.1f/s, baro = %.1f/s, gps = %.1f/s, attitude = %.1f/s, flow = %.1f/s, vision = %.1f/s, mocap = %.1f/s",
  1047. 					(double)(accel_updates / updates_dt),
  1048. 					(double)(baro_updates / updates_dt),
  1049. 					(double)(gps_updates / updates_dt),
  1050. 					(double)(attitude_updates / updates_dt),
  1051. 					(double)(flow_updates / updates_dt),
  1052. 					(double)(vision_updates / updates_dt),
  1053. 					(double)(mocap_updates / updates_dt));
  1054. 				updates_counter_start = t;
  1055. 				accel_updates = 0;
  1056. 				baro_updates = 0;
  1057. 				gps_updates = 0;
  1058. 				attitude_updates = 0;
  1059. 				flow_updates = 0;
  1060. 				vision_updates = 0;
  1061. 				mocap_updates = 0;
  1062. 			}
  1063. 		}
  1064.  
  1065. 		if (t > pub_last + PUB_INTERVAL) {
  1066. 			pub_last = t;
  1067.  
  1068. 			/* push current estimate to buffer */
  1069. 			est_buf[buf_ptr][0][0] = x_est[0];
  1070. 			est_buf[buf_ptr][0][1] = x_est[1];
  1071. 			est_buf[buf_ptr][1][0] = y_est[0];
  1072. 			est_buf[buf_ptr][1][1] = y_est[1];
  1073. 			est_buf[buf_ptr][2][0] = z_est[0];
  1074. 			est_buf[buf_ptr][2][1] = z_est[1];
  1075.  
  1076. 			/* push current rotation matrix to buffer */
  1077. 			memcpy(R_buf[buf_ptr], att.R, sizeof(att.R));
  1078.  
  1079. 			buf_ptr++;
  1080.  
  1081. 			if (buf_ptr >= EST_BUF_SIZE) {
  1082. 				buf_ptr = 0;
  1083. 			}
  1084.  
  1085. ///发布位置信息///
  1086. 			/* publish local position */
  1087. 			local_pos.xy_valid = can_estimate_xy;
  1088. 			local_pos.v_xy_valid = can_estimate_xy;
  1089. 			local_pos.xy_global = local_pos.xy_valid && use_gps_xy;
  1090. 			local_pos.z_global = local_pos.z_valid && use_gps_z;
  1091. 			local_pos.x = x_est[0];
  1092. 			local_pos.vx = x_est[1];
  1093. 			local_pos.y = y_est[0];
  1094. 			local_pos.vy = y_est[1];
  1095. 			local_pos.z = z_est[0];
  1096. 			local_pos.vz = z_est[1];
  1097. 			local_pos.yaw = att.yaw;
  1098. 			local_pos.dist_bottom_valid = dist_bottom_valid;
  1099. 			local_pos.eph = eph;
  1100. 			local_pos.epv = epv;
  1101.  
  1102. 			if (local_pos.dist_bottom_valid) {
  1103. 				local_pos.dist_bottom = dist_ground;
  1104. 				local_pos.dist_bottom_rate = - z_est[1];
  1105. 			}
  1106.  
  1107. 			local_pos.timestamp = t;
  1108.  
  1109. 			orb_publish(ORB_ID(vehicle_local_position), vehicle_local_position_pub, &local_pos);
  1110.  
  1111. 			if (local_pos.xy_global && local_pos.z_global) {
  1112. 				/* publish global position */
  1113. 				global_pos.timestamp = t;
  1114. 				global_pos.time_utc_usec = gps.time_utc_usec;
  1115.  
  1116. 				double est_lat, est_lon;
  1117. 				map_projection_reproject(&ref, local_pos.x, local_pos.y, &est_lat, &est_lon);
  1118.  
  1119. 				global_pos.lat = est_lat;
  1120. 				global_pos.lon = est_lon;
  1121. 				global_pos.alt = local_pos.ref_alt - local_pos.z;
  1122.  
  1123. 				global_pos.vel_n = local_pos.vx;
  1124. 				global_pos.vel_e = local_pos.vy;
  1125. 				global_pos.vel_d = local_pos.vz;
  1126.  
  1127. 				global_pos.yaw = local_pos.yaw;
  1128.  
  1129. 				global_pos.eph = eph;
  1130. 				global_pos.epv = epv;
  1131.  
  1132. 				if (terrain_estimator->is_valid()) {
  1133. 					global_pos.terrain_alt = global_pos.alt - terrain_estimator->get_distance_to_ground();
  1134. 					global_pos.terrain_alt_valid = true;
  1135.  
  1136. 				} else {
  1137. 					global_pos.terrain_alt_valid = false;
  1138. 				}
  1139.  
  1140. 				global_pos.pressure_alt = sensor.baro_alt_meter[0];
  1141.  
  1142. 				if (vehicle_global_position_pub == NULL) {
  1143. 					vehicle_global_position_pub = orb_advertise(ORB_ID(vehicle_global_position), &global_pos);
  1144.  
  1145. 				} else {
  1146. 					orb_publish(ORB_ID(vehicle_global_position), vehicle_global_position_pub, &global_pos);
  1147. 				}
  1148. 			}
  1149. 		}
  1150. 	}
  1151. /***************************************大循环结束********************************/
  1152. 	warnx("stopped");
  1153. 	mavlink_log_info(&mavlink_log_pub, "[inav] stopped");
  1154. 	thread_running = false;
  1155. 	return 0;
  1156. }

这里发布的信息有2个vehicle_local_position和vehicle_global_position

  1. #ifdef __cplusplus
  2. struct __EXPORT vehicle_local_position_s {
  3. #else
  4. struct vehicle_local_position_s {
  5. #endif
  6. 	uint64_t timestamp;
  7. 	bool xy_valid;
  8. 	bool z_valid;
  9. 	bool v_xy_valid;
  10. 	bool v_z_valid;
  11. 	float x;
  12. 	float y;
  13. 	float z;
  14. 	float vx;
  15. 	float vy;
  16. 	float vz;
  17. 	float yaw;
  18. 	bool xy_global;
  19. 	bool z_global;
  20. 	uint64_t ref_timestamp;
  21. 	double ref_lat;
  22. 	double ref_lon;
  23. 	float ref_alt;
  24. 	float dist_bottom;
  25. 	float dist_bottom_rate;
  26. 	uint64_t surface_bottom_timestamp;
  27. 	bool dist_bottom_valid;
  28. 	float eph;
  29. 	float epv;
  30. #ifdef __cplusplus
  31. #endif
  32. };
  1. #ifdef __cplusplus
  2. struct __EXPORT vehicle_global_position_s {
  3. #else
  4. struct vehicle_global_position_s {
  5. #endif
  6. 	uint64_t timestamp;
  7. 	uint64_t time_utc_usec;
  8. 	double lat;
  9. 	double lon;
  10. 	float alt;
  11. 	float vel_n;
  12. 	float vel_e;
  13. 	float vel_d;
  14. 	float yaw;
  15. 	float eph;
  16. 	float epv;
  17. 	float terrain_alt;
  18. 	bool terrain_alt_valid;
  19. 	bool dead_reckoning;
  20. 	float pressure_alt;
  21. #ifdef __cplusplus
  22. #endif
  23. };

其他会用到的数据全部在ardupilot/modules/PX4Firmware/src/modules/uORB/topics文件夹中

  1. #ifdef __cplusplus
  2. struct __EXPORT sensor_combined_s {
  3. #else
  4. struct sensor_combined_s {
  5. #endif
  6. 	uint64_t timestamp;
  7. 	uint64_t gyro_timestamp[3];
  8. 	int16_t gyro_raw[9];
  9. 	float gyro_rad_s[9];
  10. 	uint32_t gyro_priority[3];
  11. 	float gyro_integral_rad[9];
  12. 	uint64_t gyro_integral_dt[3];
  13. 	uint32_t gyro_errcount[3];
  14. 	float gyro_temp[3];
  15. 	int16_t accelerometer_raw[9];
  16. 	float accelerometer_m_s2[9];
  17. 	float accelerometer_integral_m_s[9];
  18. 	uint64_t accelerometer_integral_dt[3];
  19. 	int16_t accelerometer_mode[3];
  20. 	float accelerometer_range_m_s2[3];
  21. 	uint64_t accelerometer_timestamp[3];
  22. 	uint32_t accelerometer_priority[3];
  23. 	uint32_t accelerometer_errcount[3];
  24. 	float accelerometer_temp[3];
  25. 	int16_t magnetometer_raw[9];
  26. 	float magnetometer_ga[9];
  27. 	int16_t magnetometer_mode[3];
  28. 	float magnetometer_range_ga[3];
  29. 	float magnetometer_cuttoff_freq_hz[3];
  30. 	uint64_t magnetometer_timestamp[3];
  31. 	uint32_t magnetometer_priority[3];
  32. 	uint32_t magnetometer_errcount[3];
  33. 	float magnetometer_temp[3];
  34. 	float baro_pres_mbar[3];
  35. 	float baro_alt_meter[3];
  36. 	float baro_temp_celcius[3];
  37. 	uint64_t baro_timestamp[3];
  38. 	uint32_t baro_priority[3];
  39. 	uint32_t baro_errcount[3];
  40. 	float adc_voltage_v[10];
  41. 	uint16_t adc_mapping[10];
  42. 	float mcu_temp_celcius;
  43. 	float differential_pressure_pa[3];
  44. 	uint64_t differential_pressure_timestamp[3];
  45. 	float differential_pressure_filtered_pa[3];
  46. 	uint32_t differential_pressure_priority[3];
  47. 	uint32_t differential_pressure_errcount[3];
  48. #ifdef __cplusplus
  49. 	static const int32_t MAGNETOMETER_MODE_NORMAL = 0;
  50. 	static const int32_t MAGNETOMETER_MODE_POSITIVE_BIAS = 1;
  51. 	static const int32_t MAGNETOMETER_MODE_NEGATIVE_BIAS = 2;
  52. 	static const uint32_t SENSOR_PRIO_MIN = 0;
  53. 	static const uint32_t SENSOR_PRIO_VERY_LOW = 25;
  54. 	static const uint32_t SENSOR_PRIO_LOW = 50;
  55. 	static const uint32_t SENSOR_PRIO_DEFAULT = 75;
  56. 	static const uint32_t SENSOR_PRIO_HIGH = 100;
  57. 	static const uint32_t SENSOR_PRIO_VERY_HIGH = 125;
  58. 	static const uint32_t SENSOR_PRIO_MAX = 255;
  59. #endif
  60. };
  61. #ifdef __cplusplus
  62. struct __EXPORT parameter_update_s {
  63. #else
  64. struct parameter_update_s {
  65. #endif
  66. 	uint64_t timestamp;
  67. 	bool saved;
  68. #ifdef __cplusplus
  69. #endif
  70. };
  71. #ifdef __cplusplus
  72. struct __EXPORT actuator_controls_s {
  73. #else
  74. struct actuator_controls_s {
  75. #endif
  76. 	uint64_t timestamp;
  77. 	uint64_t timestamp_sample;
  78. 	float control[8];
  79. #ifdef __cplusplus
  80. 	static const uint8_t NUM_ACTUATOR_CONTROLS = 8;
  81. 	static const uint8_t NUM_ACTUATOR_CONTROL_GROUPS = 4;
  82. 	static const uint8_t INDEX_ROLL = 0;
  83. 	static const uint8_t INDEX_PITCH = 1;
  84. 	static const uint8_t INDEX_YAW = 2;
  85. 	static const uint8_t INDEX_THROTTLE = 3;
  86. 	static const uint8_t INDEX_FLAPS = 4;
  87. 	static const uint8_t INDEX_SPOILERS = 5;
  88. 	static const uint8_t INDEX_AIRBRAKES = 6;
  89. 	static const uint8_t INDEX_LANDING_GEAR = 7;
  90. 	static const uint8_t GROUP_INDEX_ATTITUDE = 0;
  91. 	static const uint8_t GROUP_INDEX_ATTITUDE_ALTERNATE = 1;
  92. #endif
  93. };
  94. #ifdef __cplusplus
  95. struct __EXPORT actuator_armed_s {
  96. #else
  97. struct actuator_armed_s {
  98. #endif
  99. 	uint64_t timestamp;
  100. 	bool armed;
  101. 	bool prearmed;
  102. 	bool ready_to_arm;
  103. 	bool lockdown;
  104. 	bool force_failsafe;
  105. 	bool in_esc_calibration_mode;
  106. #ifdef __cplusplus
  107. #endif
  108. #ifdef __cplusplus
  109. struct __EXPORT vision_position_estimate_s {
  110. #else
  111. struct vision_position_estimate_s {
  112. #endif
  113. 	uint32_t id;
  114. 	uint64_t timestamp_boot;
  115. 	uint64_t timestamp_computer;
  116. 	float x;
  117. 	float y;
  118. 	float z;
  119. 	float vx;
  120. 	float vy;
  121. 	float vz;
  122. 	float q[4];
  123. #ifdef __cplusplus
  124. #endif
  125. };
  126.  
  127. #ifdef __cplusplus
  128. struct __EXPORT att_pos_mocap_s {
  129. #else
  130. struct att_pos_mocap_s {
  131. #endif
  132. 	uint32_t id;
  133. 	uint64_t timestamp_boot;
  134. 	uint64_t timestamp_computer;
  135. 	float q[4];
  136. 	float x;
  137. 	float y;
  138. 	float z;
  139. #ifdef __cplusplus
  140. #endif
  141. };
  142.  
  143. #ifdef __cplusplus
  144. struct __EXPORT vehicle_gps_position_s {
  145. #else
  146. struct vehicle_gps_position_s {
  147. #endif
  148. 	uint64_t timestamp_position;
  149. 	int32_t lat;
  150. 	int32_t lon;
  151. 	int32_t alt;
  152. 	int32_t alt_ellipsoid;
  153. 	uint64_t timestamp_variance;
  154. 	float s_variance_m_s;
  155. 	float c_variance_rad;
  156. 	uint8_t fix_type;
  157. 	float eph;
  158. 	float epv;
  159. 	float hdop;
  160. 	float vdop;
  161. 	int32_t noise_per_ms;
  162. 	int32_t jamming_indicator;
  163. 	uint64_t timestamp_velocity;
  164. 	float vel_m_s;
  165. 	float vel_n_m_s;
  166. 	float vel_e_m_s;
  167. 	float vel_d_m_s;
  168. 	float cog_rad;
  169. 	bool vel_ned_valid;
  170. 	uint64_t timestamp_time;
  171. 	uint64_t time_utc_usec;
  172. 	uint8_t satellites_used;
  173. #ifdef __cplusplus
  174.  
  175. #endif
  176. };

把光流部分单独拿出来

  1. /**************************************这里就是光流的处理*********************************/
  2. 			/* optical flow */
  3. 			orb_check(optical_flow_sub, &updated);
  4.  
  5. 			if (updated && lidar_valid) {
  6. 				orb_copy(ORB_ID(optical_flow), optical_flow_sub, &flow);//光流数据跟新
  7.  
  8. 				flow_time = t;
  9. 				float flow_q = flow.quality / 255.0f;
  10. 				float dist_bottom = lidar.current_distance;//高度信息
  11.  
  12. 				if (dist_bottom > flow_min_dist && flow_q > params.flow_q_min && PX4_R(att.R, 2, 2) > 0.7f) {
  13. 					/* distance to surface */
  14. 					//float flow_dist = dist_bottom / PX4_R(att.R, 2, 2); //use this if using sonar
  15. 					float flow_dist = dist_bottom; //use this if using lidar
  16.  
  17. 					/* check if flow if too large for accurate measurements */检查速度是否太大，影响光流精确测量
  18. /*****************************计算机架估计的速度*****************************/
  19. 					/* calculate estimated velocity in body frame */
  20. 					float body_v_est[2] = { 0.0f, 0.0f };
  21.  
  22. 					for (int i = 0; i < 2; i++) {
  23. 						body_v_est[i] = PX4_R(att.R, 0, i) * x_est[1] + PX4_R(att.R, 1, i) * y_est[1] + PX4_R(att.R, 2, i) * z_est[1];
  24. 					}
  25. /*************设置这个标志，如果光流按照当前速度和姿态率的估计是准确的*************/
  26. 					/* set this flag if flow should be accurate according to current velocity and attitude rate estimate */
  27. 					flow_accurate = fabsf(body_v_est[1] / flow_dist - att.rollspeed) < max_flow &&
  28. 							fabsf(body_v_est[0] / flow_dist + att.pitchspeed) < max_flow;
  29. /*************利用自动驾驶仪(飞控)已经校准的陀螺仪计算光流用的陀螺仪的偏移*************/
  30. 					/*calculate offset of flow-gyro using already calibrated gyro from autopilot*/
  31. 					flow_gyrospeed[0] = flow.gyro_x_rate_integral / (float)flow.integration_timespan * 1000000.0f;
  32. 					flow_gyrospeed[1] = flow.gyro_y_rate_integral / (float)flow.integration_timespan * 1000000.0f;
  33. 					flow_gyrospeed[2] = flow.gyro_z_rate_integral / (float)flow.integration_timespan * 1000000.0f;
  34.  
  35. 					//moving average
  36. 					if (n_flow >= 100) {
  37. 						gyro_offset_filtered[0] = flow_gyrospeed_filtered[0] - att_gyrospeed_filtered[0];
  38. 						gyro_offset_filtered[1] = flow_gyrospeed_filtered[1] - att_gyrospeed_filtered[1];
  39. 						gyro_offset_filtered[2] = flow_gyrospeed_filtered[2] - att_gyrospeed_filtered[2];
  40. 						n_flow = 0;
  41. 						flow_gyrospeed_filtered[0] = 0.0f;
  42. 						flow_gyrospeed_filtered[1] = 0.0f;
  43. 						flow_gyrospeed_filtered[2] = 0.0f;
  44. 						att_gyrospeed_filtered[0] = 0.0f;
  45. 						att_gyrospeed_filtered[1] = 0.0f;
  46. 						att_gyrospeed_filtered[2] = 0.0f;
  47.  
  48. 					} else {
  49. 						flow_gyrospeed_filtered[0] = (flow_gyrospeed[0] + n_flow * flow_gyrospeed_filtered[0]) / (n_flow + 1);
  50. 						flow_gyrospeed_filtered[1] = (flow_gyrospeed[1] + n_flow * flow_gyrospeed_filtered[1]) / (n_flow + 1);
  51. 						flow_gyrospeed_filtered[2] = (flow_gyrospeed[2] + n_flow * flow_gyrospeed_filtered[2]) / (n_flow + 1);
  52. 						att_gyrospeed_filtered[0] = (att.pitchspeed + n_flow * att_gyrospeed_filtered[0]) / (n_flow + 1);
  53. 						att_gyrospeed_filtered[1] = (att.rollspeed + n_flow * att_gyrospeed_filtered[1]) / (n_flow + 1);
  54. 						att_gyrospeed_filtered[2] = (att.yawspeed + n_flow * att_gyrospeed_filtered[2]) / (n_flow + 1);
  55. 						n_flow++;
  56. 					}
  57.  
  58. /*******************************偏航补偿（光流传感器不处于旋转中心）->参数在QGC中***********************/
  59. 					/*yaw compensation (flow sensor is not in center of rotation) -> params in QGC*/
  60. 					yaw_comp[0] = - params.flow_module_offset_y * (flow_gyrospeed[2] - gyro_offset_filtered[2]);
  61. 					yaw_comp[1] = params.flow_module_offset_x * (flow_gyrospeed[2] - gyro_offset_filtered[2]);
  62. /***************************将原始流转换成角度流**********************************/
  63. 					/* convert raw flow to angular flow (rad/s) */
  64. 					float flow_ang[2];
  65. /***************************检查机体速度设定值->它低于阈值->不减去->更好悬停*********************/
  66. 					/* check for vehicle rates setpoint - it below threshold -> dont subtract -> better hover */
  67. 					orb_check(vehicle_rate_sp_sub, &updated);
  68. 					if (updated)
  69. 						orb_copy(ORB_ID(vehicle_rates_setpoint), vehicle_rate_sp_sub, &rates_setpoint);
  70.  
  71. 					double rate_threshold = 0.15f;
  72.  
  73. 					if (fabs(rates_setpoint.pitch) < rate_threshold) {
  74. 						//warnx("[inav] test ohne comp");
  75. 						flow_ang[0] = (flow.pixel_flow_x_integral / (float)flow.integration_timespan * 1000000.0f) * params.flow_k;//for now the flow has to be scaled (to small)
  76. 					}
  77. 					else {
  78. 						//warnx("[inav] test mit comp");
  79. 						//calculate flow [rad/s] and compensate for rotations (and offset of flow-gyro)
  80. 						//计算光流的速度[rad/s]并补偿旋转（和光流的陀螺仪的偏置）
  81. 						flow_ang[0] = ((flow.pixel_flow_x_integral - flow.gyro_x_rate_integral) / (float)flow.integration_timespan * 1000000.0f
  82. 							       + gyro_offset_filtered[0]) * params.flow_k;//for now the flow has to be scaled (to small)
  83. 					}
  84.  
  85. 					if (fabs(rates_setpoint.roll) < rate_threshold) {
  86. 						flow_ang[1] = (flow.pixel_flow_y_integral / (float)flow.integration_timespan * 1000000.0f) * params.flow_k;//for now the flow has to be scaled (to small)
  87. 					}
  88. 					else {
  89. 						//calculate flow [rad/s] and compensate for rotations (and offset of flow-gyro)
  90. 						//计算光流的速度[rad/s]并补偿旋转（和光流的陀螺仪的偏置）
  91. 						flow_ang[1] = ((flow.pixel_flow_y_integral - flow.gyro_y_rate_integral) / (float)flow.integration_timespan * 1000000.0f
  92. 							       + gyro_offset_filtered[1]) * params.flow_k;//for now the flow has to be scaled (to small)
  93. 					}
  94. /************************光流测量向量********************************/
  95. 					/* flow measurements vector */
  96. 					float flow_m[3];
  97. 					if (fabs(rates_setpoint.yaw) < rate_threshold) {
  98. 						flow_m[0] = -flow_ang[0] * flow_dist;
  99. 						flow_m[1] = -flow_ang[1] * flow_dist;
  100. 					} else {
  101. 						flow_m[0] = -flow_ang[0] * flow_dist - yaw_comp[0] * params.flow_k;
  102. 						flow_m[1] = -flow_ang[1] * flow_dist - yaw_comp[1] * params.flow_k;
  103. 					}
  104. 					flow_m[2] = z_est[1];
  105.  
  106. 					/* velocity in NED */NED是北东地，还有一种旋转方式是东北天，都是属于右手系
  107. 					float flow_v[2] = { 0.0f, 0.0f };
  108. /************************基于NED工程测量向量，跳过Z分量*********************/
  109. 					/* project measurements vector to NED basis, skip Z component */
  110. 					for (int i = 0; i < 2; i++) {
  111. 						for (int j = 0; j < 3; j++) {
  112. 							flow_v[i] += PX4_R(att.R, i, j) * flow_m[j];
  113. 						}
  114. 					}
  115. /********************向量矫正***********************/
  116. 					/* velocity correction */
  117. 					corr_flow[0] = flow_v[0] - x_est[1];
  118. 					corr_flow[1] = flow_v[1] - y_est[1];
  119. /********************调整矫正权重***********************/					
  120. 					/* adjust correction weight */
  121. 					float flow_q_weight = (flow_q - params.flow_q_min) / (1.0f - params.flow_q_min);
  122. 					w_flow = PX4_R(att.R, 2, 2) * flow_q_weight / fmaxf(1.0f, flow_dist);
  123.  
  124. /********************如果光流是不准确的，那么减少他的权重***********************/
  125. 					/* if flow is not accurate, reduce weight for it */
  126. 					// TODO make this more fuzzy
  127. 					if (!flow_accurate) {
  128. 						w_flow *= 0.05f;
  129. 					}
  130. /**********************在理想条件下，在1m的距离EPH =10厘米****************/
  131. 					/* under ideal conditions, on 1m distance assume EPH = 10cm */
  132. 					eph_flow = 0.1f / w_flow;
  133.  
  134. 					flow_valid = true;
  135.  
  136. 				} else {
  137. 					w_flow = 0.0f;
  138. 					flow_valid = false;
  139. 				}
  140.  
  141. 				flow_updates++;
  142. 			}
  143. /**************************************光流结束*********************************/
接下来就到了 ardupilot/modules/PX4Firmware/src/modules/mc_pos_control/mc_pos_control_main.cpp

和ardupilot/modules/PX4Firmware/src/modules/vtol_att_control_main.cpp

进行控制


先看mc_pos_control_main.cpp

extern "C" __EXPORT int mc_pos_control_main(int argc, char *argv[]);
  1. int mc_pos_control_main(int argc, char *argv[])
  2. {
  3. 	if (argc < 2) {
  4. 		warnx("usage: mc_pos_control {start|stop|status}");
  5. 		return 1;
  6. 	}
  7.  
  8. 	if (!strcmp(argv[1], "start")) {
  9.  
  10. 		if (pos_control::g_control != nullptr) {
  11. 			warnx("already running");
  12. 			return 1;
  13. 		}
  14.  
  15. 		pos_control::g_control = new MulticopterPositionControl;
  16.  
  17. 		if (pos_control::g_control == nullptr) {
  18. 			warnx("alloc failed");
  19. 			return 1;
  20. 		}
  21.  
  22. 		if (OK != pos_control::g_control->start()) {
  23. 			delete pos_control::g_control;
  24. 			pos_control::g_control = nullptr;
  25. 			warnx("start failed");
  26. 			return 1;
  27. 		}
  28.  
  29. 		return 0;
  30. 	}
  31.  
  32. 	if (!strcmp(argv[1], "stop")) {
  33. 		if (pos_control::g_control == nullptr) {
  34. 			warnx("not running");
  35. 			return 1;
  36. 		}
  37.  
  38. 		delete pos_control::g_control;
  39. 		pos_control::g_control = nullptr;
  40. 		return 0;
  41. 	}
  42.  
  43. 	if (!strcmp(argv[1], "status")) {
  44. 		if (pos_control::g_control) {
  45. 			warnx("running");
  46. 			return 0;
  47.  
  48. 		} else {
  49. 			warnx("not running");
  50. 			return 1;
  51. 		}
  52. 	}
  53.  
  54. 	warnx("unrecognized command");
  55. 	return 1;
  56. }
这个里面就start和status需要看 

  1. int
  2. MulticopterPositionControl::start()
  3. {
  4. 	ASSERT(_control_task == -1);
  5.  
  6. 	/* start the task */
  7. 	_control_task = px4_task_spawn_cmd("mc_pos_control",
  8. 					   SCHED_DEFAULT,
  9. 					   SCHED_PRIORITY_MAX - 5,
  10. 					   1900,
  11. 					   (px4_main_t)&MulticopterPositionControl::task_main_trampoline,
  12. 					   nullptr);
  13.  
  14. 	if (_control_task < 0) {
  15. 		warn("task start failed");
  16. 		return -errno;
  17. 	}
  18.  
  19. 	return OK;
  20. }
  1. void
  2. MulticopterPositionControl::task_main_trampoline(int argc, char *argv[])
  3. {
  4. 	pos_control::g_control->task_main();
  5. }
  1. void
  2. MulticopterPositionControl::task_main()
  3. {
  4. /***************************订阅各种信息***********************/
  5. 	/*
  6. 	 * do subscriptions
  7. 	 */
  8. 	_vehicle_status_sub = orb_subscribe(ORB_ID(vehicle_status));
  9. 	_ctrl_state_sub = orb_subscribe(ORB_ID(control_state));
  10. 	_att_sp_sub = orb_subscribe(ORB_ID(vehicle_attitude_setpoint));
  11. 	_control_mode_sub = orb_subscribe(ORB_ID(vehicle_control_mode));
  12. 	_params_sub = orb_subscribe(ORB_ID(parameter_update));
  13. 	_manual_sub = orb_subscribe(ORB_ID(manual_control_setpoint));
  14. 	_arming_sub = orb_subscribe(ORB_ID(actuator_armed));
  15. 	_local_pos_sub = orb_subscribe(ORB_ID(vehicle_local_position));
  16. 	//这个就是从position_estimator_inav_main.cpp中来的，里面包含光流数据
  17. 	_pos_sp_triplet_sub = orb_subscribe(ORB_ID(position_setpoint_triplet));
  18. 	_local_pos_sp_sub = orb_subscribe(ORB_ID(vehicle_local_position_setpoint));
  19. 	_global_vel_sp_sub = orb_subscribe(ORB_ID(vehicle_global_velocity_setpoint));
  20.  
  21.  
  22. 	parameters_update(true);//参数跟新
  23. /*****************初始化关键结构的值，直到第一次定期更新*****************/
  24. 	/* initialize values of critical structs until first regular update */
  25. 	_arming.armed = false;
  26. /*****************初始跟新，获取所有传感器的数据和状态数据*****************/
  27. 	/* get an initial update for all sensor and status data */
  28. 	poll_subscriptions();//具体在下面展开了
  29.  
  30. 	bool reset_int_z = true;
  31. 	bool reset_int_z_manual = false;
  32. 	bool reset_int_xy = true;
  33. 	bool reset_yaw_sp = true;
  34. 	bool was_armed = false;
  35.  
  36. 	hrt_abstime t_prev = 0;
  37.  
  38. 	math::Vector<3> thrust_int;
  39. 	thrust_int.zero();
  40.  
  41.  
  42. 	math::Matrix<3, 3> R;
  43. 	R.identity();
  44.  
  45. 	/* wakeup source */
  46. 	px4_pollfd_struct_t fds[1];
  47.  
  48. 	fds[0].fd = _local_pos_sub;
  49. 	fds[0].events = POLLIN;
  50. /***********************大循环***************************/
  51. 	while (!_task_should_exit) {
  52. 		/* wait for up to 500ms for data */等待500ms来获取数据
  53. 		int pret = px4_poll(&fds[0], (sizeof(fds) / sizeof(fds[0])), 500);
  54.  
  55. 		/* timed out - periodic check for _task_should_exit */
  56. 		if (pret == 0) {
  57. 			continue;
  58. 		}
  59.  
  60. 		/* this is undesirable but not much we can do */
  61. 		if (pret < 0) {
  62. 			warn("poll error %d, %d", pret, errno);
  63. 			continue;
  64. 		}
  65. //获取所有传感器的数据和状态数据//
  66. 		poll_subscriptions();
  67. ///参数跟新
  68. 		parameters_update(false);
  69.  
  70. 		hrt_abstime t = hrt_absolute_time();
  71. 		float dt = t_prev != 0 ? (t - t_prev) * 0.000001f : 0.0f;//dt是控制周期吗？
  72. 		t_prev = t;
  73.  
  74. 		// set dt for control blocks
  75. 		setDt(dt);
  76. /判断是否锁定
  77. 		if (_control_mode.flag_armed && !was_armed) {
  78. 			/* reset setpoints and integrals on arming */
  79. 			_reset_pos_sp = true;
  80. 			_reset_alt_sp = true;
  81. 			_vel_sp_prev.zero();
  82. 			reset_int_z = true;
  83. 			reset_int_xy = true;
  84. 			reset_yaw_sp = true;
  85. 		}
  86. //复位yaw和姿态的设定    为垂直起降     fw模式/
  87. 		/* reset yaw and altitude setpoint for VTOL which are in fw mode */
  88. 		if (_vehicle_status.is_vtol) {
  89. 			if (!_vehicle_status.is_rotary_wing) {
  90. 				reset_yaw_sp = true;
  91. 				_reset_alt_sp = true;
  92. 			}
  93. 		}
  94. 跟新前一时刻锁定的状态///
  95. 		//Update previous arming state
  96. 		was_armed = _control_mode.flag_armed;
  97.  
  98. 		update_ref();//跟新参考点，是不是期望目标？该函数在下面会展开
  99. 跟新速度导数(加速度)  独立于当前飞行模式///
  100. 		/* Update velocity derivative,
  101. 		 * independent of the current flight mode
  102. 		 */
  103. 		if (_local_pos.timestamp > 0) {
  104.  
  105. 			if (PX4_ISFINITE(_local_pos.x) && //ISFINITE是测试数据是否是个有限数的函数
  106. 					PX4_ISFINITE(_local_pos.y) &&
  107. 					PX4_ISFINITE(_local_pos.z)) {
  108.  
  109. 				_pos(0) = _local_pos.x;
  110. 				_pos(1) = _local_pos.y;
  111. 				_pos(2) = _local_pos.z;
  112. 			}
  113.  
  114. 			if (PX4_ISFINITE(_local_pos.vx) &&
  115. 					PX4_ISFINITE(_local_pos.vy) &&
  116. 					PX4_ISFINITE(_local_pos.vz)) {
  117.  
  118. 				_vel(0) = _local_pos.vx;
  119. 				_vel(1) = _local_pos.vy;
  120. 				_vel(2) = _local_pos.vz;
  121. 			}
  122.  
  123. 			_vel_err_d(0) = _vel_x_deriv.update(-_vel(0));
  124. 			_vel_err_d(1) = _vel_y_deriv.update(-_vel(1));
  125. 			_vel_err_d(2) = _vel_z_deriv.update(-_vel(2));
  126. 		}
  127. ///复位在非手动模式下的水平垂直位置保持标志位，让位置和姿态不被控制//
  128. 		// reset the horizontal and vertical position hold flags for non-manual modes
  129. 		// or if position / altitude is not controlled
  130. 		if (!_control_mode.flag_control_position_enabled || !_control_mode.flag_control_manual_enabled) {
  131. 			_pos_hold_engaged = false;
  132. 		}
  133.  
  134. 		if (!_control_mode.flag_control_altitude_enabled || !_control_mode.flag_control_manual_enabled) {
  135. 			_alt_hold_engaged = false;
  136. 		}
  137. //根据控制模式选择对应的控制函数//
  138. 		if (_control_mode.flag_control_altitude_enabled ||
  139. 				_control_mode.flag_control_position_enabled ||
  140. 				_control_mode.flag_control_climb_rate_enabled ||
  141. 				_control_mode.flag_control_velocity_enabled) {
  142.  
  143. 			_vel_ff.zero();
  144. 			/默认运行位置和姿态控制，在control_*函数中可以失能这种模式而直接在循环中运行速度控制模式/
  145. 			/* by default, run position/altitude controller. the control_* functions
  146. 			 * can disable this and run velocity controllers directly in this cycle */
  147. 			_run_pos_control = true;
  148. 			_run_alt_control = true;
  149. 			/选择控制源/
  150. 			/* select control source */
  151. 			if (_control_mode.flag_control_manual_enabled) {
  152. 				/* manual control */
  153. 				control_manual(dt);//手动控制，在下面会展开
  154. 				_mode_auto = false;
  155.  
  156. 			} else if (_control_mode.flag_control_offboard_enabled) {
  157. 				/* offboard control */
  158. 				control_offboard(dt);//和control_manual(dt);貌似都是根据具体情况设定期望值
  159. 				_mode_auto = false;
  160.  
  161. 			} else {
  162. 				/* AUTO */
  163. 				control_auto(dt);//自动控制
  164. 			}
  165. /********感觉这3个函数功能是实现根据实际复杂情况调整期望值，之后用控制函数一个一个的达到期望值*************/
  166. 			/* weather-vane mode for vtol: disable yaw control */风向标模式垂直起降：禁用偏航控制
  167. 			if (!_control_mode.flag_control_manual_enabled && _pos_sp_triplet.current.disable_mc_yaw_control == true) {
  168. 				_att_sp.disable_mc_yaw_control = true;
  169.  
  170. 			} else {
  171. 				/* reset in case of setpoint updates */
  172. 				_att_sp.disable_mc_yaw_control = false;
  173. 			}
  174. /判断///
  175. 			if (!_control_mode.flag_control_manual_enabled && _pos_sp_triplet.current.valid
  176. 					&& _pos_sp_triplet.current.type == position_setpoint_s::SETPOINT_TYPE_IDLE) {
  177. 				/* idle state, don't run controller and set zero thrust */
  178. 				//空闲状态，不运行控制，并设置为零推力//
  179. 				R.identity();
  180. 				memcpy(&_att_sp.R_body[0], R.data, sizeof(_att_sp.R_body));
  181. 				_att_sp.R_valid = true;
  182.  
  183. 				_att_sp.roll_body = 0.0f;
  184. 				_att_sp.pitch_body = 0.0f;
  185. 				_att_sp.yaw_body = _yaw;
  186. 				_att_sp.thrust = 0.0f;
  187.  
  188. 				_att_sp.timestamp = hrt_absolute_time();
  189. /发布姿态设定值///
  190. 				/* publish attitude setpoint */
  191. 				if (_att_sp_pub != nullptr) {
  192. 					orb_publish(_attitude_setpoint_id, _att_sp_pub, &_att_sp);
  193.  
  194. 				} else if (_attitude_setpoint_id) {
  195. 					_att_sp_pub = orb_advertise(_attitude_setpoint_id, &_att_sp);
  196. 				}
  197. /另一个判断///
  198. 			} else if (_control_mode.flag_control_manual_enabled
  199. 					&& _vehicle_status.condition_landed) {
  200. 				/* don't run controller when landed */着陆不要运行控制器
  201. 				_reset_pos_sp = true;
  202. 				_reset_alt_sp = true;
  203. 				_mode_auto = false;
  204. 				reset_int_z = true;
  205. 				reset_int_xy = true;
  206.  
  207. 				R.identity();
  208. 				memcpy(&_att_sp.R_body[0], R.data, sizeof(_att_sp.R_body));
  209. 				_att_sp.R_valid = true;
  210.  
  211. 				_att_sp.roll_body = 0.0f;
  212. 				_att_sp.pitch_body = 0.0f;
  213. 				_att_sp.yaw_body = _yaw;
  214. 				_att_sp.thrust = 0.0f;
  215.  
  216. 				_att_sp.timestamp = hrt_absolute_time();
  217.  
  218. 				/* publish attitude setpoint */
  219. 				if (_att_sp_pub != nullptr) {
  220. 					orb_publish(_attitude_setpoint_id, _att_sp_pub, &_att_sp);
  221.  
  222. 				} else if (_attitude_setpoint_id) {
  223. 					_att_sp_pub = orb_advertise(_attitude_setpoint_id, &_att_sp);
  224. 				}
  225. /另一个判断///
  226. 			} else {
  227. 				/* run position & altitude controllers, if enabled (otherwise use already computed velocity setpoints) */
  228. 				if (_run_pos_control) {
  229. 					_vel_sp(0) = (_pos_sp(0) - _pos(0)) * _params.pos_p(0);
  230. 					_vel_sp(1) = (_pos_sp(1) - _pos(1)) * _params.pos_p(1);
  231. 				}
  232.  
  233. 				// do not go slower than the follow target velocity when position tracking is active (set to valid)
  234.  
  235. 				if(_pos_sp_triplet.current.type == position_setpoint_s::SETPOINT_TYPE_FOLLOW_TARGET &&
  236. 					_pos_sp_triplet.current.velocity_valid &&
  237. 					_pos_sp_triplet.current.position_valid) {
  238.  
  239. 					math::Vector<3> ft_vel(_pos_sp_triplet.current.vx, _pos_sp_triplet.current.vy, 0);
  240.  
  241. 					float cos_ratio = (ft_vel*_vel_sp)/(ft_vel.length()*_vel_sp.length());
  242.  
  243. 					// only override velocity set points when uav is traveling in same direction as target and vector component
  244. 					// is greater than calculated position set point velocity component
  245.  
  246. 					if(cos_ratio > 0) {
  247. 						ft_vel *= (cos_ratio);
  248. 						// min speed a little faster than target vel
  249. 						ft_vel += ft_vel.normalized()*1.5f;
  250. 					} else {
  251. 						ft_vel.zero();
  252. 					}
  253.  
  254. 					_vel_sp(0) = fabs(ft_vel(0)) > fabs(_vel_sp(0)) ? ft_vel(0) : _vel_sp(0);
  255. 					_vel_sp(1) = fabs(ft_vel(1)) > fabs(_vel_sp(1)) ? ft_vel(1) : _vel_sp(1);
  256.  
  257. 					// track target using velocity only
  258.  
  259. 				} else if (_pos_sp_triplet.current.type == position_setpoint_s::SETPOINT_TYPE_FOLLOW_TARGET &&
  260. 					_pos_sp_triplet.current.velocity_valid) {
  261.  
  262. 					_vel_sp(0) = _pos_sp_triplet.current.vx;
  263. 					_vel_sp(1) = _pos_sp_triplet.current.vy;
  264. 				}
  265.  
  266. 				if (_run_alt_control) {
  267. 					_vel_sp(2) = (_pos_sp(2) - _pos(2)) * _params.pos_p(2);
  268. 				}
  269.  
  270. 				/* make sure velocity setpoint is saturated in xy*/
  271. 				float vel_norm_xy = sqrtf(_vel_sp(0) * _vel_sp(0) +
  272. 							  _vel_sp(1) * _vel_sp(1));
  273.  
  274. 				if (vel_norm_xy > _params.vel_max(0)) {
  275. 					/* note assumes vel_max(0) == vel_max(1) */
  276. 					_vel_sp(0) = _vel_sp(0) * _params.vel_max(0) / vel_norm_xy;
  277. 					_vel_sp(1) = _vel_sp(1) * _params.vel_max(1) / vel_norm_xy;
  278. 				}
  279.  
  280. 				/* make sure velocity setpoint is saturated in z*/
  281. 				float vel_norm_z = sqrtf(_vel_sp(2) * _vel_sp(2));
  282.  
  283. 				if (vel_norm_z > _params.vel_max(2)) {
  284. 					_vel_sp(2) = _vel_sp(2) * _params.vel_max(2) / vel_norm_z;
  285. 				}
  286.  
  287. 				if (!_control_mode.flag_control_position_enabled) {
  288. 					_reset_pos_sp = true;
  289. 				}
  290.  
  291. 				if (!_control_mode.flag_control_altitude_enabled) {
  292. 					_reset_alt_sp = true;
  293. 				}
  294.  
  295. 				if (!_control_mode.flag_control_velocity_enabled) {
  296. 					_vel_sp_prev(0) = _vel(0);
  297. 					_vel_sp_prev(1) = _vel(1);
  298. 					_vel_sp(0) = 0.0f;
  299. 					_vel_sp(1) = 0.0f;
  300. 					control_vel_enabled_prev = false;
  301. 				}
  302.  
  303. 				if (!_control_mode.flag_control_climb_rate_enabled) {
  304. 					_vel_sp(2) = 0.0f;
  305. 				}
  306.  
  307. 				/* use constant descend rate when landing, ignore altitude setpoint */
  308. 				if (!_control_mode.flag_control_manual_enabled && _pos_sp_triplet.current.valid
  309. 						&& _pos_sp_triplet.current.type == position_setpoint_s::SETPOINT_TYPE_LAND) {
  310. 					_vel_sp(2) = _params.land_speed;
  311. 				}
  312.  
  313. 				/* special thrust setpoint generation for takeoff from ground */
  314. 				if (!_control_mode.flag_control_manual_enabled && _pos_sp_triplet.current.valid
  315. 						&& _pos_sp_triplet.current.type == position_setpoint_s::SETPOINT_TYPE_TAKEOFF
  316. 						&& _control_mode.flag_armed) {
  317.  
  318. 					// check if we are not already in air.
  319. 					// if yes then we don't need a jumped takeoff anymore
  320. 					if (!_takeoff_jumped && !_vehicle_status.condition_landed && fabsf(_takeoff_thrust_sp) < FLT_EPSILON) {
  321. 						_takeoff_jumped = true;
  322. 					}
  323.  
  324. 					if (!_takeoff_jumped) {
  325. 						// ramp thrust setpoint up
  326. 						if (_vel(2) > -(_params.tko_speed / 2.0f)) {
  327. 							_takeoff_thrust_sp += 0.5f * dt;
  328. 							_vel_sp.zero();
  329. 							_vel_prev.zero();
  330.  
  331. 						} else {
  332. 							// copter has reached our takeoff speed. split the thrust setpoint up
  333. 							// into an integral part and into a P part
  334. 							thrust_int(2) = _takeoff_thrust_sp - _params.vel_p(2) * fabsf(_vel(2));
  335. 							thrust_int(2) = -math::constrain(thrust_int(2), _params.thr_min, _params.thr_max);
  336. 							_vel_sp_prev(2) = -_params.tko_speed;
  337. 							_takeoff_jumped = true;
  338. 							reset_int_z = false;
  339. 						}
  340. 					}
  341.  
  342. 					if (_takeoff_jumped) {
  343. 						_vel_sp(2) = -_params.tko_speed;
  344. 					}
  345.  
  346. 				} else {
  347. 					_takeoff_jumped = false;
  348. 					_takeoff_thrust_sp = 0.0f;
  349. 				}
  350.  
  351.  
  352.  
  353. 				// limit total horizontal acceleration
  354. 				math::Vector<2> acc_hor;
  355. 				acc_hor(0) = (_vel_sp(0) - _vel_sp_prev(0)) / dt;
  356. 				acc_hor(1) = (_vel_sp(1) - _vel_sp_prev(1)) / dt;
  357.  
  358. 				if (acc_hor.length() > _params.acc_hor_max) {
  359. 					acc_hor.normalize();
  360. 					acc_hor *= _params.acc_hor_max;
  361. 					math::Vector<2> vel_sp_hor_prev(_vel_sp_prev(0), _vel_sp_prev(1));
  362. 					math::Vector<2> vel_sp_hor = acc_hor * dt + vel_sp_hor_prev;
  363. 					_vel_sp(0) = vel_sp_hor(0);
  364. 					_vel_sp(1) = vel_sp_hor(1);
  365. 				}
  366.  
  367. 				// limit vertical acceleration
  368. 				float acc_v = (_vel_sp(2) - _vel_sp_prev(2)) / dt;
  369.  
  370. 				if (fabsf(acc_v) > 2 * _params.acc_hor_max) {
  371. 					acc_v /= fabsf(acc_v);
  372. 					_vel_sp(2) = acc_v * 2 * _params.acc_hor_max * dt + _vel_sp_prev(2);
  373. 				}
  374.  
  375. 				_vel_sp_prev = _vel_sp;
  376.  
  377. 				_global_vel_sp.vx = _vel_sp(0);
  378. 				_global_vel_sp.vy = _vel_sp(1);
  379. 				_global_vel_sp.vz = _vel_sp(2);
  380.  
  381. 				/* publish velocity setpoint */
  382. 				if (_global_vel_sp_pub != nullptr) {
  383. 					orb_publish(ORB_ID(vehicle_global_velocity_setpoint), _global_vel_sp_pub, &_global_vel_sp);
  384.  
  385. 				} else {
  386. 					_global_vel_sp_pub = orb_advertise(ORB_ID(vehicle_global_velocity_setpoint), &_global_vel_sp);
  387. 				}
  388.  
  389. 				if (_control_mode.flag_control_climb_rate_enabled || _control_mode.flag_control_velocity_enabled) {
  390. 					/* reset integrals if needed */
  391. 					if (_control_mode.flag_control_climb_rate_enabled) {
  392. 						if (reset_int_z) {
  393. 							reset_int_z = false;
  394. 							float i = _params.thr_min;
  395.  
  396. 							if (reset_int_z_manual) {
  397. 								i = _params.thr_hover;
  398.  
  399. 								if (i < _params.thr_min) {
  400. 									i = _params.thr_min;
  401.  
  402. 								} else if (i > _params.thr_max) {
  403. 									i = _params.thr_max;
  404. 								}
  405. 							}
  406.  
  407. 							thrust_int(2) = -i;
  408. 						}
  409.  
  410. 					} else {
  411. 						reset_int_z = true;
  412. 					}
  413.  
  414. 					if (_control_mode.flag_control_velocity_enabled) {
  415. 						if (reset_int_xy) {
  416. 							reset_int_xy = false;
  417. 							thrust_int(0) = 0.0f;
  418. 							thrust_int(1) = 0.0f;
  419. 						}
  420.  
  421. 					} else {
  422. 						reset_int_xy = true;
  423. 					}
  424.  
  425. 					/* velocity error */
  426. 					math::Vector<3> vel_err = _vel_sp - _vel;
  427. 					检查是否从非速度模式转到速度控制模式，如果是的话，校正xy的速度设定值以便姿态设定值保持不变/
  428. 					// check if we have switched from a non-velocity controlled mode into a velocity controlled mode
  429. 					// if yes, then correct xy velocity setpoint such that the attitude setpoint is continuous
  430. 					if (!control_vel_enabled_prev && _control_mode.flag_control_velocity_enabled) {
  431.  
  432. 						// choose velocity xyz setpoint such that the resulting thrust setpoint has the direction
  433. 						// given by the last attitude setpoint
  434. 						_vel_sp(0) = _vel(0) + (-PX4_R(_att_sp.R_body, 0, 2) * _att_sp.thrust - thrust_int(0) - _vel_err_d(0) * _params.vel_d(0)) / _params.vel_p(0);
  435. 						_vel_sp(1) = _vel(1) + (-PX4_R(_att_sp.R_body, 1, 2) * _att_sp.thrust - thrust_int(1) - _vel_err_d(1) * _params.vel_d(1)) / _params.vel_p(1);
  436. 						_vel_sp(2) = _vel(2) + (-PX4_R(_att_sp.R_body, 2, 2) * _att_sp.thrust - thrust_int(2) - _vel_err_d(2) * _params.vel_d(2)) / _params.vel_p(2);
  437. 						_vel_sp_prev(0) = _vel_sp(0);
  438. 						_vel_sp_prev(1) = _vel_sp(1);
  439. 						_vel_sp_prev(2) = _vel_sp(2);
  440. 						control_vel_enabled_prev = true;
  441.  
  442. 						// compute updated velocity error
  443. 						vel_err = _vel_sp - _vel;
  444. 					}
  445.  
  446. 					/* thrust vector in NED frame */
  447. 					math::Vector<3> thrust_sp = vel_err.emult(_params.vel_p) + _vel_err_d.emult(_params.vel_d) + thrust_int;
  448.  
  449. 					if (_pos_sp_triplet.current.type == position_setpoint_s::SETPOINT_TYPE_TAKEOFF
  450. 							&& !_takeoff_jumped && !_control_mode.flag_control_manual_enabled) {
  451. 						// for jumped takeoffs use special thrust setpoint calculated above
  452. 						thrust_sp.zero();
  453. 						thrust_sp(2) = -_takeoff_thrust_sp;
  454. 					}
  455.  
  456. 					if (!_control_mode.flag_control_velocity_enabled) {
  457. 						thrust_sp(0) = 0.0f;
  458. 						thrust_sp(1) = 0.0f;
  459. 					}
  460.  
  461. 					if (!_control_mode.flag_control_climb_rate_enabled) {
  462. 						thrust_sp(2) = 0.0f;
  463. 					}
  464.  
  465. 					/* limit thrust vector and check for saturation */
  466. 					bool saturation_xy = false;
  467. 					bool saturation_z = false;
  468.  
  469. 					/* limit min lift */
  470. 					float thr_min = _params.thr_min;
  471.  
  472. 					if (!_control_mode.flag_control_velocity_enabled && thr_min < 0.0f) {
  473. 						/* don't allow downside thrust direction in manual attitude mode */
  474. 						thr_min = 0.0f;
  475. 					}
  476.  
  477. 					float thrust_abs = thrust_sp.length();
  478. 					float tilt_max = _params.tilt_max_air;
  479. 					float thr_max = _params.thr_max;
  480. 					/* filter vel_z over 1/8sec */
  481. 					_vel_z_lp = _vel_z_lp * (1.0f - dt * 8.0f) + dt * 8.0f * _vel(2);
  482. 					/* filter vel_z change over 1/8sec */
  483. 					float vel_z_change = (_vel(2) - _vel_prev(2)) / dt;
  484. 					_acc_z_lp = _acc_z_lp * (1.0f - dt * 8.0f) + dt * 8.0f * vel_z_change;
  485.  
  486. 					/* adjust limits for landing mode */调整限制  着陆模式
  487. 					if (!_control_mode.flag_control_manual_enabled && _pos_sp_triplet.current.valid &&
  488. 							_pos_sp_triplet.current.type == position_setpoint_s::SETPOINT_TYPE_LAND) {
  489. 						/* limit max tilt and min lift when landing */
  490. 						tilt_max = _params.tilt_max_land;
  491.  
  492. 						if (thr_min < 0.0f) {
  493. 							thr_min = 0.0f;
  494. 						}
  495.  
  496. 						/* descend stabilized, we're landing */
  497. 						if (!_in_landing && !_lnd_reached_ground
  498. 								&& (float)fabs(_acc_z_lp) < 0.1f
  499. 								&& _vel_z_lp > 0.5f * _params.land_speed) {
  500. 							_in_landing = true;
  501. 						}
  502.  
  503. 						/* assume ground, cut thrust */
  504. 						if (_in_landing
  505. 								&& _vel_z_lp < 0.1f) {
  506. 							thr_max = 0.0f;
  507. 							_in_landing = false;
  508. 							_lnd_reached_ground = true;
  509. 						}
  510.  
  511. 						/* once we assumed to have reached the ground always cut the thrust.
  512. 							Only free fall detection below can revoke this
  513. 						*/
  514. 						if (!_in_landing && _lnd_reached_ground) {
  515. 							thr_max = 0.0f;
  516. 						}
  517.  
  518. 						/* if we suddenly fall, reset landing logic and remove thrust limit */
  519. 						if (_lnd_reached_ground
  520. 								/* XXX: magic value, assuming free fall above 4m/s2 acceleration */
  521. 								&& (_acc_z_lp > 4.0f
  522. 								    || _vel_z_lp > 2.0f * _params.land_speed)) {
  523. 							thr_max = _params.thr_max;
  524. 							_in_landing = false;
  525. 							_lnd_reached_ground = false;
  526. 						}
  527.  
  528. 					} else {
  529. 						_in_landing = false;
  530. 						_lnd_reached_ground = false;
  531. 					}
  532.  
  533. 					/* limit min lift */
  534. 					if (-thrust_sp(2) < thr_min) {
  535. 						thrust_sp(2) = -thr_min;
  536. 						saturation_z = true;
  537. 					}
  538.  
  539. 					if (_control_mode.flag_control_velocity_enabled) {
  540.  
  541. 						/* limit max tilt */
  542. 						if (thr_min >= 0.0f && tilt_max < M_PI_F / 2 - 0.05f) {
  543. 							/* absolute horizontal thrust */
  544. 							float thrust_sp_xy_len = math::Vector<2>(thrust_sp(0), thrust_sp(1)).length();
  545.  
  546. 							if (thrust_sp_xy_len > 0.01f) {
  547. 								/* max horizontal thrust for given vertical thrust*/
  548. 								float thrust_xy_max = -thrust_sp(2) * tanf(tilt_max);
  549.  
  550. 								if (thrust_sp_xy_len > thrust_xy_max) {
  551. 									float k = thrust_xy_max / thrust_sp_xy_len;
  552. 									thrust_sp(0) *= k;
  553. 									thrust_sp(1) *= k;
  554. 									saturation_xy = true;
  555. 								}
  556. 							}
  557. 						}
  558. 					}
  559.  
  560. 					if (_control_mode.flag_control_altitude_enabled) {
  561. 						/* thrust compensation for altitude only control modes */推力的补偿  高度控制模式
  562. 						float att_comp;
  563.  
  564. 						if (_R(2, 2) > TILT_COS_MAX) {
  565. 							att_comp = 1.0f / _R(2, 2);
  566.  
  567. 						} else if (_R(2, 2) > 0.0f) {
  568. 							att_comp = ((1.0f / TILT_COS_MAX - 1.0f) / TILT_COS_MAX) * _R(2, 2) + 1.0f;
  569. 							saturation_z = true;
  570.  
  571. 						} else {
  572. 							att_comp = 1.0f;
  573. 							saturation_z = true;
  574. 						}
  575.  
  576. 						thrust_sp(2) *= att_comp;
  577. 					}
  578. 					///限制最大推力///
  579. 					/* limit max thrust */
  580. 					thrust_abs = thrust_sp.length(); /* recalculate because it might have changed */
  581.  
  582. 					if (thrust_abs > thr_max) {
  583. 						if (thrust_sp(2) < 0.0f) {
  584. 							if (-thrust_sp(2) > thr_max) {
  585. 								/* thrust Z component is too large, limit it */
  586. 								thrust_sp(0) = 0.0f;
  587. 								thrust_sp(1) = 0.0f;
  588. 								thrust_sp(2) = -thr_max;
  589. 								saturation_xy = true;
  590. 								saturation_z = true;
  591.  
  592. 							} else {
  593. 								/* preserve thrust Z component and lower XY, keeping altitude is more important than position */
  594. 								float thrust_xy_max = sqrtf(thr_max * thr_max - thrust_sp(2) * thrust_sp(2));
  595. 								float thrust_xy_abs = math::Vector<2>(thrust_sp(0), thrust_sp(1)).length();
  596. 								float k = thrust_xy_max / thrust_xy_abs;
  597. 								thrust_sp(0) *= k;
  598. 								thrust_sp(1) *= k;
  599. 								saturation_xy = true;
  600. 							}
  601.  
  602. 						} else {
  603. 							/* Z component is negative, going down, simply limit thrust vector */
  604. 							float k = thr_max / thrust_abs;
  605. 							thrust_sp *= k;
  606. 							saturation_xy = true;
  607. 							saturation_z = true;
  608. 						}
  609.  
  610. 						thrust_abs = thr_max;
  611. 					}
  612. 					///跟新积分//
  613. 					/* update integrals */
  614. 					if (_control_mode.flag_control_velocity_enabled && !saturation_xy) {
  615. 						thrust_int(0) += vel_err(0) * _params.vel_i(0) * dt;
  616. 						thrust_int(1) += vel_err(1) * _params.vel_i(1) * dt;
  617. 					}
  618.  
  619. 					if (_control_mode.flag_control_climb_rate_enabled && !saturation_z) {
  620. 						thrust_int(2) += vel_err(2) * _params.vel_i(2) * dt;
  621.  
  622. 						/* protection against flipping on ground when landing */
  623. 						if (thrust_int(2) > 0.0f) {
  624. 							thrust_int(2) = 0.0f;
  625. 						}
  626. 					}
  627. 					/根据推力矢量计算的姿态设定值//
  628. 					/* calculate attitude setpoint from thrust vector */
  629. 					if (_control_mode.flag_control_velocity_enabled) {
  630. 						/* desired body_z axis = -normalize(thrust_vector) */
  631. 						math::Vector<3> body_x;
  632. 						math::Vector<3> body_y;
  633. 						math::Vector<3> body_z;
  634.  
  635. 						if (thrust_abs > SIGMA) {
  636. 							body_z = -thrust_sp / thrust_abs;
  637.  
  638. 						} else {
  639. 							/* no thrust, set Z axis to safe value */
  640. 							body_z.zero();
  641. 							body_z(2) = 1.0f;
  642. 						}
  643.  
  644. 						/* vector of desired yaw direction in XY plane, rotated by PI/2 */
  645. 						math::Vector<3> y_C(-sinf(_att_sp.yaw_body), cosf(_att_sp.yaw_body), 0.0f);
  646.  
  647. 						if (fabsf(body_z(2)) > SIGMA) {
  648. 							/* desired body_x axis, orthogonal to body_z */
  649. 							body_x = y_C % body_z;
  650.  
  651. 							/* keep nose to front while inverted upside down */
  652. 							if (body_z(2) < 0.0f) {
  653. 								body_x = -body_x;
  654. 							}
  655.  
  656. 							body_x.normalize();
  657.  
  658. 						} else {
  659. 							/* desired thrust is in XY plane, set X downside to construct correct matrix,
  660. 							 * but yaw component will not be used actually */
  661. 							body_x.zero();
  662. 							body_x(2) = 1.0f;
  663. 						}
  664.  
  665. 						/* desired body_y axis */
  666. 						body_y = body_z % body_x;
  667.  
  668. 						/* fill rotation matrix */
  669. 						for (int i = 0; i < 3; i++) {
  670. 							R(i, 0) = body_x(i);
  671. 							R(i, 1) = body_y(i);
  672. 							R(i, 2) = body_z(i);
  673. 						}
  674. 						复制旋转矩阵到姿态设定值主题中
  675. 						/* copy rotation matrix to attitude setpoint topic */
  676. 						memcpy(&_att_sp.R_body[0], R.data, sizeof(_att_sp.R_body));
  677. 						_att_sp.R_valid = true;
  678. 						/复制四元数设定值到姿态设定值主题中///
  679. 						/* copy quaternion setpoint to attitude setpoint topic */
  680. 						math::Quaternion q_sp;
  681. 						q_sp.from_dcm(R);
  682. 						memcpy(&_att_sp.q_d[0], &q_sp.data[0], sizeof(_att_sp.q_d));
  683. 						//计算欧拉角，只记录，不得用于控制//
  684. 						/* calculate euler angles, for logging only, must not be used for control */
  685. 						math::Vector<3> euler = R.to_euler();
  686. 						_att_sp.roll_body = euler(0);
  687. 						_att_sp.pitch_body = euler(1);
  688. 						/* yaw already used to construct rot matrix, but actual rotation matrix can have different yaw near singularity */
  689. 						/偏航已经用于构建rot矩阵，但实际旋转矩阵可以有奇点附近的不同偏航///
  690. 					} else if (!_control_mode.flag_control_manual_enabled) {
  691. 						/没有位置控制的自主高度控制（故障安全降落），强制水平姿态，不改变yaw///
  692. 						/* autonomous altitude control without position control (failsafe landing),
  693. 						 * force level attitude, don't change yaw */
  694. 						R.from_euler(0.0f, 0.0f, _att_sp.yaw_body);
  695. 						复制旋转矩阵到姿态设定值主题中
  696. 						/* copy rotation matrix to attitude setpoint topic */
  697. 						memcpy(&_att_sp.R_body[0], R.data, sizeof(_att_sp.R_body));
  698. 						_att_sp.R_valid = true;
  699. 						/复制四元数设定值到姿态设定值主题中///
  700. 						/* copy quaternion setpoint to attitude setpoint topic */
  701. 						math::Quaternion q_sp;
  702. 						q_sp.from_dcm(R);
  703. 						memcpy(&_att_sp.q_d[0], &q_sp.data[0], sizeof(_att_sp.q_d));
  704.  
  705. 						_att_sp.roll_body = 0.0f;
  706. 						_att_sp.pitch_body = 0.0f;
  707. 					}
  708.  
  709. 					_att_sp.thrust = thrust_abs;
  710.  
  711. 					/* save thrust setpoint for logging */保存推理设定值在logging里
  712. 					_local_pos_sp.acc_x = thrust_sp(0) * ONE_G;
  713. 					_local_pos_sp.acc_y = thrust_sp(1) * ONE_G;
  714. 					_local_pos_sp.acc_z = thrust_sp(2) * ONE_G;
  715.  
  716. 					_att_sp.timestamp = hrt_absolute_time();
  717.  
  718.  
  719. 				} else {
  720. 					reset_int_z = true;
  721. 				}
  722. 			}
  723.  
  724. 			/* fill local position, velocity and thrust setpoint */
  725. 			_local_pos_sp.timestamp = hrt_absolute_time();
  726. 			_local_pos_sp.x = _pos_sp(0);
  727. 			_local_pos_sp.y = _pos_sp(1);
  728. 			_local_pos_sp.z = _pos_sp(2);
  729. 			_local_pos_sp.yaw = _att_sp.yaw_body;
  730. 			_local_pos_sp.vx = _vel_sp(0);
  731. 			_local_pos_sp.vy = _vel_sp(1);
  732. 			_local_pos_sp.vz = _vel_sp(2);
  733. 发布当地位置设定值
  734. /***************这个应该是这段程序处理的最后结果********************/
  735. 			/* publish local position setpoint */
  736. 			if (_local_pos_sp_pub != nullptr) {
  737. 				orb_publish(ORB_ID(vehicle_local_position_setpoint), _local_pos_sp_pub, &_local_pos_sp);
  738.  
  739. 			} else {
  740. 				_local_pos_sp_pub = orb_advertise(ORB_ID(vehicle_local_position_setpoint), &_local_pos_sp);
  741. 			}
  742.  
  743. 		} else {位置控制模式失能，那么复位设定值
  744. 			/* position controller disabled, reset setpoints */
  745. 			_reset_alt_sp = true;
  746. 			_reset_pos_sp = true;
  747. 			_mode_auto = false;
  748. 			reset_int_z = true;
  749. 			reset_int_xy = true;
  750. 			control_vel_enabled_prev = false;
  751.  
  752. 			/* store last velocity in case a mode switch to position control occurs */
  753. 			_vel_sp_prev = _vel;
  754. 		}
  755. //从手动控制中产生姿态设定值//
  756. 		/* generate attitude setpoint from manual controls */
  757. 		if (_control_mode.flag_control_manual_enabled && _control_mode.flag_control_attitude_enabled) {
  758.  
  759. 			/* reset yaw setpoint to current position if needed */
  760. 			if (reset_yaw_sp) {
  761. 				reset_yaw_sp = false;
  762. 				_att_sp.yaw_body = _yaw;
  763. 			}
  764. //在地上时，不动偏航方向/
  765. 			/* do not move yaw while sitting on the ground */
  766. 			else if (!_vehicle_status.condition_landed &&
  767. 					!(!_control_mode.flag_control_altitude_enabled && _manual.z < 0.1f)) {
  768.  
  769. 				/* we want to know the real constraint, and global overrides manual */
  770. 				const float yaw_rate_max = (_params.man_yaw_max < _params.global_yaw_max) ? _params.man_yaw_max :
  771. 							   _params.global_yaw_max;
  772. 				const float yaw_offset_max = yaw_rate_max / _params.mc_att_yaw_p;
  773.  
  774. 				_att_sp.yaw_sp_move_rate = _manual.r * yaw_rate_max;
  775. 				float yaw_target = _wrap_pi(_att_sp.yaw_body + _att_sp.yaw_sp_move_rate * dt);
  776. 				float yaw_offs = _wrap_pi(yaw_target - _yaw);
  777. 如果对于系统跟踪，yaw偏移变得过大，那么停止其转移
  778. 				// If the yaw offset became too big for the system to track stop
  779. 				// shifting it
  780.  
  781. 				// XXX this needs inspection - probably requires a clamp, not an if
  782. 				if (fabsf(yaw_offs) < yaw_offset_max) {
  783. 					_att_sp.yaw_body = yaw_target;
  784. 				}
  785. 			}
  786. //直接控制侧倾和俯仰，如果协助速度控制器没有被激活
  787. 			/* control roll and pitch directly if we no aiding velocity controller is active */
  788. 			if (!_control_mode.flag_control_velocity_enabled) {
  789. 				_att_sp.roll_body = _manual.y * _params.man_roll_max;
  790. 				_att_sp.pitch_body = -_manual.x * _params.man_pitch_max;
  791. 			}
  792. //控制油门直接，如果没有爬升率控制器被激活/
  793. 			/* control throttle directly if no climb rate controller is active */
  794. 			if (!_control_mode.flag_control_climb_rate_enabled) {
  795. 				float thr_val = throttle_curve(_manual.z, _params.thr_hover);
  796. 				_att_sp.thrust = math::min(thr_val, _manual_thr_max.get());
  797.  
  798. 				/* enforce minimum throttle if not landed */
  799. 				if (!_vehicle_status.condition_landed) {
  800. 					_att_sp.thrust = math::max(_att_sp.thrust, _manual_thr_min.get());
  801. 				}
  802. 			}
  803.  
  804. 			math::Matrix<3, 3> R_sp;
  805. //构建姿态设定值的旋转矩阵///
  806. 			/* construct attitude setpoint rotation matrix */
  807. 			R_sp.from_euler(_att_sp.roll_body, _att_sp.pitch_body, _att_sp.yaw_body);
  808. 			memcpy(&_att_sp.R_body[0], R_sp.data, sizeof(_att_sp.R_body));
  809. 为所有非姿态飞行模式      复位加速度设定值/
  810. 			/* reset the acceleration set point for all non-attitude flight modes */
  811. 			if (!(_control_mode.flag_control_offboard_enabled &&
  812. 					!(_control_mode.flag_control_position_enabled ||
  813. 					  _control_mode.flag_control_velocity_enabled))) {
  814.  
  815. 				_thrust_sp_prev = R_sp * math::Vector<3>(0, 0, -_att_sp.thrust);
  816. 			}
  817. /将四元数设定值复制到姿态设定值的主题中/
  818. 			/* copy quaternion setpoint to attitude setpoint topic */
  819. 			math::Quaternion q_sp;
  820. 			q_sp.from_dcm(R_sp);
  821. 			memcpy(&_att_sp.q_d[0], &q_sp.data[0], sizeof(_att_sp.q_d));
  822. 			_att_sp.timestamp = hrt_absolute_time();
  823.  
  824. 		} else {
  825. 			reset_yaw_sp = true;
  826. 		}
  827.  
  828. 		/* update previous velocity for velocity controller D part */
  829. 		_vel_prev = _vel;
  830. /发布设定值//
  831. 		/* publish attitude setpoint
  832. 		 * Do not publish if offboard is enabled but position/velocity control is disabled,
  833. 		 * in this case the attitude setpoint is published by the mavlink app. Also do not publish
  834. 		 * if the vehicle is a VTOL and it's just doing a transition (the VTOL attitude control module will generate
  835. 		 * attitude setpoints for the transition).
  836. 		 */
  837. 		if (!(_control_mode.flag_control_offboard_enabled &&
  838. 				!(_control_mode.flag_control_position_enabled ||
  839. 				  _control_mode.flag_control_velocity_enabled))) {
  840.  
  841. 			if (_att_sp_pub != nullptr) {
  842. 				orb_publish(_attitude_setpoint_id, _att_sp_pub, &_att_sp);
  843.  
  844. 			} else if (_attitude_setpoint_id) {
  845. 				_att_sp_pub = orb_advertise(_attitude_setpoint_id, &_att_sp);
  846. 			}
  847. 		}
  848. /手动油门控制后,将高度控制器积分（悬停油门）复位到手动油门///
  849. 		/* reset altitude controller integral (hovering throttle) to manual throttle after manual throttle control */
  850. 		reset_int_z_manual = _control_mode.flag_armed && _control_mode.flag_control_manual_enabled
  851. 				     && !_control_mode.flag_control_climb_rate_enabled;
  852. 	}
  853.  
  854. 	mavlink_log_info(&_mavlink_log_pub, "[mpc] stopped");
  855.  
  856. 	_control_task = -1;
  857. }
这些设定值的确定是不是就是控制策略的体现？ 
  1. void
  2. MulticopterPositionControl::poll_subscriptions()
  3. {
  4. 	bool updated;
  5. /**********************机体状态跟新**************************/
  6. 	orb_check(_vehicle_status_sub, &updated);
  7.  
  8. 	if (updated) {
  9. 		orb_copy(ORB_ID(vehicle_status), _vehicle_status_sub, &_vehicle_status);
  10.  
  11. 		/* set correct uORB ID, depending on if vehicle is VTOL or not */
  12. 		if (!_attitude_setpoint_id) {
  13. 			if (_vehicle_status.is_vtol) {
  14. 				_attitude_setpoint_id = ORB_ID(mc_virtual_attitude_setpoint);
  15.  
  16. 			} else {
  17. 				_attitude_setpoint_id = ORB_ID(vehicle_attitude_setpoint);
  18. 			}
  19. 		}
  20. 	}
  21. /**********************控制状态跟新**************************/
  22. 	orb_check(_ctrl_state_sub, &updated);
  23.  
  24. 	if (updated) {
  25. 		orb_copy(ORB_ID(control_state), _ctrl_state_sub, &_ctrl_state);
  26.  
  27. 		/* get current rotation matrix and euler angles from control state quaternions */
  28. 		math::Quaternion q_att(_ctrl_state.q[0], _ctrl_state.q[1], _ctrl_state.q[2], _ctrl_state.q[3]);
  29. 		_R = q_att.to_dcm();
  30. 		math::Vector<3> euler_angles;
  31. 		euler_angles = _R.to_euler();
  32. 		_yaw = euler_angles(2);
  33. 	}
  34. /**********************setpoint状态跟新**************************/
  35. 	orb_check(_att_sp_sub, &updated);
  36.  
  37. 	if (updated) {
  38. 		orb_copy(ORB_ID(vehicle_attitude_setpoint), _att_sp_sub, &_att_sp);
  39. 	}
  40.  
  41. 	orb_check(_control_mode_sub, &updated);
  42.  
  43. 	if (updated) {
  44. 		orb_copy(ORB_ID(vehicle_control_mode), _control_mode_sub, &_control_mode);
  45. 	}
  46. /**********************手动控制状态跟新**************************/
  47. 	orb_check(_manual_sub, &updated); 
  48.  
  49. 	if (updated) {
  50. 		orb_copy(ORB_ID(manual_control_setpoint), _manual_sub, &_manual);
  51. 	}
  52. /**********************解锁状态跟新**************************/
  53. 	orb_check(_arming_sub, &updated);
  54.  
  55. 	if (updated) {
  56. 		orb_copy(ORB_ID(actuator_armed), _arming_sub, &_arming);
  57. 	}
  58. /**********************位置跟新**************************/
  59. 	orb_check(_local_pos_sub, &updated);
  60.  
  61. 	if (updated) {
  62. 		orb_copy(ORB_ID(vehicle_local_position), _local_pos_sub, &_local_pos);
  63. 	}
  64. }
  1. void
  2. MulticopterPositionControl::update_ref()
  3. {
  4. 	if (_local_pos.ref_timestamp != _ref_timestamp) {
  5. 		double lat_sp, lon_sp;//维度设定、经度设定
  6. 		float alt_sp = 0.0f;//姿态设定
  7.  
  8. 		if (_ref_timestamp != 0) { 
  9. 		/*************在整体框架下计算当前位置设定值***************/
  10. 			/* calculate current position setpoint in global frame */
  11. 			map_projection_reproject(&_ref_pos, _pos_sp(0), _pos_sp(1), &lat_sp, &lon_sp);
  12. 			alt_sp = _ref_alt - _pos_sp(2);
  13. 		}
  14. 		/*************跟新投影参考*********************/
  15. 		/* update local projection reference */
  16. 		map_projection_init(&_ref_pos, _local_pos.ref_lat, _local_pos.ref_lon);
  17. 		_ref_alt = _local_pos.ref_alt;
  18.  
  19. 		if (_ref_timestamp != 0) {
  20. 		/*********************设置新的位置设定值*******************/
  21. 			/* reproject position setpoint to new reference */
  22. 			map_projection_project(&_ref_pos, lat_sp, lon_sp, &_pos_sp.data[0], &_pos_sp.data[1]);
  23. 			_pos_sp(2) = -(alt_sp - _ref_alt);
  24. 		}
  25.  
  26. 		_ref_timestamp = _local_pos.ref_timestamp;
  27. 	}
  28. }
  1. void
  2. MulticopterPositionControl::control_manual(float dt)
  3. {
  4. 	math::Vector<3> req_vel_sp; // X,Y in local frame and Z in global (D), in [-1,1] normalized range
  5. 	req_vel_sp.zero();
  6.  
  7. 	if (_control_mode.flag_control_altitude_enabled) {
  8. 		/* set vertical velocity setpoint with throttle stick */用油门棒设置垂直速度
  9. 		req_vel_sp(2) = -scale_control(_manual.z - 0.5f, 0.5f, _params.alt_ctl_dz, _params.alt_ctl_dy); // D
  10. 	}
  11.  
  12. 	if (_control_mode.flag_control_position_enabled) {
  13. 		/* set horizontal velocity setpoint with roll/pitch stick */用roll/pitch棒设置水平速度
  14. 		req_vel_sp(0) = _manual.x;
  15. 		req_vel_sp(1) = _manual.y;
  16. 	}
  17.  
  18. 	if (_control_mode.flag_control_altitude_enabled) {
  19. 		/* reset alt setpoint to current altitude if needed */复位姿态设定值
  20. 		reset_alt_sp();
  21. 	}
  22.  
  23. 	if (_control_mode.flag_control_position_enabled) {
  24. 		/* reset position setpoint to current position if needed */复位位置设定值
  25. 		reset_pos_sp();
  26. 	}
  27.  
  28. 	/* limit velocity setpoint */限制速度设定值
  29. 	float req_vel_sp_norm = req_vel_sp.length();
  30.  
  31. 	if (req_vel_sp_norm > 1.0f) {
  32. 		req_vel_sp /= req_vel_sp_norm;//相当于是向量除以它的模值
  33. 	}
  34. ///_req_vel_sp的范围是0到1，围绕yaw旋转
  35. 	/* _req_vel_sp scaled to 0..1, scale it to max speed and rotate around yaw */
  36. 	math::Matrix<3, 3> R_yaw_sp;
  37. 	R_yaw_sp.from_euler(0.0f, 0.0f, _att_sp.yaw_body);
  38. 	math::Vector<3> req_vel_sp_scaled = R_yaw_sp * req_vel_sp.emult(
  39. 			_params.vel_cruise); // in NED and scaled to actual velocity在NED旋转方式下缩放到实际速度
  40. /用户辅助模式：用户控制速度，但是，如果速度足够小，相应的轴的位置就会保持
  41. 	/*
  42. 	 * assisted velocity mode: user controls velocity, but if	velocity is small enough, position
  43. 	 * hold is activated for the corresponding axis
  44. 	 */
  45.  
  46. 	/* horizontal axes */水平轴
  47. 	if (_control_mode.flag_control_position_enabled) {
  48. 		/* check for pos. hold */检测位置的保持状态
  49. 		if (fabsf(req_vel_sp(0)) < _params.hold_xy_dz && fabsf(req_vel_sp(1)) < _params.hold_xy_dz) {
  50. 			if (!_pos_hold_engaged) {
  51. 				if (_params.hold_max_xy < FLT_EPSILON || (fabsf(_vel(0)) < _params.hold_max_xy
  52. 						&& fabsf(_vel(1)) < _params.hold_max_xy)) {
  53. 					_pos_hold_engaged = true;
  54.  
  55. 				} else {
  56. 					_pos_hold_engaged = false;
  57. 				}
  58. 			}
  59.  
  60. 		} else {
  61. 			_pos_hold_engaged = false;
  62. 		}
  63.  
  64. 		/* set requested velocity setpoint */设置所需的速度设定值
  65. 		if (!_pos_hold_engaged) {
  66. 			_pos_sp(0) = _pos(0);
  67. 			_pos_sp(1) = _pos(1);
  68. 			//速度设定值会被利用，位置设定值不会被用/
  69. 			_run_pos_control = false; /* request velocity setpoint to be used, instead of position setpoint */
  70. 			_vel_sp(0) = req_vel_sp_scaled(0);
  71. 			_vel_sp(1) = req_vel_sp_scaled(1);
  72. 		}
  73. 	}
  74.  
  75. 	/* vertical axis */垂直轴
  76. 	if (_control_mode.flag_control_altitude_enabled) {
  77. 		/* check for pos. hold */
  78. 		if (fabsf(req_vel_sp(2)) < FLT_EPSILON) {
  79. 			if (!_alt_hold_engaged) {
  80. 				if (_params.hold_max_z < FLT_EPSILON || fabsf(_vel(2)) < _params.hold_max_z) {
  81. 					_alt_hold_engaged = true;
  82.  
  83. 				} else {
  84. 					_alt_hold_engaged = false;
  85. 				}
  86. 			}
  87.  
  88. 		} else {
  89. 			_alt_hold_engaged = false;
  90. 		}
  91.  
  92. 		/* set requested velocity setpoint */
  93. 		if (!_alt_hold_engaged) {
  94. 			_run_alt_control = false; /* request velocity setpoint to be used, instead of altitude setpoint */
  95. 			_vel_sp(2) = req_vel_sp_scaled(2);
  96. 			_pos_sp(2) = _pos(2);
  97. 		}
  98. 	}
  99. }
  1. #ifdef __cplusplus
  2. struct __EXPORT position_setpoint_triplet_s {
  3. #else
  4. struct position_setpoint_triplet_s {
  5. #endif
  6. 	struct position_setpoint_s previous;
  7. 	struct position_setpoint_s current;
  8. 	struct position_setpoint_s next;
  9. 	uint8_t nav_state;
  10. #ifdef __cplusplus
  11. #endif
  12. };

整个MulticopterPositionControl::task_main()的输出应该是

  1. orb_publish(_attitude_setpoint_id, _att_sp_pub, &_att_sp);
  2. orb_publish(ORB_ID(vehicle_global_velocity_setpoint), _global_vel_sp_pub, &_global_vel_sp);
  3. orb_publish(ORB_ID(vehicle_local_position_setpoint), _local_pos_sp_pub, &_local_pos_sp);

其中orb_publish(_attitude_setpoint_id, _att_sp_pub, &_att_sp);是在main的最下面，应该这个是最后处理的结果

但是全局搜索ORB_ID(vehicle_local_position_setpoint)
 没有找到orb_copy(ORB_ID(vehicle_local_position_setpoint) 或者 orb_copy(_attitude_setpoint_id  

  1. if (!_attitude_setpoint_id) {
  2. 	if (_vehicle_status.is_vtol) {
  3. 		_attitude_setpoint_id = ORB_ID(mc_virtual_attitude_setpoint);
  4.  
  5. 	} else {
  6. 		_attitude_setpoint_id = ORB_ID(vehicle_attitude_setpoint);
  7. 	}
  8. }
再看看 
  1. void
  2. MulticopterPositionControl::task_main()
  3. {
  4.  
  5. 	/*
  6. 	 * do subscriptions
  7. 	 */
  8. 	_vehicle_status_sub = orb_subscribe(ORB_ID(vehicle_status));
  9. 	_ctrl_state_sub = orb_subscribe(ORB_ID(control_state));
  10. 	_att_sp_sub = orb_subscribe(ORB_ID(vehicle_attitude_setpoint));
  11. 	_control_mode_sub = orb_subscribe(ORB_ID(vehicle_control_mode));
  12. 	_params_sub = orb_subscribe(ORB_ID(parameter_update));
  13. 	_manual_sub = orb_subscribe(ORB_ID(manual_control_setpoint));
  14. 	_arming_sub = orb_subscribe(ORB_ID(actuator_armed));
  15. 	_local_pos_sub = orb_subscribe(ORB_ID(vehicle_local_position));
  16. 	_pos_sp_triplet_sub = orb_subscribe(ORB_ID(position_setpoint_triplet));
  17. 	_local_pos_sp_sub = orb_subscribe(ORB_ID(vehicle_local_position_setpoint));
  18. 	_global_vel_sp_sub = orb_subscribe(ORB_ID(vehicle_global_velocity_setpoint));
全局搜索vehicle_attitude_setpoint
由此可以看出最后mc_pos_control_main.cpp最后处理的结果一方面被自己重新订阅，用于不断跟新；另一方面被mc_att_control订阅，用于控制姿态
在进入姿态前，再来理下光流信息如何流动的
//ardupilot/modules/PX4Firmware/src/drivers/px4flow/px4flow.cpp
orb_publish(ORB_ID(optical_flow), _px4flow_topic, &report);
//ardupilot/modules/PX4Firmware/src/modules/position_estimator_inav/position_estimator_inav_main.cpp
int optical_flow_sub = orb_subscribe(ORB_ID(optical_flow));
orb_copy(ORB_ID(optical_flow), optical_flow_sub, &flow);
orb_publish(ORB_ID(vehicle_local_position), vehicle_local_position_pub, &local_pos);
//ardupilot/modules/PX4Firmware/src/modules/mc_pos_contol/mc_pos_contol_main.cpp
_local_pos_sub = orb_subscribe(ORB_ID(vehicle_local_position));
orb_copy(ORB_ID(vehicle_local_position), _local_pos_sub, &_local_pos);
orb_publish(_attitude_setpoint_id, _att_sp_pub, &_att_sp);
_att_sp_sub = orb_subscribe(ORB_ID(vehicle_attitude_setpoint));
orb_copy(ORB_ID(vehicle_attitude_setpoint), _att_sp_sub, &_att_sp);
orb_copy(ORB_ID(vehicle_local_position), _local_pos_sub, &_local_pos);
 
  1. if (!_attitude_setpoint_id) {
  2. 	if (_vehicle_status.is_vtol) {
  3. 		_attitude_setpoint_id = ORB_ID(mc_virtual_attitude_setpoint);
  4.  
  5. 	} else {
  6. 		_attitude_setpoint_id = ORB_ID(vehicle_attitude_setpoint);
  7. 	}
  8. }
//ardupilot/modules/PX4Firmware/src/modules/mc_att_control/mc_att_control_main.cpp
_v_att_sp_sub = orb_subscribe(ORB_ID(vehicle_attitude_setpoint)); 
  1. void
  2. MulticopterAttitudeControl::vehicle_attitude_setpoint_poll()
  3. {
  4. 	/* check if there is a new setpoint */
  5. 	bool updated;
  6. 	orb_check(_v_att_sp_sub, &updated);
  7.  
  8. 	if (updated) {
  9. 		orb_copy(ORB_ID(vehicle_attitude_setpoint), _v_att_sp_sub, &_v_att_sp);
  10. 	}
  11. }
突然发现//ardupilot/modules/PX4Firmware/src/modules/vtol_att_control//vtol_att_control_main.cpp中也有orb_copy(ORB_ID(vehicle_attitude_setpoint) 
  1. void
  2. VtolAttitudeControl::vehicle_attitude_setpoint_poll()
  3. {
  4. 	/* check if there is a new setpoint */
  5. 	bool updated;
  6. 	orb_check(_v_att_sp_sub, &updated);
  7.  
  8. 	if (updated) {
  9. 		orb_copy(ORB_ID(vehicle_attitude_setpoint), _v_att_sp_sub, &_v_att_sp);
  10. 	}
  11. }
  1. void
  2. VtolAttitudeControl::mc_virtual_att_sp_poll()
  3. {
  4. 	bool updated;
  5.  
  6. 	orb_check(_mc_virtual_att_sp_sub, &updated);
  7.  
  8. 	if (updated) {
  9. 		orb_copy(ORB_ID(mc_virtual_attitude_setpoint), _mc_virtual_att_sp_sub , &_mc_virtual_att_sp);
  10. 	}
  11.  
  12. }
  1. void VtolAttitudeControl::publish_att_sp()
  2. {
  3. 	if (_v_att_sp_pub != nullptr) {
  4. 		/* publish the attitude setpoint */
  5. 		orb_publish(ORB_ID(vehicle_attitude_setpoint), _v_att_sp_pub, &_v_att_sp);
  6.  
  7. 	} else {
  8. 		/* advertise and publish */
  9. 		_v_att_sp_pub = orb_advertise(ORB_ID(vehicle_attitude_setpoint), &_v_att_sp);
  10. 	}
  11. }
好奇葩啊，//ardupilot/modules/PX4Firmware/src/modules/vtol_att_control//vtol_att_control_main.cpp
字面意思应该是控制垂直起降的，怎么什么信息都会发布，都会接收啊？！暂时先放着
恩，产生一个新的想法：可以把这些订阅和发布的所有信息用图反映出来，大概就能直观的察觉到pixhawk的位置姿态估计和控制的结构，可以先看默认启动的那几个cpp文件
现在进入//ardupilot/modules/PX4Firmware/src/modules/mc_att_control/mc_att_control_main.cpp看看（上一篇写的有点乱，这一篇重新写） 
extern "C" __EXPORT int mc_att_control_main(int argc, char *argv[]);
  1. int mc_att_control_main(int argc, char *argv[])
  2. {
  3. 	if (argc < 2) {
  4. 		warnx("usage: mc_att_control {start|stop|status}");
  5. 		return 1;
  6. 	}
  7.  
  8. 	if (!strcmp(argv[1], "start")) {
  9.  
  10. 		if (mc_att_control::g_control != nullptr) {
  11. 			warnx("already running");
  12. 			return 1;
  13. 		}
  14.  
  15. 		mc_att_control::g_control = new MulticopterAttitudeControl;
  16.  
  17. 		if (mc_att_control::g_control == nullptr) {
  18. 			warnx("alloc failed");
  19. 			return 1;
  20. 		}
  21.  
  22. 		if (OK != mc_att_control::g_control->start()) {
  23. 			delete mc_att_control::g_control;
  24. 			mc_att_control::g_control = nullptr;
  25. 			warnx("start failed");
  26. 			return 1;
  27. 		}
  28.  
  29. 		return 0;
  30. 	}
  31.  
  32. 	if (!strcmp(argv[1], "stop")) {
  33. 		if (mc_att_control::g_control == nullptr) {
  34. 			warnx("not running");
  35. 			return 1;
  36. 		}
  37.  
  38. 		delete mc_att_control::g_control;
  39. 		mc_att_control::g_control = nullptr;
  40. 		return 0;
  41. 	}
  42.  
  43. 	if (!strcmp(argv[1], "status")) {
  44. 		if (mc_att_control::g_control) {
  45. 			warnx("running");
  46. 			return 0;
  47.  
  48. 		} else {
  49. 			warnx("not running");
  50. 			return 1;
  51. 		}
  52. 	}
  53.  
  54. 	warnx("unrecognized command");
  55. 	return 1;
  56. }
这里MulticopterAttitudeControl *g_control;*g_control是最大的一个类的实例
mc_att_control::g_control = new MulticopterAttitudeControl;相当于是分配空间
里面包含 
  1. /* fetch initial parameter values */
  2. 	parameters_update();
里面好多参数，不知道是不是上位机配置的参数，就是说是不是把上位机的各种关于飞行的参数存到了sd卡中，在通过这个函数跟新到程序中用于飞行器的控制？？
接着就进了start 
  1. int
  2. MulticopterAttitudeControl::start()
  3. {
  4. 	ASSERT(_control_task == -1);
  5.  
  6. 	/* start the task */
  7. 	_control_task = px4_task_spawn_cmd("mc_att_control",
  8. 					   SCHED_DEFAULT,
  9. 					   SCHED_PRIORITY_MAX - 5,
  10. 					   1500,
  11. 					   (px4_main_t)&MulticopterAttitudeControl::task_main_trampoline,
  12. 					   nullptr);
  13.  
  14. 	if (_control_task < 0) {
  15. 		warn("task start failed");
  16. 		return -errno;
  17. 	}
  18.  
  19. 	return OK;
  20. }
  1. void
  2. MulticopterAttitudeControl::task_main_trampoline(int argc, char *argv[])
  3. {
  4. 	mc_att_control::g_control->task_main();
  5. }
  1. void
  2. MulticopterAttitudeControl::task_main()
  3. {
  4. /**************************订阅各种信息**************************/
  5. 	/*
  6. 	 * do subscriptions
  7. 	 */
  8. 	_v_att_sp_sub = orb_subscribe(ORB_ID(vehicle_attitude_setpoint));
  9. 	_v_rates_sp_sub = orb_subscribe(ORB_ID(vehicle_rates_setpoint));
  10. 	_ctrl_state_sub = orb_subscribe(ORB_ID(control_state));
  11. 	_v_control_mode_sub = orb_subscribe(ORB_ID(vehicle_control_mode));
  12. 	_params_sub = orb_subscribe(ORB_ID(parameter_update));
  13. 	_manual_control_sp_sub = orb_subscribe(ORB_ID(manual_control_setpoint));
  14. 	_armed_sub = orb_subscribe(ORB_ID(actuator_armed));
  15. 	_vehicle_status_sub = orb_subscribe(ORB_ID(vehicle_status));
  16. 	_motor_limits_sub = orb_subscribe(ORB_ID(multirotor_motor_limits));
  17. /**************************初始化各种参数**************************/
  18. 	/* initialize parameters cache */
  19. 	parameters_update();
  20. /**************************启动机体姿态原函数**************************/
  21. 	/* wakeup source: vehicle attitude */
  22. 	px4_pollfd_struct_t fds[1];
  23.  
  24. 	fds[0].fd = _ctrl_state_sub;
  25. 	fds[0].events = POLLIN;
  26. /**************************大循环**************************/
  27. 	while (!_task_should_exit) {
  28. /等待100ms   为了获取数据/
  29. 		/* wait for up to 100ms for data */
  30. 		int pret = px4_poll(&fds[0], (sizeof(fds) / sizeof(fds[0])), 100);
  31.  
  32. 		/* timed out - periodic check for _task_should_exit */
  33. 		if (pret == 0) {
  34. 			continue;
  35. 		}
  36.  
  37. 		/* this is undesirable but not much we can do - might want to flag unhappy status */
  38. 		if (pret < 0) {
  39. 			warn("mc att ctrl: poll error %d, %d", pret, errno);
  40. 			/* sleep a bit before next try */
  41. 			usleep(100000);
  42. 			continue;
  43. 		}
  44.  
  45. 		perf_begin(_loop_perf);
  46. ///运行姿态变化控制器//
  47. 		/* run controller on attitude changes */
  48. 		if (fds[0].revents & POLLIN) {
  49. 			static uint64_t last_run = 0;
  50. 			float dt = (hrt_absolute_time() - last_run) / 1000000.0f;
  51. 			last_run = hrt_absolute_time();
  52. /防止dt太小或太大   2ms<dt<20ms
  53. 			/* guard against too small (< 2ms) and too large (> 20ms) dt's */
  54. 			if (dt < 0.002f) {
  55. 				dt = 0.002f;
  56.  
  57. 			} else if (dt > 0.02f) {
  58. 				dt = 0.02f;
  59. 			}
  60.  
  61. 			/* copy attitude and control state topics */
  62. 			orb_copy(ORB_ID(control_state), _ctrl_state_sub, &_ctrl_state);
  63.  
  64. 			/* check for updates in other topics */
  65. 			parameter_update_poll();
  66. 			vehicle_control_mode_poll();
  67. 			arming_status_poll();
  68. 			vehicle_manual_poll();
  69. 			vehicle_status_poll();
  70. 			vehicle_motor_limits_poll();
  71. 若xy轴的设定值大于阈值，那么不运行姿态控制/
  72. 			/* Check if we are in rattitude mode and the pilot is above the threshold on pitch
  73. 			 * or roll (yaw can rotate 360 in normal att control).  If both are true don't
  74. 			 * even bother running the attitude controllers */
  75. 			if (_vehicle_status.main_state == vehicle_status_s::MAIN_STATE_RATTITUDE) {
  76. 				if (fabsf(_manual_control_sp.y) > _params.rattitude_thres ||
  77. 				    fabsf(_manual_control_sp.x) > _params.rattitude_thres) {
  78. 					_v_control_mode.flag_control_attitude_enabled = false;
  79. 				}
  80. 			}
  81. /姿态控制使能
  82. 			if (_v_control_mode.flag_control_attitude_enabled) {
  83.  
  84. 				if (_ts_opt_recovery == nullptr) {
  85. 					// the  tailsitter recovery instance has not been created, thus, the vehicle
  86. 					// is not a tailsitter, do normal attitude control
  87. 					control_attitude(dt);
  88.  
  89. 				} else {
  90. 					vehicle_attitude_setpoint_poll();
  91. 					_thrust_sp = _v_att_sp.thrust;
  92. 					math::Quaternion q(_ctrl_state.q[0], _ctrl_state.q[1], _ctrl_state.q[2], _ctrl_state.q[3]);
  93. 					math::Quaternion q_sp(&_v_att_sp.q_d[0]);
  94. 					_ts_opt_recovery->setAttGains(_params.att_p, _params.yaw_ff);
  95. 					_ts_opt_recovery->calcOptimalRates(q, q_sp, _v_att_sp.yaw_sp_move_rate, _rates_sp);
  96.  
  97. 					/* limit rates */
  98. 					for (int i = 0; i < 3; i++) {
  99. 						_rates_sp(i) = math::constrain(_rates_sp(i), -_params.mc_rate_max(i), _params.mc_rate_max(i));
  100. 					}
  101. 				}
  102.  
  103. 				/* publish attitude rates setpoint */
  104. 				_v_rates_sp.roll = _rates_sp(0);
  105. 				_v_rates_sp.pitch = _rates_sp(1);
  106. 				_v_rates_sp.yaw = _rates_sp(2);
  107. 				_v_rates_sp.thrust = _thrust_sp;
  108. 				_v_rates_sp.timestamp = hrt_absolute_time();
  109.  
  110. 				if (_v_rates_sp_pub != nullptr) {
  111. 					orb_publish(_rates_sp_id, _v_rates_sp_pub, &_v_rates_sp);
  112.  
  113. 				} else if (_rates_sp_id) {
  114. 					_v_rates_sp_pub = orb_advertise(_rates_sp_id, &_v_rates_sp);
  115. 				}
  116.  
  117. 				//}
  118.  
  119. 			} else {
  120. /手动控制使能//
  121. 				/* attitude controller disabled, poll rates setpoint topic */
  122. 				if (_v_control_mode.flag_control_manual_enabled) {
  123. 				///特技模式/
  124. 					/* manual rates control - ACRO mode */
  125. 					_rates_sp = math::Vector<3>(_manual_control_sp.y, -_manual_control_sp.x,
  126. 								    _manual_control_sp.r).emult(_params.acro_rate_max);
  127. 					_thrust_sp = math::min(_manual_control_sp.z, MANUAL_THROTTLE_MAX_MULTICOPTER);
  128. 				/发布角速度设定值
  129. 					/* publish attitude rates setpoint */
  130. 					_v_rates_sp.roll = _rates_sp(0);
  131. 					_v_rates_sp.pitch = _rates_sp(1);
  132. 					_v_rates_sp.yaw = _rates_sp(2);
  133. 					_v_rates_sp.thrust = _thrust_sp;
  134. 					_v_rates_sp.timestamp = hrt_absolute_time();
  135.  
  136. 					if (_v_rates_sp_pub != nullptr) {
  137. 						orb_publish(_rates_sp_id, _v_rates_sp_pub, &_v_rates_sp);
  138.  
  139. 					} else if (_rates_sp_id) {
  140. 						_v_rates_sp_pub = orb_advertise(_rates_sp_id, &_v_rates_sp);
  141. 					}
  142.  
  143. 				} 
  144. 				//非特技模式//
  145. 				else {
  146. 				//速度设定值来自于vehicle_rates_setpoint//
  147. 				//vehicle_rates_setpoint来自于哪里？？///
  148. 					/* attitude controller disabled, poll rates setpoint topic */
  149. 					vehicle_rates_setpoint_poll();
  150. 					_rates_sp(0) = _v_rates_sp.roll;
  151. 					_rates_sp(1) = _v_rates_sp.pitch;
  152. 					_rates_sp(2) = _v_rates_sp.yaw;
  153. 					_thrust_sp = _v_rates_sp.thrust;
  154. 				}
  155. 			}
  156. ///速度控制/
  157. 			if (_v_control_mode.flag_control_rates_enabled) {
  158. 			//角速度控制
  159. 				control_attitude_rates(dt);
  160. 			//发布执行器控制///
  161. 			//这个就是pwm吗？？
  162. 				/* publish actuator controls */
  163. 				_actuators.control[0] = (PX4_ISFINITE(_att_control(0))) ? _att_control(0) : 0.0f;
  164. 				_actuators.control[1] = (PX4_ISFINITE(_att_control(1))) ? _att_control(1) : 0.0f;
  165. 				_actuators.control[2] = (PX4_ISFINITE(_att_control(2))) ? _att_control(2) : 0.0f;
  166. 				_actuators.control[3] = (PX4_ISFINITE(_thrust_sp)) ? _thrust_sp : 0.0f;
  167. 				_actuators.timestamp = hrt_absolute_time();
  168. 				_actuators.timestamp_sample = _ctrl_state.timestamp;
  169.  
  170. 				_controller_status.roll_rate_integ = _rates_int(0);
  171. 				_controller_status.pitch_rate_integ = _rates_int(1);
  172. 				_controller_status.yaw_rate_integ = _rates_int(2);
  173. 				_controller_status.timestamp = hrt_absolute_time();
  174.  
  175. 				if (!_actuators_0_circuit_breaker_enabled) {
  176. 					if (_actuators_0_pub != nullptr) {
  177.  
  178. 						orb_publish(_actuators_id, _actuators_0_pub, &_actuators);
  179. 						perf_end(_controller_latency_perf);
  180.  
  181. 					} else if (_actuators_id) {
  182. 						_actuators_0_pub = orb_advertise(_actuators_id, &_actuators);
  183. 					}
  184.  
  185. 				}
  186.  
  187. 				/* publish controller status */
  188. 				if (_controller_status_pub != nullptr) {
  189. 					orb_publish(ORB_ID(mc_att_ctrl_status), _controller_status_pub, &_controller_status);
  190.  
  191. 				} else {
  192. 					_controller_status_pub = orb_advertise(ORB_ID(mc_att_ctrl_status), &_controller_status);
  193. 				}
  194. 			}
  195. 		}
  196.  
  197. 		perf_end(_loop_perf);
  198. 	}
  199.  
  200. 	_control_task = -1;
  201. 	return;
  202. }
看看这3段控制程序的输入输出，大概能猜想他们的运行逻辑并不是并列的，很有可能_v_control_mode.flag_control_rates_enabled跟在_v_control_mode.flag_control_attitude_enabled之后，这样就形成了常用的外环角度(也就是这里的姿态)内环角速度(姿态速度)的控制结构，这个还需要细看里面的输入输出和标志位如何赋值控制运行逻辑。 
  1. /**
  2.  * Attitude controller.
  3.  * Input: 'vehicle_attitude_setpoint' topics (depending on mode)
  4.  * Output: '_rates_sp' vector, '_thrust_sp'
  5.  */
  6. /*************************注意看注释！！********************/
  7. /***********输入是'vehicle_attitude_setpoint'主题***********/
  8. /************输出是角速度设定值向量和油门设定值*************/
  9. void
  10. MulticopterAttitudeControl::control_attitude(float dt)
  11. {
  12. 	vehicle_attitude_setpoint_poll();//获取'vehicle_attitude_setpoint'主题
  13.  
  14. 	_thrust_sp = _v_att_sp.thrust;
  15. //构建姿态设定值的旋转矩阵就是vehicle_attitude_setpoint_poll()获取的//
  16. 	/* construct attitude setpoint rotation matrix */
  17. 	math::Matrix<3, 3> R_sp;
  18. 	R_sp.set(_v_att_sp.R_body);//这里做的事只是把订阅_v_att_sp复制到一个新的地方
  19. /****		void set(const float *d) {
  20.  ****		memcpy(data, d, sizeof(data));
  21.  ****		}
  22.  ****/
  23. //定义一个控制状态的四元数和相应的方向余弦矩阵(dcm)/
  24. 	/* get current rotation matrix from control state quaternions */
  25. 	math::Quaternion q_att(_ctrl_state.q[0], _ctrl_state.q[1], _ctrl_state.q[2], _ctrl_state.q[3]);
  26. 	math::Matrix<3, 3> R = q_att.to_dcm();
  27. /输入就准备好了，就是姿态设定值，并转变成能用的形式四元数和dcm///
  28. 	/* all input data is ready, run controller itself */
  29.  
  30. 	/* try to move thrust vector shortest way, because yaw response is slower than roll/pitch */
  31. 	math::Vector<3> R_z(R(0, 2), R(1, 2), R(2, 2));//控制状态的z轴
  32. 	math::Vector<3> R_sp_z(R_sp(0, 2), R_sp(1, 2), R_sp(2, 2));//设定姿态的z轴
  33.  
  34. 	/* axis and sin(angle) of desired rotation */轴与角的理想旋转
  35. 	math::Vector<3> e_R = R.transposed() * (R_z % R_sp_z);//transposed()换位函数,%是求叉积的意思
  36. /***		ardupilot/modules/PX4Firmware/src/lib/matrix/matrix/Matrix.hpp
  37.  ***		Matrix<Type, N, M> transpose() const
  38.  ***		{
  39.  ***			Matrix<Type, N, M> res;
  40.  ***			const Matrix<Type, M, N> &self = *this;
  41.  ***
  42.  ***			for (size_t i = 0; i < M; i++) {
  43.  ***				for (size_t j = 0; j < N; j++) {
  44.  ***					res(j, i) = self(i, j);
  45.  ***				}
  46.  ***			}
  47.  ***
  48.  ***			return res;
  49.  ***		}
  50.  ***/
  51. 	/* calculate angle error */计算角的误差
  52. 	float e_R_z_sin = e_R.length();
  53. 	float e_R_z_cos = R_z * R_sp_z;
  54. //计算姿态角度误差（姿态误差），一个数学知识背景：由公式a×b=︱a︱︱b︱sinθ，a•b=︱a︱︱b︱cosθ
  55. //这里的R_z和R_sp_z都是单位向量，模值为1，因此误差向量e_R（a×b叉积就是误差）的模就是sinθ，点积就是cosθ。
  56. /***	ardupilot/modules/PX4Firmware/src/lib/mathlib/math/Vector.hpp
  57.  ***	float length() const {
  58.  ***		float res = 0.0f;
  59.  ***
  60.  ***		for (unsigned int i = 0; i < N; i++)
  61.  ***			res += data[i] * data[i];
  62.  ***
  63.  ***		return sqrtf(res);
  64.  ***	}
  65.  ***/
  66. 	/* calculate weight for yaw control */计算yaw控制的权重
  67. 	float yaw_w = R_sp(2, 2) * R_sp(2, 2);//姿态设定值矩阵的第三行第三列元素的平方
  68.  
  69. 	/* calculate rotation matrix after roll/pitch only rotation */
  70. 	math::Matrix<3, 3> R_rp;
  71.  
  72. 	if (e_R_z_sin > 0.0f) {
  73. 		/* get axis-angle representation */
  74. 		float e_R_z_angle = atan2f(e_R_z_sin, e_R_z_cos);//得到z轴的误差角度
  75. 		math::Vector<3> e_R_z_axis = e_R / e_R_z_sin;
  76. /***	  R.transposed() * (R_z % R_sp_z)
  77.  ***	-----------------------------------
  78.  ***	||R.transposed() * (R_z % R_sp_z)||
  79.  ***/
  80. /***********这些式子的数据具体怎么算的可以看得出来，具体含义应该与理论知识有关吧**********/
  81. 		e_R = e_R_z_axis * e_R_z_angle;
  82.  
  83. 		/* cross product matrix for e_R_axis */
  84. 		math::Matrix<3, 3> e_R_cp;
  85. 		e_R_cp.zero();
  86. 		e_R_cp(0, 1) = -e_R_z_axis(2);
  87. 		e_R_cp(0, 2) = e_R_z_axis(1);
  88. 		e_R_cp(1, 0) = e_R_z_axis(2);
  89. 		e_R_cp(1, 2) = -e_R_z_axis(0);
  90. 		e_R_cp(2, 0) = -e_R_z_axis(1);
  91. 		e_R_cp(2, 1) = e_R_z_axis(0);
  92.  
  93. 		/* rotation matrix for roll/pitch only rotation */
  94. 		R_rp = R * (_I + e_R_cp * e_R_z_sin + e_R_cp * e_R_cp * (1.0f - e_R_z_cos));
  95.  
  96. 	} else {
  97. 		/* zero roll/pitch rotation */
  98. 		R_rp = R;
  99. 	}
  100.  
  101. 	/* R_rp and R_sp has the same Z axis, calculate yaw error */
  102. 	math::Vector<3> R_sp_x(R_sp(0, 0), R_sp(1, 0), R_sp(2, 0));
  103. 	math::Vector<3> R_rp_x(R_rp(0, 0), R_rp(1, 0), R_rp(2, 0));
  104. 	e_R(2) = atan2f((R_rp_x % R_sp_x) * R_sp_z, R_rp_x * R_sp_x) * yaw_w;
  105.  
  106. 	if (e_R_z_cos < 0.0f) {
  107. 		/* for large thrust vector rotations use another rotation method:
  108. 		 * calculate angle and axis for R -> R_sp rotation directly */
  109. 		math::Quaternion q;
  110. 		q.from_dcm(R.transposed() * R_sp);
  111. 		math::Vector<3> e_R_d = q.imag();
  112. 		e_R_d.normalize();
  113. 		e_R_d *= 2.0f * atan2f(e_R_d.length(), q(0));
  114.  
  115. 		/* use fusion of Z axis based rotation and direct rotation */
  116. 		float direct_w = e_R_z_cos * e_R_z_cos * yaw_w;//计算权重
  117. 		e_R = e_R * (1.0f - direct_w) + e_R_d * direct_w;
  118. 	}
  119. ///计算角速度设定值  也是矩阵表达的1*3矩阵///
  120. 	/* calculate angular rates setpoint */
  121. 	_rates_sp = _params.att_p.emult(e_R);
  122.  
  123. 	/* limit rates */限制角速度大小
  124. 	for (int i = 0; i < 3; i++) {
  125. 		if (_v_control_mode.flag_control_velocity_enabled && !_v_control_mode.flag_control_manual_enabled) {
  126. 			_rates_sp(i) = math::constrain(_rates_sp(i), -_params.auto_rate_max(i), _params.auto_rate_max(i));
  127. 		} else {
  128. 			_rates_sp(i) = math::constrain(_rates_sp(i), -_params.mc_rate_max(i), _params.mc_rate_max(i));
  129. 		}
  130. 	}
  131. //风向标模式，抑制yaw角速度(风向标模式是定航向的意思吗？？)///
  132. //yaw控制失能、速度模式使能、非手动模式/
  133. 	/* weather-vane mode, dampen yaw rate */
  134. 	if (_v_att_sp.disable_mc_yaw_control == true && _v_control_mode.flag_control_velocity_enabled && !_v_control_mode.flag_control_manual_enabled) {
  135. 		float wv_yaw_rate_max = _params.auto_rate_max(2) * _params.vtol_wv_yaw_rate_scale;
  136. 		_rates_sp(2) = math::constrain(_rates_sp(2), -wv_yaw_rate_max, wv_yaw_rate_max);
  137. 		// prevent integrator winding up in weathervane mode
  138. 		_rates_int(2) = 0.0f;
  139. 	}
  140. ///反馈角速度设定值(2)  矩阵中第三个元素/
  141. 	/* feed forward yaw setpoint rate */
  142. 	_rates_sp(2) += _v_att_sp.yaw_sp_move_rate * yaw_w * _params.yaw_ff;
  143. ///风向标模式，减小yaw角速度///
  144. 	/* weather-vane mode, scale down yaw rate */
  145. 	if (_v_att_sp.disable_mc_yaw_control == true && _v_control_mode.flag_control_velocity_enabled && !_v_control_mode.flag_control_manual_enabled) {
  146. 		float wv_yaw_rate_max = _params.auto_rate_max(2) * _params.vtol_wv_yaw_rate_scale;
  147. 		_rates_sp(2) = math::constrain(_rates_sp(2), -wv_yaw_rate_max, wv_yaw_rate_max);
  148. 		// prevent integrator winding up in weathervane mode
  149. 		_rates_int(2) = 0.0f;
  150. 	}
  151.  
  152. }
  1. /*
  2.  * Attitude rates controller.
  3.  * Input: '_rates_sp' vector, '_thrust_sp'
  4.  * Output: '_att_control' vector
  5.  */
  6. /*************************注意看注释！！********************/
  7. /************输入是角速度设定值向量、油门设定值*************/
  8. /**************************姿态控制值***********************/
  9. void
  10. MulticopterAttitudeControl::control_attitude_rates(float dt)
  11. {
  12. 	/* reset integral if disarmed */如果锁定，则复位角速度积分
  13. 	if (!_armed.armed || !_vehicle_status.is_rotary_wing) {
  14. 		_rates_int.zero();
  15. 	}
  16. //当前机体角速度设定值//
  17. /由此可知_ctrl_state表示的传感器测得的机体状态
  18. //_ctrl_state来源应该是ardupilot/modules/PX4Firmware/src/modules/attitude_estimator_q/attitude_estimator_q.main.cpp//
  19. 	/* current body angular rates */
  20. 	math::Vector<3> rates;
  21. 	rates(0) = _ctrl_state.roll_rate;
  22. 	rates(1) = _ctrl_state.pitch_rate;
  23. 	rates(2) = _ctrl_state.yaw_rate;
  24. 角速度误差=角速度设定值-机体角速度///
  25. 	/* angular rates error */
  26. 	math::Vector<3> rates_err = _rates_sp - rates;
  27. 	/_att_control的三个量分别为pitch、roll、yaw方向的pwm值
  28. /***	_att_control的三个量就是下面的Roll、pitch、yaw
  29.  ***	Motor1=(int)(COMD.THR + Roll - pitch + yaw);
  30.  ***	Motor2=(int)(COMD.THR + Roll + pitch - yaw);
  31.  ***	Motor3=(int)(COMD.THR - Roll + pitch + yaw);
  32.  ***	Motor4=(int)(COMD.THR - Roll - pitch - yaw);
  33.  ***/
  34. 	_att_control = _params.rate_p.emult(rates_err) + _params.rate_d.emult(_rates_prev - rates) / dt + _rates_int +
  35. 		       _params.rate_ff.emult(_rates_sp - _rates_sp_prev) / dt;
  36. 	_rates_sp_prev = _rates_sp;
  37. 	_rates_prev = rates;
  38. //注意这里用的算法pwm=P*error+D*(角速度_last-角速度_now)/dt+角速度积分+(角速度设定值_now-角速度设定值_last)/dt
  39. /***    Matrix<Type, M, N> emult(const Matrix<Type, M, N> &other) const
  40.  ***    {
  41.  ***        Matrix<Type, M, N> res;
  42.  ***        const Matrix<Type, M, N> &self = *this;
  43.  ***
  44.  ***        for (size_t i = 0; i < M; i++) {
  45.  ***            for (size_t j = 0; j < N; j++) {
  46.  ***                res(i , j) = self(i, j)*other(i, j);
  47.  ***            }
  48.  ***        }
  49.  ***
  50.  ***        return res;
  51.  ***    }
  52.  ***/
  53. 	/* update integral only if not saturated on low limit and if motor commands are not saturated */
  54. 	/下面是积分的计算,注意积分饱和问题
  55. 	if (_thrust_sp > MIN_TAKEOFF_THRUST && !_motor_limits.lower_limit && !_motor_limits.upper_limit) {
  56. 		for (int i = 0; i < 3; i++) {
  57. 			if (fabsf(_att_control(i)) < _thrust_sp) {
  58. 				float rate_i = _rates_int(i) + _params.rate_i(i) * rates_err(i) * dt;
  59.  
  60. 				if (PX4_ISFINITE(rate_i) && rate_i > -RATES_I_LIMIT && rate_i < RATES_I_LIMIT &&
  61. 				    _att_control(i) > -RATES_I_LIMIT && _att_control(i) < RATES_I_LIMIT) {
  62. 					_rates_int(i) = rate_i;
  63. 				}
  64. 			}
  65. 		}
  66. 	}
  67. }
  1. <pre name="code" class="cpp">	ardupilot/modules/PX4Firmware/src/modules/position_estimator_inav/position_estimator_inav_main.cpp
  2. 	int parameter_update_sub = orb_subscribe(ORB_ID(parameter_update));
  3. 	int actuator_sub = orb_subscribe(ORB_ID_VEHICLE_ATTITUDE_CONTROLS);
  4. 	int armed_sub = orb_subscribe(ORB_ID(actuator_armed));
  5. 	int sensor_combined_sub = orb_subscribe(ORB_ID(sensor_combined));
  6. 	int vehicle_attitude_sub = orb_subscribe(ORB_ID(vehicle_attitude));
  7. 	int optical_flow_sub = orb_subscribe(ORB_ID(optical_flow));
  8. 	int vehicle_gps_position_sub = orb_subscribe(ORB_ID(vehicle_gps_position));
  9. 	int vision_position_estimate_sub = orb_subscribe(ORB_ID(vision_position_estimate));
  10. 	int att_pos_mocap_sub = orb_subscribe(ORB_ID(att_pos_mocap));
  11. 	int distance_sensor_sub = orb_subscribe(ORB_ID(distance_sensor));
  12. 	int vehicle_rate_sp_sub = orb_subscribe(ORB_ID(vehicle_rates_setpoint));
  13. 	
  14. 	orb_copy(ORB_ID(sensor_combined), sensor_combined_sub, &sensor);
  15. 	orb_copy(ORB_ID(vehicle_attitude), vehicle_attitude_sub, &att);
  16. 	orb_copy(ORB_ID(parameter_update), parameter_update_sub, &update);
  17. 	orb_copy(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, actuator_sub, &actuator);//actuator_controls_0
  18. 	orb_copy(ORB_ID(actuator_armed), armed_sub, &armed);
  19. 	orb_copy(ORB_ID(distance_sensor), distance_sensor_sub, &lidar);
  20. 	orb_copy(ORB_ID(optical_flow), optical_flow_sub, &flow);
  21. 	orb_copy(ORB_ID(vehicle_rates_setpoint), vehicle_rate_sp_sub, &rates_setpoint);
  22. 	orb_copy(ORB_ID(vision_position_estimate), vision_position_estimate_sub, &vision);
  23. 	orb_copy(ORB_ID(att_pos_mocap), att_pos_mocap_sub, &mocap);
  24. 	orb_copy(ORB_ID(vehicle_gps_position), vehicle_gps_position_sub, &gps);
  25. 	
  26. 	orb_advert_t vehicle_local_position_pub = orb_advertise(ORB_ID(vehicle_local_position), &local_pos);
  27. 	orb_publish(ORB_ID(vehicle_local_position), vehicle_local_position_pub, &local_pos);
  28. 	vehicle_global_position_pub = orb_advertise(ORB_ID(vehicle_global_position), &global_pos);
  29. 	orb_publish(ORB_ID(vehicle_global_position), vehicle_global_position_pub, &global_pos);
  30.  
  31. 	
  32. 	
  33. 	ardupilot/modules/PX4Firmware/src/modules/attitude_estimator_q/attitude_estimator_q_main.cpp
  34. 	_sensors_sub = orb_subscribe(ORB_ID(sensor_combined));
  35. 	_vision_sub = orb_subscribe(ORB_ID(vision_position_estimate));
  36. 	_mocap_sub = orb_subscribe(ORB_ID(att_pos_mocap));
  37. 	_airspeed_sub = orb_subscribe(ORB_ID(airspeed));
  38. 	_params_sub = orb_subscribe(ORB_ID(parameter_update));
  39. 	_global_pos_sub = orb_subscribe(ORB_ID(vehicle_global_position));
  40.  
  41. 	orb_copy(ORB_ID(sensor_combined), _sensors_sub, &sensors)
  42. 	orb_copy(ORB_ID(vision_position_estimate), _vision_sub, &_vision);
  43. 	orb_copy(ORB_ID(att_pos_mocap), _mocap_sub, &_mocap);
  44. 	orb_copy(ORB_ID(airspeed), _airspeed_sub, &_airspeed);
  45. 	orb_copy(ORB_ID(vehicle_global_position), _global_pos_sub, &_gpos);
  46. 	orb_copy(ORB_ID(att_pos_mocap), _mocap_sub, &_mocap);
  47. 	
  48. 	orb_publish_auto(ORB_ID(vehicle_attitude), &_att_pub, &att, &att_inst, ORB_PRIO_HIGH);
  49. 	orb_publish_auto(ORB_ID(control_state), &_ctrl_state_pub, &ctrl_state, &ctrl_inst, ORB_PRIO_HIGH);
  50. 	orb_publish_auto(ORB_ID(estimator_status), &_est_state_pub, &est, &est_inst, ORB_PRIO_HIGH);
  51. 	
  52. 	
  53. 	
  54. 	ardupilot/modules/PX4Firmware/src/modules/mc_pos_control/mc_pos_control_main.cpp
  55. 	_vehicle_status_sub = orb_subscribe(ORB_ID(vehicle_status));
  56. 	_ctrl_state_sub = orb_subscribe(ORB_ID(control_state));
  57. 	_att_sp_sub = orb_subscribe(ORB_ID(vehicle_attitude_setpoint));
  58. 	_control_mode_sub = orb_subscribe(ORB_ID(vehicle_control_mode));
  59. 	_params_sub = orb_subscribe(ORB_ID(parameter_update));
  60. 	_manual_sub = orb_subscribe(ORB_ID(manual_control_setpoint));
  61. 	_arming_sub = orb_subscribe(ORB_ID(actuator_armed));
  62. 	_local_pos_sub = orb_subscribe(ORB_ID(vehicle_local_position));
  63. 	_pos_sp_triplet_sub = orb_subscribe(ORB_ID(position_setpoint_triplet));
  64. 	_local_pos_sp_sub = orb_subscribe(ORB_ID(vehicle_local_position_setpoint));
  65. 	_global_vel_sp_sub = orb_subscribe(ORB_ID(vehicle_global_velocity_setpoint));
  66. 	
  67. 	orb_copy(ORB_ID(vehicle_status), _vehicle_status_sub, &_vehicle_status);
  68. 	orb_copy(ORB_ID(control_state), _ctrl_state_sub, &_ctrl_state);
  69. 	orb_copy(ORB_ID(vehicle_attitude_setpoint), _att_sp_sub, &_att_sp);
  70. 	orb_copy(ORB_ID(vehicle_control_mode), _control_mode_sub, &_control_mode);
  71. 	orb_copy(ORB_ID(manual_control_setpoint), _manual_sub, &_manual);
  72. 	orb_copy(ORB_ID(actuator_armed), _arming_sub, &_arming);
  73. 	orb_copy(ORB_ID(vehicle_local_position), _local_pos_sub, &_local_pos);
  74. 	orb_copy(ORB_ID(position_setpoint_triplet), _pos_sp_triplet_sub, &_pos_sp_triplet);
  75. 	
  76. 	
  77. 	orb_publish(_attitude_setpoint_id, _att_sp_pub, &_att_sp);
  78. 	_att_sp_pub = orb_advertise(_attitude_setpoint_id, &_att_sp);
  79. 	if (!_attitude_setpoint_id) {
  80. 		if (_vehicle_status.is_vtol) {
  81. 			_attitude_setpoint_id = ORB_ID(mc_virtual_attitude_setpoint);
  82.  
  83. 		} else {
  84. 			_attitude_setpoint_id = ORB_ID(vehicle_attitude_setpoint);
  85. 		}
  86. 	}
  87. 	orb_publish(ORB_ID(vehicle_global_velocity_setpoint), _global_vel_sp_pub, &_global_vel_sp);
  88. 	_global_vel_sp_pub = orb_advertise(ORB_ID(vehicle_global_velocity_setpoint), &_global_vel_sp);
  89. 	
  90. 	
  91. 	
  92. 	ardupilot/modules/PX4Firmware/src/modules/mc_att_control/mc_att_control_main.cpp
  93. 	_v_att_sp_sub = orb_subscribe(ORB_ID(vehicle_attitude_setpoint));
  94. 	_v_rates_sp_sub = orb_subscribe(ORB_ID(vehicle_rates_setpoint));
  95. 	_ctrl_state_sub = orb_subscribe(ORB_ID(control_state));
  96. 	_v_control_mode_sub = orb_subscribe(ORB_ID(vehicle_control_mode));
  97. 	_params_sub = orb_subscribe(ORB_ID(parameter_update));
  98. 	_manual_control_sp_sub = orb_subscribe(ORB_ID(manual_control_setpoint));
  99. 	_armed_sub = orb_subscribe(ORB_ID(actuator_armed));
  100. 	_vehicle_status_sub = orb_subscribe(ORB_ID(vehicle_status));
  101. 	_motor_limits_sub = orb_subscribe(ORB_ID(multirotor_motor_limits));
  102. 	
  103. 	orb_copy(ORB_ID(vehicle_attitude_setpoint), _v_att_sp_sub, &_v_att_sp);
  104. 	orb_copy(ORB_ID(control_state), _ctrl_state_sub, &_ctrl_state);
  105. 	orb_copy(ORB_ID(parameter_update), _params_sub, ¶m_update);
  106. 	orb_copy(ORB_ID(vehicle_control_mode), _v_control_mode_sub, &_v_control_mode);
  107. 	orb_copy(ORB_ID(actuator_armed), _armed_sub, &_armed);
  108. 	orb_copy(ORB_ID(manual_control_setpoint), _manual_control_sp_sub, &_manual_control_sp);
  109. 	orb_copy(ORB_ID(vehicle_status), _vehicle_status_sub, &_vehicle_status);
  110. 	if (!_rates_sp_id) {
  111. 			if (_vehicle_status.is_vtol) {
  112. 				_rates_sp_id = ORB_ID(mc_virtual_rates_setpoint);
  113. 				_actuators_id = ORB_ID(actuator_controls_virtual_mc);
  114.  
  115. 			} else {
  116. 				_rates_sp_id = ORB_ID(vehicle_rates_setpoint);
  117. 				_actuators_id = ORB_ID(actuator_controls_0);
  118. 			}
  119. 		}
  120. 	}
  121. 	orb_copy(ORB_ID(multirotor_motor_limits), _motor_limits_sub, &_motor_limits);
  122. 	orb_copy(ORB_ID(vehicle_rates_setpoint), _v_rates_sp_sub, &_v_rates_sp);
  123. 	
  124. 	orb_publish(_rates_sp_id, _v_rates_sp_pub, &_v_rates_sp);
  125. 	_v_rates_sp_pub = orb_advertise(_rates_sp_id, &_v_rates_sp);
  126. 	orb_publish(_actuators_id, _actuators_0_pub, &_actuators);
  127. 	_actuators_0_pub = orb_advertise(_actuators_id, &_actuators);
  128. 	orb_publish(ORB_ID(mc_att_ctrl_status), _controller_status_pub, &_controller_status);
  129. 	_controller_status_pub = orb_advertise(ORB_ID(mc_att_ctrl_status), &_controller_status);
 
  

 


  上图是 

 


  姿态估计 Attitude_estimator_q  位置估计 position_estimator_inav  姿态控制 mc_att_control  位置控制 mc_pos_control  

 


  的逻辑图 

 

 
 
  1. #ifdef __cplusplus
  2. struct __EXPORT vehicle_local_position_s {
  3. #else
  4. struct vehicle_local_position_s {
  5. #endif
  6. 	uint64_t timestamp;
  7. 	bool xy_valid;
  8. 	bool z_valid;
  9. 	bool v_xy_valid;
  10. 	bool v_z_valid;
  11. 	float x;
  12. 	float y;
  13. 	float z;
  14. 	float vx;
  15. 	float vy;
  16. 	float vz;
  17. 	float yaw;
  18. 	bool xy_global;
  19. 	bool z_global;
  20. 	uint64_t ref_timestamp;
  21. 	double ref_lat;
  22. 	double ref_lon;
  23. 	float ref_alt;
  24. 	float dist_bottom;
  25. 	float dist_bottom_rate;
  26. 	uint64_t surface_bottom_timestamp;
  27. 	bool dist_bottom_valid;
  28. 	float eph;
  29. 	float epv;
  30. #ifdef __cplusplus
  31. #endif
  32. };
  33. #ifdef __cplusplus
  34. struct __EXPORT vehicle_global_position_s {
  35. #else
  36. struct vehicle_global_position_s {
  37. #endif
  38. 	uint64_t timestamp;
  39. 	uint64_t time_utc_usec;
  40. 	double lat;
  41. 	double lon;
  42. 	float alt;
  43. 	float vel_n;
  44. 	float vel_e;
  45. 	float vel_d;
  46. 	float yaw;
  47. 	float eph;
  48. 	float epv;
  49. 	float terrain_alt;
  50. 	bool terrain_alt_valid;
  51. 	bool dead_reckoning;
  52. 	float pressure_alt;
  53. #ifdef __cplusplus
  54. #endif
  55. };
  56. #ifdef __cplusplus
  57. struct __EXPORT vehicle_attitude_s {
  58. #else
  59. struct vehicle_attitude_s {
  60. #endif
  61. 	uint64_t timestamp;
  62. 	float roll;
  63. 	float pitch;
  64. 	float yaw;
  65. 	float rollspeed;
  66. 	float pitchspeed;
  67. 	float yawspeed;
  68. 	float rollacc;
  69. 	float pitchacc;
  70. 	float yawacc;
  71. 	float rate_vibration;
  72. 	float accel_vibration;
  73. 	float mag_vibration;
  74. 	float rate_offsets[3];
  75. 	float R[9];
  76. 	float q[4];
  77. 	float g_comp[3];
  78. 	bool R_valid;
  79. 	bool q_valid;
  80. #ifdef __cplusplus
  81.  
  82. #endif
  83. };
  84. #ifdef __cplusplus
  85. struct __EXPORT control_state_s {
  86. #else
  87. struct control_state_s {
  88. #endif
  89. 	uint64_t timestamp;
  90. 	float x_acc;
  91. 	float y_acc;
  92. 	float z_acc;
  93. 	float x_vel;
  94. 	float y_vel;
  95. 	float z_vel;
  96. 	float x_pos;
  97. 	float y_pos;
  98. 	float z_pos;
  99. 	float airspeed;
  100. 	bool airspeed_valid;
  101. 	float vel_variance[3];
  102. 	float pos_variance[3];
  103. 	float q[4];
  104. 	float roll_rate;
  105. 	float pitch_rate;
  106. 	float yaw_rate;
  107. 	float horz_acc_mag;
  108. #ifdef __cplusplus
  109.  
  110. #endif
  111. };
  112. #ifdef __cplusplus
  113. struct __EXPORT vehicle_attitude_setpoint_s {
  114. #else
  115. struct vehicle_attitude_setpoint_s {
  116. #endif
  117. 	uint64_t timestamp;
  118. 	float roll_body;
  119. 	float pitch_body;
  120. 	float yaw_body;
  121. 	float yaw_sp_move_rate;
  122. 	float R_body[9];
  123. 	bool R_valid;
  124. 	float q_d[4];
  125. 	bool q_d_valid;
  126. 	float q_e[4];
  127. 	bool q_e_valid;
  128. 	float thrust;
  129. 	bool roll_reset_integral;
  130. 	bool pitch_reset_integral;
  131. 	bool yaw_reset_integral;
  132. 	bool fw_control_yaw;
  133. 	bool disable_mc_yaw_control;
  134. 	bool apply_flaps;
  135. #ifdef __cplusplus
  136.  
  137. #endif
  138. };
  139.  
  140. #ifdef __cplusplus
  141. struct __EXPORT vehicle_rates_setpoint_s {
  142. #else
  143. struct vehicle_rates_setpoint_s {
  144. #endif
  145. 	uint64_t timestamp;
  146. 	float roll;
  147. 	float pitch;
  148. 	float yaw;
  149. 	float thrust;
  150. #ifdef __cplusplus
  151.  
  152. #endif
  153. };
  154. #ifdef __cplusplus
  155. struct __EXPORT actuator_controls_0_s {
  156. #else
  157. struct actuator_controls_0_s {
  158. #endif
  159. 	uint64_t timestamp;
  160. 	uint64_t timestamp_sample;
  161. 	float control[8];
  162. #ifdef __cplusplus
  163. 	static const uint8_t NUM_ACTUATOR_CONTROLS = 8;
  164. 	static const uint8_t NUM_ACTUATOR_CONTROL_GROUPS = 4;
  165.  
  166. #endif
  167. };
这是用到的结构体 

 


  上述结构还没有包括传感器的数据流，现在还没时间看到每个传感器。 

 


  恩，这篇就先到这吧，内容还挺多的。^_^ 
 
 至于传感器系统、pwm系统，再另外写一篇blog。 
 
 

 

 
 
 

 

 
 如果您觉得此文对您的发展有用，请随意打赏。  
 
 
 您的鼓励将是笔者书写高质量文章的最大动力^_^!! 
 
 

 

 
 
  

 

 
 
 


                

        
声明：本文内容由网友自发贡献，不代表【wpsshop博客】立场，版权归原作者所有，本站不承担相应法律责任。如您发现有侵权的内容，请联系我们。转载请注明出处：https://www.wpsshop.cn/w/IT小白/article/detail/488870
推荐阅读
相关标签
Copyright ©  2003-2013 www.wpsshop.cn  版权所有，并保留所有权利。

  
闽ICP备14008679号