pixhawk 光流--位置估计--姿态估计--位置控制--姿态控制

原创 2016年06月01日 20:09:48

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

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

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

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

voidsetup()
{
    hal.console->println("OpticalFlowlibrary test ver 1.6");
 
    hal.scheduler->delay(1000);
 
    // flowSensor initialization
    optflow.init();
 
    if (!optflow.healthy()) {
        hal.console->print("Failed toinitialise PX4Flow ");
    }
 
    hal.scheduler->delay(1000);只需要看
}
voidloop()
{
    hal.console->println("this onlytests compilation succeeds");
 
    hal.scheduler->delay(5000);
}

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

跳转到optflow.init();

voidOpticalFlow::init(void)
{
    if (backend != NULL) {
        backend->init();
    } else {
        _enabled = 0;
    }
}

跳转到init();

classOpticalFlow_backend
{
    friend class OpticalFlow;
 
public:
    // constructor
    OpticalFlow_backend(OpticalFlow&_frontend) : frontend(_frontend) {}
 
    // init - initialise sensor
    virtual void init() = 0;
 
    // read latest values from sensor and fillin x,y and totals.
    virtual void update() = 0;
 
protected:
    // access to frontend
    OpticalFlow &frontend;
 
    // update the frontend
    void _update_frontend(const structOpticalFlow::OpticalFlow_state &state);
 
    // get the flow scaling parameters
    Vector2f _flowScaler(void) const { returnVector2f(frontend._flowScalerX, frontend._flowScalerY); }
 
    // get the yaw angle in radians
    float _yawAngleRad(void) const { returnradians(float(frontend._yawAngle_cd) * 0.01f); }
};

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

怎么找到它的实例化呢?

Ctrl+H全局搜索public OpticalFlow_backend


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

路径是:ardupilot/libraries/AP_OpticalFlow/ AP_OpticalFlow_PX4.h
class AP_OpticalFlow_PX4 : public OpticalFlow_backend
{
public:
    /// constructor
    AP_OpticalFlow_PX4(OpticalFlow &_frontend);

    // init - initialise the sensor
    void init();

    // update - read latest values from sensor and fill in x,y and totals.
    void update(void);

private:
    int         _fd;                // file descriptor for sensor
    uint64_t    _last_timestamp;    // time of last update (used to avoid processing old reports)
};
void init();就是实例化,继续搜索AP_OpticalFlow_PX4:: init

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


本体就是

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

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

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

/****************************************************************************
 *Name: open
 *
 *Description:
 *   Standard 'open' interface
 *
 ****************************************************************************/
int open(const char *path, int oflags, ...)
{
  FARstruct filelist *list;
  FARstruct inode    *inode;
  FARconst char      *relpath = NULL;
#if defined(CONFIG_FILE_MODE) ||!defined(CONFIG_DISABLE_MOUNTPOINT)
 mode_t               mode = 0666;
#endif
 int                  ret;
 int                  fd;
  /*Get the thread-specific file list */得到具体线程列表
 list = sched_getfiles();
  if(!list)
    {
     ret = EMFILE;
     goto errout;
    }
#ifdef CONFIG_FILE_MODE
# ifdef CONFIG_CPP_HAVE_WARNING
#   warning "File creation not implemented"
#  endif
  /*If the file is opened for creation, then get the mode bits */
  if(oflags & (O_WRONLY|O_CREAT) != 0)
    {
     va_list ap;
     va_start(ap, oflags);
     mode = va_arg(ap, mode_t);
     va_end(ap);
    }
#endif
  /*Get an inode for this file */得到索引节点
 inode = inode_find(path, &relpath);
  if(!inode)
    {
     /* "O_CREAT is not set and the named file does not exist.  Or, a
      * directory component in pathname does not exist or is a dangling
      * symbolic link."
      */
     ret = ENOENT;
     goto errout;
    }
  /*Verify that the inode is valid and either a "normal" or amountpoint.  We
   *specifically exclude block drivers.
   */
#ifndef CONFIG_DISABLE_MOUNTPOINT
  if((!INODE_IS_DRIVER(inode) && !INODE_IS_MOUNTPT(inode)) ||!inode->u.i_ops)
#else
  if(!INODE_IS_DRIVER(inode) || !inode->u.i_ops)
#endif
    {
     ret = ENXIO;
     goto errout_with_inode;
    }
  /*Make sure that the inode supports the requested access */
  ret= inode_checkflags(inode, oflags);
  if(ret < 0)
    {
     ret = -ret;
     goto errout_with_inode;
    }
  /*Associate the inode with a file structure */
  fd= files_allocate(inode, oflags, 0, 0);
  if(fd < 0)
    {
     ret = EMFILE;
     goto errout_with_inode;
    }
  /*Perform the driver open operation.  NOTEthat the open method may be
   *called many times.  The driver/mountpointlogic should handled this
   *because it may also be closed that many times.
   */
  ret= OK;
  if(inode->u.i_ops->open)
    {
#ifndef CONFIG_DISABLE_MOUNTPOINT
     if (INODE_IS_MOUNTPT(inode))
       {
         ret = inode->u.i_mops->open((FAR structfile*)&list->fl_files[fd],
                                      relpath,oflags, mode);
       }
     else
#endif
       {
         ret = inode->u.i_ops->open((FAR structfile*)&list->fl_files[fd]);
       }
    }
  if(ret < 0)
    {
     ret = -ret;
     goto errout_with_fd;
    }
 return fd;
 errout_with_fd:
 files_release(fd);
 errout_with_inode:
 inode_release(inode);
 errout:
 set_errno(ret);
 return ERROR;
}
跳转到list = sched_getfiles();
路径是ardupilot/modules/PX4NuttX/nuttx/sched/sched_getfiles.c
/************************************************************************
 *Name: sched_getfiles
 *
 *Description:
 *  Return a pointer to the file list for this thread
 *
 *Parameters:
 *   None
 *
 *Return Value:
 *   Apointer to the errno.
 *
 *Assumptions:
 *
 ************************************************************************/
#if CONFIG_NFILE_DESCRIPTORS > 0
FAR struct filelist *sched_getfiles(void)
{
  FARstruct tcb_s *rtcb = (FAR struct tcb_s*)g_readytorun.head;
  FARstruct task_group_s *group = rtcb->group;
  /*The group may be NULL under certain conditions. For example, if
   *debug output is attempted from the IDLE thead before the group has
   *been allocated.  I have only seen thiscase when memory management
   *debug is enabled.
   */
  if(group)
    {
     return &group->tg_filelist;
    }
  /*Higher level logic must handle the NULL gracefully */
 return NULL;
}
#endif /* CONFIG_NFILE_DESCRIPTORS */
跳转到inode = inode_find(path, &relpath);
路径是ardupilot/modules/PX4NuttX/nuttx/fs/fs_inodefine.c
/****************************************************************************
 *Name: inode_find
 *
 *Description:
 *   Thisis called from the open() logic to get a reference to the inode
 *  associated with a path.
 *
 ****************************************************************************/
//得到一个inode与路径相关的引用
FAR struct inode *inode_find(FAR const char*path, FAR const char **relpath)
{
  FARstruct inode *node;
  if(!*path || path[0] != '/')
    {
     return NULL;
    }
  /*Find the node matching the path.  Iffound, increment the count of
   *references on the node.
   */
 inode_semtake();
 node = inode_search(&path, (FAR struct inode**)NULL, (FAR struct inode**)NULL,relpath);
  if(node)
    {
     node->i_crefs++;
    }
 inode_semgive();
 return node;
}

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

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

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

继续往下看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)

/****************************************************************************
 *Name: ioctl
 *
 *Description:
 *  Perform device specific operations.
 *
 *Parameters:
 *  fd       File/socket descriptor ofdevice
 *  req      The ioctl command
 *  arg      The argument of the ioctlcmd
 *
 *Return:
 *  >=0 on success (positive non-zero values are cmd-specific)
 *   -1on failure withi errno set properly:
 *
 *  EBADF
 *    'fd' is not a valid descriptor.
 *  EFAULT
 *    'arg' references an inaccessible memory area.
 *  EINVAL
 *    'cmd' or 'arg' is not valid.
 *  ENOTTY
 *    'fd' is not associated with a character special device.
 *  ENOTTY
 *     The specified request does not apply to the kind of object that the
 *     descriptor 'fd' references.
 *
 ****************************************************************************/
int ioctl(int fd, int req, unsigned longarg)
{
  interr;
#if CONFIG_NFILE_DESCRIPTORS > 0
  FARstruct filelist *list;
  FARstruct file     *this_file;
  FARstruct inode    *inode;
  int                  ret = OK;
  /*Did we get a valid file descriptor? */
  if((unsigned int)fd >= CONFIG_NFILE_DESCRIPTORS)
#endif
    {
      /* Perform the socket ioctl */
#if defined(CONFIG_NET) &&CONFIG_NSOCKET_DESCRIPTORS > 0
     if ((unsigned int)fd <(CONFIG_NFILE_DESCRIPTORS+CONFIG_NSOCKET_DESCRIPTORS))
       {
         return netdev_ioctl(fd, req, arg);
       }
     else
#endif
       {
         err = EBADF;
         goto errout;
       }
    }
#if CONFIG_NFILE_DESCRIPTORS > 0
  /*Get the thread-specific file list */
 list = sched_getfiles();
  if(!list)
    {
     err = EMFILE;
     goto errout;
    }
  /*Is a driver registered? Does it support the ioctl method? */
 this_file = &list->fl_files[fd];
 inode     = this_file->f_inode;
  if(inode && inode->u.i_ops && inode->u.i_ops->ioctl)
    {
     /* Yes, then let it perform the ioctl */
      ret = (int)inode->u.i_ops->ioctl(this_file,req, arg);
     if (ret < 0)
       {
         err = -ret;
         goto errout;
       }
    }
 return ret;
#endif
errout:
 set_errno(err);
 return ERROR;
}

这句话是这个函数的核心

/* 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

int
PX4FLOW::ioctl(structfile *filp, int cmd, unsigned long arg)
{
         switch(cmd) {
         caseSENSORIOCSPOLLRATE: {
                            switch(arg) {
                            /*switching to manual polling */
                            caseSENSOR_POLLRATE_MANUAL:
                                     stop();
                                     _measure_ticks= 0;
                                     returnOK;
                            /*external signalling (DRDY) not supported */
                            case SENSOR_POLLRATE_EXTERNAL:
                            /*zero would be bad */
                            case0:
                                     return-EINVAL;
                            /*set default/max polling rate */
                            caseSENSOR_POLLRATE_MAX:
                            caseSENSOR_POLLRATE_DEFAULT: {
                                               /*do we need to start internal polling? */
                                               boolwant_start = (_measure_ticks == 0);
                                               /*set interval for next measurement to minimum legal value */
                                               _measure_ticks= USEC2TICK(PX4FLOW_CONVERSION_INTERVAL);
                                               /*if we need to start the poll state machine, do it */
                                               if(want_start) {
                                                        start();
                                               }
                                               returnOK;
                                     }
                            /*adjust to a legal polling interval in Hz */
                            default:{
                                               /*do we need to start internal polling? */
                                               boolwant_start = (_measure_ticks == 0);
                                               /*convert hz to tick interval via microseconds */
                                               unsignedticks = USEC2TICK(1000000 / arg);
                                               /*check against maximum rate */
                                               if(ticks < USEC2TICK(PX4FLOW_CONVERSION_INTERVAL)) {
                                                        return-EINVAL;
                                               }
                                               /*update interval for next measurement */
                                               _measure_ticks= ticks;
                                               /*if we need to start the poll state machine, do it */
                                               if(want_start) {
                                                        start();
                                               }
                                               returnOK;
                                     }
                            }
                   }
         caseSENSORIOCGPOLLRATE:
                   if(_measure_ticks == 0) {
                            returnSENSOR_POLLRATE_MANUAL;
                   }
                   return(1000 / _measure_ticks);
         caseSENSORIOCSQUEUEDEPTH: {
                            /*lower bound is mandatory, upper bound is a sanity check */
                            if((arg < 1) || (arg > 100)) {
                                     return-EINVAL;
                            }
                            irqstate_t flags = irqsave();
                            if(!_reports->resize(arg)) {
                                     irqrestore(flags);
                                     return-ENOMEM;
                            }
                            irqrestore(flags);
                            returnOK;
                   }
         caseSENSORIOCGQUEUEDEPTH:
                   return_reports->size();
         caseSENSORIOCSROTATION:
                   _sensor_rotation= (enum Rotation)arg;
                   returnOK;
         caseSENSORIOCGROTATION:
                   return_sensor_rotation;
         caseSENSORIOCRESET:
                   /*XXX implement this */
                   return-EINVAL;
         default:
                   /*give it to the superclass */
                   returnI2C::ioctl(filp, cmd, arg);
         }
}

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

case SENSORIOCSPOLLRATE: {
                            switch(arg) {
                            /*switching to manual polling */
                            caseSENSOR_POLLRATE_MANUAL:
                                     stop();
                                     _measure_ticks= 0;
                                     returnOK;
                            /*external signalling (DRDY) not supported */
                            case SENSOR_POLLRATE_EXTERNAL:
                            /*zero would be bad */
                            case0:
                                     return-EINVAL;
                            /*set default/max polling rate */
                            caseSENSOR_POLLRATE_MAX:
                            caseSENSOR_POLLRATE_DEFAULT: {
                                               /*do we need to start internal polling? */
                                               boolwant_start = (_measure_ticks == 0);
                                               /*set interval for next measurement to minimum legal value */
                                               _measure_ticks= USEC2TICK(PX4FLOW_CONVERSION_INTERVAL);
                                               /*if we need to start the poll state machine, do it */
                                               if(want_start) {
                                                        start();
                                               }
                                               returnOK;
                                     }
                            /*adjust to a legal polling interval in Hz */
                            default:{
                                               /*do we need to start internal polling? */
                                               boolwant_start = (_measure_ticks == 0);
                                               /*convert hz to tick interval via microseconds */
                                               unsignedticks = USEC2TICK(1000000 / arg);
                                               /*check against maximum rate */
                                               if(ticks < USEC2TICK(PX4FLOW_CONVERSION_INTERVAL)) {
                                                        return-EINVAL;
                                               }
                                               /*update interval for next measurement */
                                               _measure_ticks= ticks;
                                               /* if we need to start thepoll state machine, do it */
                                               if(want_start) {
                                                        start();
                                               }
                                               returnOK;
                                     }

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();

void
PX4FLOW::start()
{
         /*reset the report ring and state machine *///复位报告环和状态机
         _collect_phase= false;
         _reports->flush();
         /*schedule a cycle to start things *///安排一个周期开始一些事情
         work_queue(HPWORK,&_work, (worker_t)&PX4FLOW::cycle_trampoline, this, 1);
         /*notify about state change */
         structsubsystem_info_s info = {
                   true,
                   true,
                   true,
                   subsystem_info_s::SUBSYSTEM_TYPE_OPTICALFLOW
         };
         staticorb_advert_t pub = nullptr;
         if(pub != nullptr) {
                   orb_publish(ORB_ID(subsystem_info),pub, &info);
         }else {
                   pub= orb_advertise(ORB_ID(subsystem_info), &info);
         }
}

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

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


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

在看一下调用的函数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)

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

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

也就是说

  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的定义

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

跳转至irqsave();

static inline irqstate_t irqsave(void)
{
#ifdef CONFIG_ARMV7M_USEBASEPRI
 uint8_t basepri = getbasepri();
 setbasepri(NVIC_SYSH_DISABLE_PRIORITY);
 return (irqstate_t)basepri;
#else
 unsigned short primask;
  /*Return the current value of primask register and set
   *bit 0 of the primask register to disable interrupts
   */
 __asm__ __volatile__
    (
    "\tmrs    %0, primask\n"
    "\tcpsid  i\n"
    : "=r" (primask)
    :
    : "memory");
 return primask;
#endif
}

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()

/************************************************************************
 * Name: kill
 *
 * Description:
 *   Thekill() system call can be used to send any signal to any task.
 *
 *  Limitation: Sending of signals to 'process groups' is not
 *  supported in NuttX
 *
 * Parameters:
 *   pid- The id of the task to receive the signal. The POSIX kill
 *    specification encodes process group information as zero and
 *    negative pid values.  Onlypositive, non-zero values of pid are
 *    supported by this implementation.
 *  signo - The signal number to send. If signo is zero, no signal is
 *    sent, but all error checking is performed.
 *
 * Returned Value:
 *    Onsuccess (at least one signal was sent), zero is returned.  On
 *   error, -1 is returned, and errno is set appropriately:
 *
 *   EINVAL An invalid signal was specified.
 *   EPERM  The process does not havepermission to send the
 *          signal to any of the target processes.
 *   ESRCH  The pid or process groupdoes not exist.
 *   ENOSYS Do not support sending signals to process groups.
 *
 * Assumptions:
 *
 ************************************************************************/
注释翻译:
这个kill()系统调用可用于任何信号发送到任何任务。
限制:信号发送到“进程组”在NuttX不支持
参数:
pid -任务的ID用来接收信号。如果pid的值是0或负的,POSIX的kill规范会编码进程组信息
    只有正的pid才会被执行
signo  -需发送的信号数量。如果signo为0,就是没有信号发送,但是所有错误检查会被执行
返回值:
0,  成功;-1,发生错误,并且会返回错误码:
EINVAL   无效的信号指定。
EPERM   进程没有权限将信号发送到任何目标进程。
ESRCH    PID或进程组不存在。
ENOSYS  不支持发送的信号来处理组。
int kill(pid_t pid, int signo)
{
#ifdef CONFIG_SCHED_HAVE_PARENT
 FAR struct tcb_s *rtcb = (FAR struct tcb_s *)g_readytorun.head;
#endif
 siginfo_t info;
 int ret;
 /* We do not support sending signals to process groups */
 if (pid <= 0)
   {
      ret = -ENOSYS;
      goto errout;
   }
 /* Make sure that the signal is valid */
 if (!GOOD_SIGNO(signo))
   {
      ret = -EINVAL;
      goto errout;
}
//至此都是保证pid大于0
 /* Keep things stationary through the following */
 sched_lock();
 /* Create the siginfo structure */创建siginfo结构体
 info.si_signo           = signo;
 info.si_code            = SI_USER;
 info.si_value.sival_ptr = NULL;
#ifdef CONFIG_SCHED_HAVE_PARENT
 info.si_pid             =rtcb->pid;
 info.si_status          = OK;
#endif
 /* Send the signal */
 ret = sig_dispatch(pid, &info);
 sched_unlock();
 if (ret < 0)
   {
      goto errout;
   }
 return OK;
errout:
 set_errno(-ret);
 return ERROR;
}

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

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

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

跳转至sig_dispatch()

/****************************************************************************
 * Name: sig_dispatch
 *
 * Description:
 *   Thisis the front-end for sig_tcbdispatch that should be typically
 *   beused to dispatch a signal.  If HAVE_GROUP_MEMBERS is defined,
 *   thenfunction will follow the group signal delivery algorthrims:
 *
 *   Thisfront-end does the following things before calling
 *  sig_tcbdispatch.
 *
 *    With HAVE_GROUP_MEMBERS defined:
 *     -Get the TCB associated with the pid.
 *     -If the TCB was found, get the group from the TCB.
 *     - If the PID has already exited, lookup thegroup that that was
 *      started by this task.
 *     -Use the group to pick the TCB to receive the signal
 *     -Call sig_tcbdispatch with the TCB
 *
 *    With HAVE_GROUP_MEMBERS *not* defined
 *     -Get the TCB associated with the pid.
 *     -Call sig_tcbdispatch with the TCB
 *
 * Returned Value:
 *  Returns 0 (OK) on success or a negated errno value on failure.
 *
 ****************************************************************************/
注释翻译:(TCB是线程控制块的意思)
这个函数是用作派遣任务
如果HAVE_GROUP_MEMBERS被定义了,那么要完成
获取与PID相关的TCB
如果TCB被发现,请从TCB里获取组
如果PID已经退出,查找被此任务开始的组
使用组pick TCB来接收信号
用TCB来调用sig_tcbdispatch
如果HAVE_GROUP_MEMBERS没有被定义,那么
获取与PID相关的TCB
       用TCB来调用sig_tcbdispatch
int sig_dispatch(pid_t pid, FARsiginfo_t *info)
{
#ifdef HAVE_GROUP_MEMBERS
 FAR struct tcb_s *stcb;
 FAR struct task_group_s *group;
 /* Get the TCB associated with the pid */
 stcb = sched_gettcb(pid);//获取一个任务ID,这个函数会返回一个指针指向相应的TCB
 if (stcb)
   {
      /* The task/thread associated with thisPID is still active.  Get its
       * task group.
       */
//与PID相关的任务/线程还是激活的,得到它的任务组
      group = stcb->group;
   }
 else
   {
      /* The task/thread associated with thisPID has exited.  In the normal
       * usage model, the PID should correspondto the PID of the task that
       * created the task group.  Try looking it up.
       */
      group = group_findbypid(pid);
   }
  /* Didwe locate the group? */
 if (group)
   {
      /* Yes.. call group_signal() to send thesignal to the correct group
       * member.
       */
      return group_signal(group, info);
   }
 else
   {
      return -ESRCH;
   }
#else
 FAR struct tcb_s *stcb;
 /* Get the TCB associated with the pid */
 stcb = sched_gettcb(pid);
 if (!stcb)
   {
     return -ESRCH;
   }
 return sig_tcbdispatch(stcb, info);
#endif
}

ret = sig_dispatch(pid,&info);

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

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

跳转至sched_gettcb()

/****************************************************************************
 * Name: sched_gettcb
 *
 * Description:
 *  Given a task ID, this function will return
 *   thea pointer to the corresponding TCB (or NULL if there
 *   isno such task ID).
 *
 ****************************************************************************/
注释翻译:获取一个任务ID,这个函数会返回一个指针指向相应的TCB
FAR struct tcb_s*sched_gettcb(pid_t pid)
{
 FAR struct tcb_s *ret = NULL;
 int hash_ndx;
 /* Verify that the PID is within range */
 if (pid >= 0 )
   {
      /* Get the hash_ndx associated with thepid */
      hash_ndx = PIDHASH(pid);
      /* Verify that the correct TCB was found.*/
      if (pid == g_pidhash[hash_ndx].pid)
        {
          /* Return the TCB associated withthis pid (if any) */
          ret = g_pidhash[hash_ndx].tcb;
        }
   }
 /* Return the TCB. */
 return ret;
}
现在应该到了任务分配层了,应该也算内核了吧,下一层就是物理驱动层
细细看来,跳转至hash_ndx =PIDHASH(pid);
 
group = stcb->group;看看这个group是什么东西
#ifdef HAVE_TASK_GROUP
 FAR struct task_group_s *group;       /* Pointer to shared task group data  */
#endif
struct task_group_s
{
#ifdef HAVE_GROUP_MEMBERS
 struct task_group_s *flink;      /* Supports a singly linked list            */
 gid_t tg_gid;                    /* The ID of this task group                */
 gid_t tg_pgid;                   /* The ID of the parent task group          */
#endif
#if!defined(CONFIG_DISABLE_PTHREAD) && defined(CONFIG_SCHED_HAVE_PARENT)
 pid_t tg_task;                   /* The ID of the task within the group      */
#endif
 uint8_t tg_flags;                /* See GROUP_FLAG_* definitions             */
 /* Group membership ***********************************************************/
 uint8_t    tg_nmembers;           /* Number of members in thegroup           */
#ifdef HAVE_GROUP_MEMBERS
 uint8_t    tg_mxmembers;          /* Number of members inallocation          */
 FAR pid_t *tg_members;           /* Members of the group                     */
#endif
 /* atexit/on_exit support ****************************************************/
#if defined(CONFIG_SCHED_ATEXIT)&& !defined(CONFIG_SCHED_ONEXIT)
# ifdefined(CONFIG_SCHED_ATEXIT_MAX) && CONFIG_SCHED_ATEXIT_MAX > 1
 atexitfunc_t tg_atexitfunc[CONFIG_SCHED_ATEXIT_MAX];
# else
 atexitfunc_t tg_atexitfunc;      /* Called when exit is called.             */
# endif
#endif
#ifdef CONFIG_SCHED_ONEXIT
# ifdefined(CONFIG_SCHED_ONEXIT_MAX) && CONFIG_SCHED_ONEXIT_MAX > 1
 onexitfunc_t tg_onexitfunc[CONFIG_SCHED_ONEXIT_MAX];
 FAR void *tg_onexitarg[CONFIG_SCHED_ONEXIT_MAX];
# else
 onexitfunc_t tg_onexitfunc;      /* Called when exit is called.             */
 FAR void *tg_onexitarg;          /* The argument passed to the function     */
# endif
#endif
 /* Child exit status **********************************************************/
#ifdefined(CONFIG_SCHED_HAVE_PARENT) && defined(CONFIG_SCHED_CHILD_STATUS)
 FAR struct child_status_s *tg_children; /* Head of a list of childstatus     */
#endif
 /* waitpid support************************************************************/
  /*Simple mechanism used only when there is no support for SIGCHLD            */
#if defined(CONFIG_SCHED_WAITPID)&& !defined(CONFIG_SCHED_HAVE_PARENT)
 sem_t tg_exitsem;                /* Support for waitpid                      */
 int *tg_statloc;                  /* Location to return exitstatus           */
#endif
 /* Pthreads *******************************************************************/
#ifndef CONFIG_DISABLE_PTHREAD
                                    /* Pthreadjoin Info:                       */
 sem_t tg_joinsem;                /*   Mutually exclusive access tojoin data */
 FAR struct join_s *tg_joinhead;  /*   Head of a list of joindata            */
 FAR struct join_s *tg_jointail;  /*   Tail of a list of joindata            */
 uint8_t tg_nkeys;                /* Number pthread keys allocated            */
#endif
 /* POSIX Signal Control Fields ************************************************/
#ifndef CONFIG_DISABLE_SIGNALS
 sq_queue_t sigpendingq;          /* List of pending signals                  */
#endif
 /* Environment variables ******************************************************/
#ifndef CONFIG_DISABLE_ENVIRON
 size_t     tg_envsize;            /* Size of environment stringallocation    */
 FAR char  *tg_envp;               /* Allocated environmentstrings            */
#endif
 /* PIC data space and address environments************************************/
 /* Logically the PIC data space belongs here (see struct dspace_s).  The
  * current logic needs review: There are differences in the away that the
  * life of the PIC data is managed.
  */
 /* File descriptors ***********************************************************/
#if CONFIG_NFILE_DESCRIPTORS > 0
 struct filelist tg_filelist;     /* Maps file descriptor to file             */
#endif
 /* FILE streams***************************************************************/
 /* In a flat, single-heap build. The stream list is allocated with this
  * structure.  But kernel mode witha kernel allocator, it must be separately
  * allocated using a user-space allocator.
  */
#if CONFIG_NFILE_STREAMS > 0
#if defined(CONFIG_NUTTX_KERNEL)&& defined(CONFIG_MM_KERNEL_HEAP)
 FAR struct streamlist *tg_streamlist;
#else
 struct streamlist tg_streamlist; /* Holds C buffered I/O info                */
#endif
#endif
 /* Sockets ********************************************************************/
#if CONFIG_NSOCKET_DESCRIPTORS >0
 struct socketlist tg_socketlist; /* Maps socket descriptor to socket        */
#endif
 /* POSIX Named Message Queue Fields *******************************************/
#ifndef CONFIG_DISABLE_MQUEUE
 sq_queue_t tg_msgdesq;           /* List of opened message queues           */
#endif
};
好吧,还以为是个什么结构体呢,好长一段啊!!

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

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

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

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

int
px4flow_main(int argc, char *argv[])
{
	/*
	 * Start/load the driver.
	 */
	if (!strcmp(argv[1], "start")) {
		return px4flow::start();
	}
	/*
	 * Stop the driver
	 */
	if (!strcmp(argv[1], "stop")) {
		px4flow::stop();
	}
	/*
	 * Test the driver/device.
	 */
	if (!strcmp(argv[1], "test")) {
		px4flow::test();
	}
	/*
	 * Reset the driver.
	 */
	if (!strcmp(argv[1], "reset")) {
		px4flow::reset();
	}
	/*
	 * Print driver information.
	 */
	if (!strcmp(argv[1], "info") || !strcmp(argv[1], "status")) {
		px4flow::info();
	}
	errx(1, "unrecognized command, try 'start', 'test', 'reset' or 'info'");
}
跳转至start()

/**
 * Start the driver.
 */
int
start()
{
	int fd;

	/* entry check: */
	if (start_in_progress) {
		warnx("start already in progress");
		return 1;
	}

	start_in_progress = true;

	if (g_dev != nullptr) {
		start_in_progress = false;
		warnx("already started");
		return 1;
	}

	warnx("scanning I2C buses for device..");

	int retry_nr = 0;

	while (1) {
		const int busses_to_try[] = {
			PX4_I2C_BUS_EXPANSION,
#ifdef PX4_I2C_BUS_ESC
			PX4_I2C_BUS_ESC,
#endif
#ifdef PX4_I2C_BUS_ONBOARD
			PX4_I2C_BUS_ONBOARD,
#endif
			-1
		};

		const int *cur_bus = busses_to_try;

		while (*cur_bus != -1) {
			/* create the driver */
			/* warnx("trying bus %d", *cur_bus); */
			g_dev = new PX4FLOW(*cur_bus);

			if (g_dev == nullptr) {
				/* this is a fatal error */
				break;
			}

			/* init the driver: */
			if (OK == g_dev->init()) {
				/* success! */
				break;
			}

			/* destroy it again because it failed. */
			delete g_dev;
			g_dev = nullptr;

			/* try next! */
			cur_bus++;
		}

		/* check whether we found it: */
		if (*cur_bus != -1) {

			/* check for failure: */
			if (g_dev == nullptr) {
				break;
			}

			/* set the poll rate to default, starts automatic data collection */
			fd = open(PX4FLOW0_DEVICE_PATH, O_RDONLY);

			if (fd < 0) {
				break;
			}

			if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_MAX) < 0) {
				break;
			}

			/* success! */
			start_in_progress = false;
			return 0;
		}

		if (retry_nr < START_RETRY_COUNT) {
			/* lets not be too verbose */
			// warnx("PX4FLOW not found on I2C busses. Retrying in %d ms. Giving up in %d retries.", START_RETRY_TIMEOUT, START_RETRY_COUNT - retry_nr);
			usleep(START_RETRY_TIMEOUT * 1000);
			retry_nr++;

		} else {
			break;
		}
	}

	if (g_dev != nullptr) {
		delete g_dev;
		g_dev = nullptr;
	}

	start_in_progress = false;
	return 1;
}

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

读数据在test()里面

/**
 * Perform some basic functional tests on the driver;
 * make sure we can collect data from the sensor in polled
 * and automatic modes.
 */
void
test()
{
	struct optical_flow_s report;
	ssize_t sz;
	int ret;

	int fd = open(PX4FLOW0_DEVICE_PATH, O_RDONLY);

	if (fd < 0) {
		err(1, "%s open failed (try 'px4flow start' if the driver is not running", PX4FLOW0_DEVICE_PATH);
	}


	/* do a simple demand read */
	sz = read(fd, &report, sizeof(report));

	if (sz != sizeof(report)) {
		warnx("immediate read failed");
	}

	warnx("single read");
	warnx("pixel_flow_x_integral: %i", f_integral.pixel_flow_x_integral);
	warnx("pixel_flow_y_integral: %i", f_integral.pixel_flow_y_integral);
	warnx("framecount_integral: %u",
	      f_integral.frame_count_since_last_readout);

	/* start the sensor polling at 10Hz */
	if (OK != ioctl(fd, SENSORIOCSPOLLRATE, 10)) {
		errx(1, "failed to set 10Hz poll rate");
	}

	/* read the sensor 5x and report each value */
	for (unsigned i = 0; i < 10; i++) {
		struct pollfd fds;

		/* wait for data to be ready */
		fds.fd = fd;
		fds.events = POLLIN;
		ret = poll(&fds, 1, 2000);

		if (ret != 1) {
			errx(1, "timed out waiting for sensor data");
		}

		/* now go get it */
		sz = read(fd, &report, sizeof(report));

		if (sz != sizeof(report)) {
			err(1, "periodic read failed");
		}

		warnx("periodic read %u", i);

		warnx("framecount_total: %u", f.frame_count);
		warnx("framecount_integral: %u",
		      f_integral.frame_count_since_last_readout);
		warnx("pixel_flow_x_integral: %i", f_integral.pixel_flow_x_integral);
		warnx("pixel_flow_y_integral: %i", f_integral.pixel_flow_y_integral);
		warnx("gyro_x_rate_integral: %i", f_integral.gyro_x_rate_integral);
		warnx("gyro_y_rate_integral: %i", f_integral.gyro_y_rate_integral);
		warnx("gyro_z_rate_integral: %i", f_integral.gyro_z_rate_integral);
		warnx("integration_timespan [us]: %u", f_integral.integration_timespan);
		warnx("ground_distance: %0.2f m",
		      (double) f_integral.ground_distance / 1000);
		warnx("time since last sonar update [us]: %i",
		      f_integral.sonar_timestamp);
		warnx("quality integration average : %i", f_integral.qual);
		warnx("quality : %i", f.qual);


	}

	errx(0, "PASS");
}
看注释能明白sz = read(fd, &report, sizeof(report));是读的操作

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

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

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

#if defined(CONFIG_NET) && CONFIG_NSOCKET_DESCRIPTORS > 0
      return recv(fd, buf, nbytes, 0);
#else
      /* No networking... it is a bad descriptor in any event */

      set_errno(EBADF);
      return ERROR;
#endif
    }

  /* The descriptor is in a valid range to file descriptor... do the read */

#if CONFIG_NFILE_DESCRIPTORS > 0
  return file_read(fd, buf, nbytes);
#endif
}
看注释能明白return file_read(fd, buf, nbytes);是读操作

/****************************************************************************
 * Private Functions
 ****************************************************************************/

#if CONFIG_NFILE_DESCRIPTORS > 0
static inline ssize_t file_read(int fd, FAR void *buf, size_t nbytes)
{
  FAR struct filelist *list;
  int ret = -EBADF;

  /* Get the thread-specific file list */

  list = sched_getfiles();
  if (!list)
    {
      /* Failed to get the file list */

      ret = -EMFILE;
    }

  /* Were we given a valid file descriptor? */

  else if ((unsigned int)fd < CONFIG_NFILE_DESCRIPTORS)
    {
      FAR struct file *this_file = &list->fl_files[fd];
      FAR struct inode *inode    = this_file->f_inode;

      /* Yes.. Was this file opened for read access? */

      if ((this_file->f_oflags & O_RDOK) == 0)
        {
          /* No.. File is not read-able */

          ret = -EACCES;
        }

      /* Is a driver or mountpoint registered? If so, does it support
       * the read method?
       */

      else if (inode && inode->u.i_ops && inode->u.i_ops->read)
        {
          /* Yes.. then let it perform the read.  NOTE that for the case
           * of the mountpoint, we depend on the read methods bing
           * identical in signature and position in the operations vtable.
           */

          ret = (int)inode->u.i_ops->read(this_file, (char*)buf, (size_t)nbytes);
        }
    }

  /* If an error occurred, set errno and return -1 (ERROR) */

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

  /* Otherwise, return the number of bytes read */

  return ret;
}
#endif
跟踪buf可知ret = (int)inode->u.i_ops->read(this_file, (char*)buf, (size_t)nbytes);是读操作

于是进入了“文件操作”

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

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

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

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

  /* The two structures need not be common after this point */
};
怎么继续找下去呢?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函数

ssize_t
PX4FLOW::read(struct file *filp, char *buffer, size_t buflen)
{
	unsigned count = buflen / sizeof(struct optical_flow_s);
	struct optical_flow_s *rbuf = reinterpret_cast<struct optical_flow_s *>(buffer);
	int ret = 0;

	/* buffer must be large enough */
	if (count < 1) {
		return -ENOSPC;
	}

	/* if automatic measurement is enabled */
	if (_measure_ticks > 0) {

		/*
		 * While there is space in the caller's buffer, and reports, copy them.
		 * Note that we may be pre-empted by the workq thread while we are doing this;
		 * we are careful to avoid racing with them.
		 */
		while (count--) {
			if (_reports->get(rbuf)) {
				ret += sizeof(*rbuf);
				rbuf++;
			}
		}

		/* if there was no data, warn the caller */
		return ret ? ret : -EAGAIN;
	}

	/* manual measurement - run one conversion */
	do {
		_reports->flush();

		/* trigger a measurement */
		if (OK != measure()) {
			ret = -EIO;
			break;
		}

		/* run the collection phase */
		if (OK != collect()) {
			ret = -EIO;
			break;
		}

		/* state machine will have generated a report, copy it out */
		if (_reports->get(rbuf)) {
			ret = sizeof(*rbuf);
		}

	} while (0);

	return ret;
}
get(rbuf)  (read函数中)

bool
RingBuffer::get(void *val, size_t val_size)
{
	if (_tail != _head) {
		unsigned candidate;
		unsigned next;

		if ((val_size == 0) || (val_size > _item_size)) {
			val_size = _item_size;
		}

		do {
			/* decide which element we think we're going to read */
			candidate = _tail;

			/* and what the corresponding next index will be */
			next = _next(candidate);

			/* go ahead and read from this index */
			if (val != NULL) {
				memcpy(val, &_buf[candidate * _item_size], val_size);
			}

			/* if the tail pointer didn't change, we got our item */
		} while (!__PX4_SBCAP(&_tail, candidate, next));

		return true;

	} else {
		return false;
	}
}
字面意思的从缓冲环中得到数据,并存到rbuf中
继续看measure()collect()(read函数中)
int
PX4FLOW::measure()
{
	int ret;

	/*
	 * Send the command to begin a measurement.
	 */
	uint8_t cmd = PX4FLOW_REG;
	ret = transfer(&cmd, 1, nullptr, 0);

	if (OK != ret) {
		perf_count(_comms_errors);
		DEVICE_DEBUG("i2c::transfer returned %d", ret);
		return ret;
	}

	ret = OK;

	return ret;
}
看到 PX4FLOW_REG
/* PX4FLOW Registers addresses */
#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就是我们要找的函数

int
I2C::transfer(const uint8_t *send, unsigned send_len, uint8_t *recv, unsigned recv_len)
{
	struct i2c_msg_s msgv[2];
	unsigned msgs;
	int ret;
	unsigned retry_count = 0;

	do {
		//	DEVICE_DEBUG("transfer out %p/%u  in %p/%u", send, send_len, recv, recv_len);

		msgs = 0;

		if (send_len > 0) {
			msgv[msgs].addr = _address;
			msgv[msgs].flags = 0;
			msgv[msgs].buffer = const_cast<uint8_t *>(send);
			msgv[msgs].length = send_len;
			msgs++;
		}

		if (recv_len > 0) {
			msgv[msgs].addr = _address;
			msgv[msgs].flags = I2C_M_READ;
			msgv[msgs].buffer = recv;
			msgv[msgs].length = recv_len;
			msgs++;
		}

		if (msgs == 0) {
			return -EINVAL;
		}

		ret = I2C_TRANSFER(_dev, &msgv[0], msgs);

		/* success */
		if (ret == OK) {
			break;
		}

		/* if we have already retried once, or we are going to give up, then reset the bus */
		if ((retry_count >= 1) || (retry_count >= _retries)) {
			up_i2creset(_dev);
		}

	} while (retry_count++ < _retries);

	return ret;

}

struct i2c_msg_s msgv[2];

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

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

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

继续看collect()

int
PX4FLOW::collect()
{
	int ret = -EIO;

	/* read from the sensor */
	uint8_t val[I2C_FRAME_SIZE + I2C_INTEGRAL_FRAME_SIZE] = { 0 };

	perf_begin(_sample_perf);

	if (PX4FLOW_REG == 0x00) {
		ret = transfer(nullptr, 0, &val[0], I2C_FRAME_SIZE + I2C_INTEGRAL_FRAME_SIZE);
	}

	if (PX4FLOW_REG == 0x16) {
		ret = transfer(nullptr, 0, &val[0], I2C_INTEGRAL_FRAME_SIZE);
	}

	if (ret < 0) {
		DEVICE_DEBUG("error reading from sensor: %d", ret);
		perf_count(_comms_errors);
		perf_end(_sample_perf);
		return ret;
	}

	if (PX4FLOW_REG == 0) {
		memcpy(&f, val, I2C_FRAME_SIZE);
		memcpy(&f_integral, &(val[I2C_FRAME_SIZE]), I2C_INTEGRAL_FRAME_SIZE);
	}

	if (PX4FLOW_REG == 0x16) {
		memcpy(&f_integral, val, I2C_INTEGRAL_FRAME_SIZE);
	}


	struct optical_flow_s report;

	report.timestamp = hrt_absolute_time();

	report.pixel_flow_x_integral = static_cast<float>(f_integral.pixel_flow_x_integral) / 10000.0f;//convert to radians

	report.pixel_flow_y_integral = static_cast<float>(f_integral.pixel_flow_y_integral) / 10000.0f;//convert to radians

	report.frame_count_since_last_readout = f_integral.frame_count_since_last_readout;

	report.ground_distance_m = static_cast<float>(f_integral.ground_distance) / 1000.0f;//convert to meters

	report.quality = f_integral.qual; //0:bad ; 255 max quality

	report.gyro_x_rate_integral = static_cast<float>(f_integral.gyro_x_rate_integral) / 10000.0f; //convert to radians

	report.gyro_y_rate_integral = static_cast<float>(f_integral.gyro_y_rate_integral) / 10000.0f; //convert to radians

	report.gyro_z_rate_integral = static_cast<float>(f_integral.gyro_z_rate_integral) / 10000.0f; //convert to radians

	report.integration_timespan = f_integral.integration_timespan; //microseconds

	report.time_since_last_sonar_update = f_integral.sonar_timestamp;//microseconds

	report.gyro_temperature = f_integral.gyro_temperature;//Temperature * 100 in centi-degrees Celsius

	report.sensor_id = 0;

	/* rotate measurements according to parameter */根据参数测量旋转
	float zeroval = 0.0f;

	rotate_3f(_sensor_rotation, report.pixel_flow_x_integral, report.pixel_flow_y_integral, zeroval);

	rotate_3f(_sensor_rotation, report.gyro_x_rate_integral, report.gyro_y_rate_integral, report.gyro_z_rate_integral);

	if (_px4flow_topic == nullptr) {
		_px4flow_topic = orb_advertise(ORB_ID(optical_flow), &report);

	} else {
		/* publish it */
		orb_publish(ORB_ID(optical_flow), _px4flow_topic, &report);
	}

	/* publish to the distance_sensor topic as well */将distance_sensor主题发布出去
	struct distance_sensor_s distance_report;
	distance_report.timestamp = report.timestamp;
	distance_report.min_distance = PX4FLOW_MIN_DISTANCE;
	distance_report.max_distance = PX4FLOW_MAX_DISTANCE;
	distance_report.current_distance = report.ground_distance_m;
	distance_report.covariance = 0.0f;
	distance_report.type = distance_sensor_s::MAV_DISTANCE_SENSOR_ULTRASOUND;
	/* TODO: the ID needs to be properly set */
	distance_report.id = 0;
	distance_report.orientation = 8;

	orb_publish(ORB_ID(distance_sensor), _distance_sensor_topic, &distance_report);

	/* post a report to the ring */发布到环的报告
	if (_reports->force(&report)) {
		perf_count(_buffer_overflows);
	}

	/* notify anyone waiting for data */通知别人等待数据
	poll_notify(POLLIN);

	ret = OK;

	perf_end(_sample_perf);
	return ret;
}

注意看

	if (PX4FLOW_REG == 0x00) {
		ret = transfer(nullptr, 0, &val[0], I2C_FRAME_SIZE + I2C_INTEGRAL_FRAME_SIZE);
	}

	if (PX4FLOW_REG == 0x16) {
		ret = transfer(nullptr, 0, &val[0], I2C_INTEGRAL_FRAME_SIZE);
	}

这个是把数据读回

意思就是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结构体中

struct i2c_frame f;
struct i2c_integral_frame f_integral;
typedef  struct i2c_frame {
	uint16_t frame_count;
	int16_t pixel_flow_x_sum;
	int16_t pixel_flow_y_sum;
	int16_t flow_comp_m_x;
	int16_t flow_comp_m_y;
	int16_t qual;
	int16_t gyro_x_rate;
	int16_t gyro_y_rate;
	int16_t gyro_z_rate;
	uint8_t gyro_range;
	uint8_t sonar_timestamp;
	int16_t ground_distance;
} i2c_frame;

#define I2C_FRAME_SIZE (sizeof(i2c_frame))


typedef struct i2c_integral_frame {
	uint16_t frame_count_since_last_readout;
	int16_t pixel_flow_x_integral;
	int16_t pixel_flow_y_integral;
	int16_t gyro_x_rate_integral;
	int16_t gyro_y_rate_integral;
	int16_t gyro_z_rate_integral;
	uint32_t integration_timespan;
	uint32_t sonar_timestamp;
	uint16_t ground_distance;
	int16_t gyro_temperature;
	uint8_t qual;
} 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

#include <px4_posix.h>
#include <unistd.h>
#include <stdlib.h>
#include <stdio.h>
#include <stdbool.h>
#include <fcntl.h>
#include <string.h>
#include <px4_config.h>
#include <math.h>
#include <float.h>
#include <uORB/uORB.h>
#include <uORB/topics/parameter_update.h>
#include <uORB/topics/actuator_controls.h>
#include <uORB/topics/actuator_controls_0.h>
#include <uORB/topics/actuator_controls_1.h>
#include <uORB/topics/actuator_controls_2.h>
#include <uORB/topics/actuator_controls_3.h>
#include <uORB/topics/actuator_armed.h>
#include <uORB/topics/sensor_combined.h>
#include <uORB/topics/vehicle_attitude.h>
#include <uORB/topics/vehicle_local_position.h>
#include <uORB/topics/vehicle_global_position.h>
#include <uORB/topics/vehicle_gps_position.h>
#include <uORB/topics/vision_position_estimate.h>
#include <uORB/topics/att_pos_mocap.h>
#include <uORB/topics/optical_flow.h>
#include <uORB/topics/distance_sensor.h>
#include <poll.h>
#include <systemlib/err.h>
#include <systemlib/mavlink_log.h>
#include <geo/geo.h>
#include <systemlib/systemlib.h>
#include <drivers/drv_hrt.h>
#include <platforms/px4_defines.h>

#include <terrain_estimation/terrain_estimator.h>
#include "position_estimator_inav_params.h"
#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中呢?估计有个总的流程,分别调用启动这些功能“函数”

/****************************************************************************
 * main
 ****************************************************************************/
int position_estimator_inav_thread_main(int argc, char *argv[])
{
/**************************类似于初始化或者声明******************************/
	orb_advert_t mavlink_log_pub = nullptr;

	float x_est[2] = { 0.0f, 0.0f };	// pos, vel
	float y_est[2] = { 0.0f, 0.0f };	// pos, vel
	float z_est[2] = { 0.0f, 0.0f };	// pos, vel

	float est_buf[EST_BUF_SIZE][3][2];	// estimated position buffer
	float R_buf[EST_BUF_SIZE][3][3];	// rotation matrix buffer
	float R_gps[3][3];					// rotation matrix for GPS correction moment
	memset(est_buf, 0, sizeof(est_buf));
	memset(R_buf, 0, sizeof(R_buf));
	memset(R_gps, 0, sizeof(R_gps));
	int buf_ptr = 0;

	static const float min_eph_epv = 2.0f;	// min EPH/EPV, used for weight calculation
	static const float max_eph_epv = 20.0f;	// max EPH/EPV acceptable for estimation

	float eph = max_eph_epv;
	float epv = 1.0f;

	float eph_flow = 1.0f;

	float eph_vision = 0.2f;
	float epv_vision = 0.2f;

	float eph_mocap = 0.05f;
	float epv_mocap = 0.05f;

	float x_est_prev[2], y_est_prev[2], z_est_prev[2];
	memset(x_est_prev, 0, sizeof(x_est_prev));
	memset(y_est_prev, 0, sizeof(y_est_prev));
	memset(z_est_prev, 0, sizeof(z_est_prev));

	int baro_init_cnt = 0;
	int baro_init_num = 200;
	float baro_offset = 0.0f;		// baro offset for reference altitude, initialized on start, then adjusted

	hrt_abstime accel_timestamp = 0;
	hrt_abstime baro_timestamp = 0;

	bool ref_inited = false;
	hrt_abstime ref_init_start = 0;
	const hrt_abstime ref_init_delay = 1000000;	// wait for 1s after 3D fix
	struct map_projection_reference_s ref;
	memset(&ref, 0, sizeof(ref));

	uint16_t accel_updates = 0;
	uint16_t baro_updates = 0;
	uint16_t gps_updates = 0;
	uint16_t attitude_updates = 0;
	uint16_t flow_updates = 0;
	uint16_t vision_updates = 0;
	uint16_t mocap_updates = 0;

	hrt_abstime updates_counter_start = hrt_absolute_time();
	hrt_abstime pub_last = hrt_absolute_time();

	hrt_abstime t_prev = 0;

	/* store error when sensor updates, but correct on each time step to avoid jumps in estimated value */
	float acc[] = { 0.0f, 0.0f, 0.0f };	// N E D
	float acc_bias[] = { 0.0f, 0.0f, 0.0f };	// body frame
	float corr_baro = 0.0f;		// D
	float corr_gps[3][2] = {
		{ 0.0f, 0.0f },		// N (pos, vel)
		{ 0.0f, 0.0f },		// E (pos, vel)
		{ 0.0f, 0.0f },		// D (pos, vel)
	};
	float w_gps_xy = 1.0f;
	float w_gps_z = 1.0f;

	float corr_vision[3][2] = {
		{ 0.0f, 0.0f },		// N (pos, vel)
		{ 0.0f, 0.0f },		// E (pos, vel)
		{ 0.0f, 0.0f },		// D (pos, vel)
	};

	float corr_mocap[3][1] = {
		{ 0.0f },		// N (pos)
		{ 0.0f },		// E (pos)
		{ 0.0f },		// D (pos)
	};
	const int mocap_heading = 2;

	float dist_ground = 0.0f;		//variables for lidar altitude estimation
	float corr_lidar = 0.0f;
	float lidar_offset = 0.0f;
	int lidar_offset_count = 0;
	bool lidar_first = true;
	bool use_lidar = false;
	bool use_lidar_prev = false;

	float corr_flow[] = { 0.0f, 0.0f };	// N E
	float w_flow = 0.0f;

	hrt_abstime lidar_time = 0;			// time of last lidar measurement (not filtered)
	hrt_abstime lidar_valid_time = 0;	// time of last lidar measurement used for correction (filtered)

	int n_flow = 0;
	float gyro_offset_filtered[] = { 0.0f, 0.0f, 0.0f };
	float flow_gyrospeed[] = { 0.0f, 0.0f, 0.0f };
	float flow_gyrospeed_filtered[] = { 0.0f, 0.0f, 0.0f };
	float att_gyrospeed_filtered[] = { 0.0f, 0.0f, 0.0f };
	float yaw_comp[] = { 0.0f, 0.0f };
	hrt_abstime flow_time = 0;
	float flow_min_dist = 0.2f;

	bool gps_valid = false;			// GPS is valid
	bool lidar_valid = false;		// lidar is valid
	bool flow_valid = false;		// flow is valid
	bool flow_accurate = false;		// flow should be accurate (this flag not updated if flow_valid == false)
	bool vision_valid = false;		// vision is valid
	bool mocap_valid = false;		// mocap is valid

	/* declare and safely initialize all structs */
	struct actuator_controls_s actuator;
	memset(&actuator, 0, sizeof(actuator));
	struct actuator_armed_s armed;
	memset(&armed, 0, sizeof(armed));
	struct sensor_combined_s sensor;
	memset(&sensor, 0, sizeof(sensor));
	struct vehicle_gps_position_s gps;
	memset(&gps, 0, sizeof(gps));
	struct vehicle_attitude_s att;
	memset(&att, 0, sizeof(att));
	struct vehicle_local_position_s local_pos;
	memset(&local_pos, 0, sizeof(local_pos));
	struct optical_flow_s flow;
	memset(&flow, 0, sizeof(flow));
	struct vision_position_estimate_s vision;
	memset(&vision, 0, sizeof(vision));
	struct att_pos_mocap_s mocap;
	memset(&mocap, 0, sizeof(mocap));
	struct vehicle_global_position_s global_pos;
	memset(&global_pos, 0, sizeof(global_pos));
	struct distance_sensor_s lidar;
	memset(&lidar, 0, sizeof(lidar));
	struct vehicle_rates_setpoint_s rates_setpoint;
	memset(&rates_setpoint, 0, sizeof(rates_setpoint));
/**************************订阅各种信息******************************/
	/* subscribe */
	int parameter_update_sub = orb_subscribe(ORB_ID(parameter_update));
	int actuator_sub = orb_subscribe(ORB_ID_VEHICLE_ATTITUDE_CONTROLS);
	int armed_sub = orb_subscribe(ORB_ID(actuator_armed));
	int sensor_combined_sub = orb_subscribe(ORB_ID(sensor_combined));
	int vehicle_attitude_sub = orb_subscribe(ORB_ID(vehicle_attitude));
	int optical_flow_sub = orb_subscribe(ORB_ID(optical_flow));
	int vehicle_gps_position_sub = orb_subscribe(ORB_ID(vehicle_gps_position));
	int vision_position_estimate_sub = orb_subscribe(ORB_ID(vision_position_estimate));
	int att_pos_mocap_sub = orb_subscribe(ORB_ID(att_pos_mocap));
	int distance_sensor_sub = orb_subscribe(ORB_ID(distance_sensor));
	int vehicle_rate_sp_sub = orb_subscribe(ORB_ID(vehicle_rates_setpoint));
/**************************发布位置信息******************************/
	/* advertise */
	orb_advert_t vehicle_local_position_pub = orb_advertise(ORB_ID(vehicle_local_position), &local_pos);
	orb_advert_t vehicle_global_position_pub = NULL;
/**************************类似于初始化params******************************/
	struct position_estimator_inav_params params;
	memset(¶ms, 0, sizeof(params));
	struct position_estimator_inav_param_handles pos_inav_param_handles;
	/* initialize parameter handles */
	inav_parameters_init(&pos_inav_param_handles);

	/* first parameters read at start up */
	struct parameter_update_s param_update;
	orb_copy(ORB_ID(parameter_update), parameter_update_sub,
		 ¶m_update); /* read from param topic to clear updated flag */
	/* first parameters update */
	inav_parameters_update(&pos_inav_param_handles, ¶ms);
/**************************sensor_combined******************************/
	px4_pollfd_struct_t fds_init[1] = {};
	fds_init[0].fd = sensor_combined_sub;
	fds_init[0].events = POLLIN;
////////////////////////气压计///////////////////////////////////
	/* wait for initial baro value */
	bool wait_baro = true;
	TerrainEstimator *terrain_estimator = new TerrainEstimator();

	thread_running = true;
	hrt_abstime baro_wait_for_sample_time = hrt_absolute_time();

	while (wait_baro && !thread_should_exit) {
		int ret = px4_poll(&fds_init[0], 1, 1000);

		if (ret < 0) {
			/* poll error */
			mavlink_log_info(&mavlink_log_pub, "[inav] poll error on init");
		} else if (hrt_absolute_time() - baro_wait_for_sample_time > MAX_WAIT_FOR_BARO_SAMPLE) {
			wait_baro = false;
			mavlink_log_info(&mavlink_log_pub, "[inav] timed out waiting for a baro sample");
		}
		else if (ret > 0) {
			if (fds_init[0].revents & POLLIN) {
				orb_copy(ORB_ID(sensor_combined), sensor_combined_sub, &sensor);

				if (wait_baro && sensor.baro_timestamp[0] != baro_timestamp) {
					baro_timestamp = sensor.baro_timestamp[0];
					baro_wait_for_sample_time = hrt_absolute_time();

					/* mean calculation over several measurements */
					if (baro_init_cnt < baro_init_num) {
						if (PX4_ISFINITE(sensor.baro_alt_meter[0])) {
							baro_offset += sensor.baro_alt_meter[0];
							baro_init_cnt++;
						}

					} else {
						wait_baro = false;
						baro_offset /= (float) baro_init_cnt;
						local_pos.z_valid = true;
						local_pos.v_z_valid = true;
					}
				}
			}

		} else {
			PX4_WARN("INAV poll timeout");
		}
	}
/**************************主循环******************************/
	/* main loop */
	px4_pollfd_struct_t fds[1];
	fds[0].fd = vehicle_attitude_sub;
	fds[0].events = POLLIN;
////////////////////////判断是否应该退出该线程///////////////////////////////////
	while (!thread_should_exit) {   //这个判断条件经常使用,到处都用过类似的
		int ret = px4_poll(fds, 1, 20); // wait maximal 20 ms = 50 Hz minimum rate
		hrt_abstime t = hrt_absolute_time();

		if (ret < 0) {
			/* poll error */
			mavlink_log_info(&mavlink_log_pub, "[inav] poll error on init");
			continue;

		} else if (ret > 0) {
			/* act on attitude updates */

			/* vehicle attitude */
			orb_copy(ORB_ID(vehicle_attitude), vehicle_attitude_sub, &att);//姿态跟新
			attitude_updates++;

			bool updated;

			/* parameter update */
			orb_check(parameter_update_sub, &updated);

			if (updated) {
				struct parameter_update_s update;
				orb_copy(ORB_ID(parameter_update), parameter_update_sub, &update);//parameter跟新
				inav_parameters_update(&pos_inav_param_handles, ¶ms);
			}

			/* actuator */
			orb_check(actuator_sub, &updated);

			if (updated) {
				orb_copy(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, actuator_sub, &actuator);//执行器数据跟新(这里并不是pwm)
			}

			/* armed */
			orb_check(armed_sub, &updated);

			if (updated) {
				orb_copy(ORB_ID(actuator_armed), armed_sub, &armed);//解锁数据跟新
			}

			/* sensor combined */
			orb_check(sensor_combined_sub, &updated);

			if (updated) {
				orb_copy(ORB_ID(sensor_combined), sensor_combined_sub, &sensor);//传感器融合数据跟新

				if (sensor.accelerometer_timestamp[0] != accel_timestamp) {
					if (att.R_valid) {
						/* correct accel bias */
						sensor.accelerometer_m_s2[0] -= acc_bias[0];
						sensor.accelerometer_m_s2[1] -= acc_bias[1];
						sensor.accelerometer_m_s2[2] -= acc_bias[2];

						/* transform acceleration vector from body frame to NED frame */
						for (int i = 0; i < 3; i++) {
							acc[i] = 0.0f;

							for (int j = 0; j < 3; j++) {
								acc[i] += PX4_R(att.R, i, j) * sensor.accelerometer_m_s2[j];
							}
						}

						acc[2] += CONSTANTS_ONE_G;

					} else {
						memset(acc, 0, sizeof(acc));
					}

					accel_timestamp = sensor.accelerometer_timestamp[0];
					accel_updates++;
				}

				if (sensor.baro_timestamp[0] != baro_timestamp) {
					corr_baro = baro_offset - sensor.baro_alt_meter[0] - z_est[0];
					baro_timestamp = sensor.baro_timestamp[0];
					baro_updates++;
				}
			}


			/* lidar alt estimation */
			orb_check(distance_sensor_sub, &updated);

			/* update lidar separately, needed by terrain estimator */
			if (updated) {
				orb_copy(ORB_ID(distance_sensor), distance_sensor_sub, &lidar);//高度数据跟新
				lidar.current_distance += params.lidar_calibration_offset;
			}

			if (updated) { //check if altitude estimation for lidar is enabled and new sensor data

				if (params.enable_lidar_alt_est && lidar.current_distance > lidar.min_distance && lidar.current_distance < lidar.max_distance
			    		&& (PX4_R(att.R, 2, 2) > 0.7f)) {

					if (!use_lidar_prev && use_lidar) {
						lidar_first = true;
					}

					use_lidar_prev = use_lidar;

					lidar_time = t;
					dist_ground = lidar.current_distance * PX4_R(att.R, 2, 2); //vertical distance

					if (lidar_first) {
						lidar_first = false;
						lidar_offset = dist_ground + z_est[0];
						mavlink_log_info(&mavlink_log_pub, "[inav] LIDAR: new ground offset");
						warnx("[inav] LIDAR: new ground offset");
					}

					corr_lidar = lidar_offset - dist_ground - z_est[0];

					if (fabsf(corr_lidar) > params.lidar_err) { //check for spike
						corr_lidar = 0;
						lidar_valid = false;
						lidar_offset_count++;

						if (lidar_offset_count > 3) { //if consecutive bigger/smaller measurements -> new ground offset -> reinit
							lidar_first = true;
							lidar_offset_count = 0;
						}

					} else {
						corr_lidar = lidar_offset - dist_ground - z_est[0];
						lidar_valid = true;
						lidar_offset_count = 0;
						lidar_valid_time = t;
					}
				} else {
					lidar_valid = false;
				}
			}
/**************************************这里就是光流的处理*********************************/
			/* optical flow */
			orb_check(optical_flow_sub, &updated);

			if (updated && lidar_valid) {
				orb_copy(ORB_ID(optical_flow), optical_flow_sub, &flow);//光流数据跟新

				flow_time = t;
				float flow_q = flow.quality / 255.0f;
				float dist_bottom = lidar.current_distance;//高度信息

				if (dist_bottom > flow_min_dist && flow_q > params.flow_q_min && PX4_R(att.R, 2, 2) > 0.7f) {
					/* distance to surface */
					//float flow_dist = dist_bottom / PX4_R(att.R, 2, 2); //use this if using sonar
					float flow_dist = dist_bottom; //use this if using lidar

					/* check if flow if too large for accurate measurements */
					/* calculate estimated velocity in body frame */
					float body_v_est[2] = { 0.0f, 0.0f };

					for (int i = 0; i < 2; i++) {
						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];
					}

					/* set this flag if flow should be accurate according to current velocity and attitude rate estimate */
					flow_accurate = fabsf(body_v_est[1] / flow_dist - att.rollspeed) < max_flow &&
							fabsf(body_v_est[0] / flow_dist + att.pitchspeed) < max_flow;

					/*calculate offset of flow-gyro using already calibrated gyro from autopilot*/
					flow_gyrospeed[0] = flow.gyro_x_rate_integral / (float)flow.integration_timespan * 1000000.0f;
					flow_gyrospeed[1] = flow.gyro_y_rate_integral / (float)flow.integration_timespan * 1000000.0f;
					flow_gyrospeed[2] = flow.gyro_z_rate_integral / (float)flow.integration_timespan * 1000000.0f;

					//moving average
					if (n_flow >= 100) {
						gyro_offset_filtered[0] = flow_gyrospeed_filtered[0] - att_gyrospeed_filtered[0];
						gyro_offset_filtered[1] = flow_gyrospeed_filtered[1] - att_gyrospeed_filtered[1];
						gyro_offset_filtered[2] = flow_gyrospeed_filtered[2] - att_gyrospeed_filtered[2];
						n_flow = 0;
						flow_gyrospeed_filtered[0] = 0.0f;
						flow_gyrospeed_filtered[1] = 0.0f;
						flow_gyrospeed_filtered[2] = 0.0f;
						att_gyrospeed_filtered[0] = 0.0f;
						att_gyrospeed_filtered[1] = 0.0f;
						att_gyrospeed_filtered[2] = 0.0f;

					} else {
						flow_gyrospeed_filtered[0] = (flow_gyrospeed[0] + n_flow * flow_gyrospeed_filtered[0]) / (n_flow + 1);
						flow_gyrospeed_filtered[1] = (flow_gyrospeed[1] + n_flow * flow_gyrospeed_filtered[1]) / (n_flow + 1);
						flow_gyrospeed_filtered[2] = (flow_gyrospeed[2] + n_flow * flow_gyrospeed_filtered[2]) / (n_flow + 1);
						att_gyrospeed_filtered[0] = (att.pitchspeed + n_flow * att_gyrospeed_filtered[0]) / (n_flow + 1);
						att_gyrospeed_filtered[1] = (att.rollspeed + n_flow * att_gyrospeed_filtered[1]) / (n_flow + 1);
						att_gyrospeed_filtered[2] = (att.yawspeed + n_flow * att_gyrospeed_filtered[2]) / (n_flow + 1);
						n_flow++;
					}


					/*yaw compensation (flow sensor is not in center of rotation) -> params in QGC*/
					yaw_comp[0] = - params.flow_module_offset_y * (flow_gyrospeed[2] - gyro_offset_filtered[2]);
					yaw_comp[1] = params.flow_module_offset_x * (flow_gyrospeed[2] - gyro_offset_filtered[2]);

					/* convert raw flow to angular flow (rad/s) */
					float flow_ang[2];

					/* check for vehicle rates setpoint - it below threshold -> dont subtract -> better hover */
					orb_check(vehicle_rate_sp_sub, &updated);
					if (updated)
						orb_copy(ORB_ID(vehicle_rates_setpoint), vehicle_rate_sp_sub, &rates_setpoint);

					double rate_threshold = 0.15f;

					if (fabs(rates_setpoint.pitch) < rate_threshold) {
						//warnx("[inav] test ohne comp");
						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)
					}
					else {
						//warnx("[inav] test mit comp");
						//calculate flow [rad/s] and compensate for rotations (and offset of flow-gyro)
						flow_ang[0] = ((flow.pixel_flow_x_integral - flow.gyro_x_rate_integral) / (float)flow.integration_timespan * 1000000.0f
							       + gyro_offset_filtered[0]) * params.flow_k;//for now the flow has to be scaled (to small)
					}

					if (fabs(rates_setpoint.roll) < rate_threshold) {
						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)
					}
					else {
						//calculate flow [rad/s] and compensate for rotations (and offset of flow-gyro)
						flow_ang[1] = ((flow.pixel_flow_y_integral - flow.gyro_y_rate_integral) / (float)flow.integration_timespan * 1000000.0f
							       + gyro_offset_filtered[1]) * params.flow_k;//for now the flow has to be scaled (to small)
					}

					/* flow measurements vector */
					float flow_m[3];
					if (fabs(rates_setpoint.yaw) < rate_threshold) {
						flow_m[0] = -flow_ang[0] * flow_dist;
						flow_m[1] = -flow_ang[1] * flow_dist;
					} else {
						flow_m[0] = -flow_ang[0] * flow_dist - yaw_comp[0] * params.flow_k;
						flow_m[1] = -flow_ang[1] * flow_dist - yaw_comp[1] * params.flow_k;
					}
					flow_m[2] = z_est[1];

					/* velocity in NED */
					float flow_v[2] = { 0.0f, 0.0f };

					/* project measurements vector to NED basis, skip Z component */
					for (int i = 0; i < 2; i++) {
						for (int j = 0; j < 3; j++) {
							flow_v[i] += PX4_R(att.R, i, j) * flow_m[j];
						}
					}

					/* velocity correction */
					corr_flow[0] = flow_v[0] - x_est[1];
					corr_flow[1] = flow_v[1] - y_est[1];
					/* adjust correction weight */
					float flow_q_weight = (flow_q - params.flow_q_min) / (1.0f - params.flow_q_min);
					w_flow = PX4_R(att.R, 2, 2) * flow_q_weight / fmaxf(1.0f, flow_dist);


					/* if flow is not accurate, reduce weight for it */
					// TODO make this more fuzzy
					if (!flow_accurate) {
						w_flow *= 0.05f;
					}

					/* under ideal conditions, on 1m distance assume EPH = 10cm */
					eph_flow = 0.1f / w_flow;

					flow_valid = true;

				} else {
					w_flow = 0.0f;
					flow_valid = false;
				}

				flow_updates++;
			}
/**************************************光流结束*********************************/
//////////////////////////////////视觉的处理////////////////////////////////////
			/* check no vision circuit breaker is set */
			if (params.no_vision != CBRK_NO_VISION_KEY) {
				/* vehicle vision position */
				orb_check(vision_position_estimate_sub, &updated);

				if (updated) {
					orb_copy(ORB_ID(vision_position_estimate), vision_position_estimate_sub, &vision);

					static float last_vision_x = 0.0f;
					static float last_vision_y = 0.0f;
					static float last_vision_z = 0.0f;

					/* reset position estimate on first vision update */
					if (!vision_valid) {
						x_est[0] = vision.x;
						x_est[1] = vision.vx;
						y_est[0] = vision.y;
						y_est[1] = vision.vy;

						/* only reset the z estimate if the z weight parameter is not zero */
						if (params.w_z_vision_p > MIN_VALID_W) {
							z_est[0] = vision.z;
							z_est[1] = vision.vz;
						}

						vision_valid = true;

						last_vision_x = vision.x;
						last_vision_y = vision.y;
						last_vision_z = vision.z;

						warnx("VISION estimate valid");
						mavlink_log_info(&mavlink_log_pub, "[inav] VISION estimate valid");
					}

					/* calculate correction for position */
					corr_vision[0][0] = vision.x - x_est[0];
					corr_vision[1][0] = vision.y - y_est[0];
					corr_vision[2][0] = vision.z - z_est[0];

					static hrt_abstime last_vision_time = 0;

					float vision_dt = (vision.timestamp_boot - last_vision_time) / 1e6f;
					last_vision_time = vision.timestamp_boot;

					if (vision_dt > 0.000001f && vision_dt < 0.2f) {
						vision.vx = (vision.x - last_vision_x) / vision_dt;
						vision.vy = (vision.y - last_vision_y) / vision_dt;
						vision.vz = (vision.z - last_vision_z) / vision_dt;

						last_vision_x = vision.x;
						last_vision_y = vision.y;
						last_vision_z = vision.z;

						/* calculate correction for velocity */
						corr_vision[0][1] = vision.vx - x_est[1];
						corr_vision[1][1] = vision.vy - y_est[1];
						corr_vision[2][1] = vision.vz - z_est[1];

					} else {
						/* assume zero motion */
						corr_vision[0][1] = 0.0f - x_est[1];
						corr_vision[1][1] = 0.0f - y_est[1];
						corr_vision[2][1] = 0.0f - z_est[1];
					}

					vision_updates++;
				}
			}
///////////////////////////////////mocap数据处理///////////////////////////
			/* vehicle mocap position */
			orb_check(att_pos_mocap_sub, &updated);

			if (updated) {
				orb_copy(ORB_ID(att_pos_mocap), att_pos_mocap_sub, &mocap);

				if (!params.disable_mocap) {
					/* reset position estimate on first mocap update */
					if (!mocap_valid) {
						x_est[0] = mocap.x;
						y_est[0] = mocap.y;
						z_est[0] = mocap.z;

						mocap_valid = true;

						warnx("MOCAP data valid");
						mavlink_log_info(&mavlink_log_pub, "[inav] MOCAP data valid");
					}

					/* calculate correction for position */
					corr_mocap[0][0] = mocap.x - x_est[0];
					corr_mocap[1][0] = mocap.y - y_est[0];
					corr_mocap[2][0] = mocap.z - z_est[0];

					mocap_updates++;
				}
			}
//////////////////////////////////////GPS数据处理///////////////////////////////////////
			/* vehicle GPS position */
			orb_check(vehicle_gps_position_sub, &updated);

			if (updated) {
				orb_copy(ORB_ID(vehicle_gps_position), vehicle_gps_position_sub, &gps);

				bool reset_est = false;

				/* hysteresis for GPS quality */
				if (gps_valid) {
					if (gps.eph > max_eph_epv || gps.epv > max_eph_epv || gps.fix_type < 3) {
						gps_valid = false;
						mavlink_log_info(&mavlink_log_pub, "[inav] GPS signal lost");
						warnx("[inav] GPS signal lost");
					}

				} else {
					if (gps.eph < max_eph_epv * 0.7f && gps.epv < max_eph_epv * 0.7f && gps.fix_type >= 3) {
						gps_valid = true;
						reset_est = true;
						mavlink_log_info(&mavlink_log_pub, "[inav] GPS signal found");
						warnx("[inav] GPS signal found");
					}
				}

				if (gps_valid) {
					double lat = gps.lat * 1e-7;
					double lon = gps.lon * 1e-7;
					float alt = gps.alt * 1e-3;

					/* initialize reference position if needed */
					if (!ref_inited) {
						if (ref_init_start == 0) {
							ref_init_start = t;

						} else if (t > ref_init_start + ref_init_delay) {
							ref_inited = true;

							/* set position estimate to (0, 0, 0), use GPS velocity for XY */
							x_est[0] = 0.0f;
							x_est[1] = gps.vel_n_m_s;
							y_est[0] = 0.0f;
							y_est[1] = gps.vel_e_m_s;

							local_pos.ref_lat = lat;
							local_pos.ref_lon = lon;
							local_pos.ref_alt = alt + z_est[0];
							local_pos.ref_timestamp = t;

							/* initialize projection */
							map_projection_init(&ref, lat, lon);
							// XXX replace this print
							warnx("init ref: lat=%.7f, lon=%.7f, alt=%8.4f", (double)lat, (double)lon, (double)alt);
							mavlink_log_info(&mavlink_log_pub, "[inav] init ref: %.7f, %.7f, %8.4f", (double)lat, (double)lon, (double)alt);
						}
					}

					if (ref_inited) {
						/* project GPS lat lon to plane */
						float gps_proj[2];
						map_projection_project(&ref, lat, lon, &(gps_proj[0]), &(gps_proj[1]));

						/* reset position estimate when GPS becomes good */
						if (reset_est) {
							x_est[0] = gps_proj[0];
							x_est[1] = gps.vel_n_m_s;
							y_est[0] = gps_proj[1];
							y_est[1] = gps.vel_e_m_s;
						}

						/* calculate index of estimated values in buffer */
						int est_i = buf_ptr - 1 - min(EST_BUF_SIZE - 1, max(0, (int)(params.delay_gps * 1000000.0f / PUB_INTERVAL)));

						if (est_i < 0) {
							est_i += EST_BUF_SIZE;
						}

						/* calculate correction for position */
						corr_gps[0][0] = gps_proj[0] - est_buf[est_i][0][0];
						corr_gps[1][0] = gps_proj[1] - est_buf[est_i][1][0];
						corr_gps[2][0] = local_pos.ref_alt - alt - est_buf[est_i][2][0];

						/* calculate correction for velocity */
						if (gps.vel_ned_valid) {
							corr_gps[0][1] = gps.vel_n_m_s - est_buf[est_i][0][1];
							corr_gps[1][1] = gps.vel_e_m_s - est_buf[est_i][1][1];
							corr_gps[2][1] = gps.vel_d_m_s - est_buf[est_i][2][1];

						} else {
							corr_gps[0][1] = 0.0f;
							corr_gps[1][1] = 0.0f;
							corr_gps[2][1] = 0.0f;
						}

						/* save rotation matrix at this moment */
						memcpy(R_gps, R_buf[est_i], sizeof(R_gps));

						w_gps_xy = min_eph_epv / fmaxf(min_eph_epv, gps.eph);
						w_gps_z = min_eph_epv / fmaxf(min_eph_epv, gps.epv);
					}

				} else {
					/* no GPS lock */
					memset(corr_gps, 0, sizeof(corr_gps));
					ref_init_start = 0;
				}

				gps_updates++;
			}
		}
///////////////////////////////检查是否timeout//////////////////////////////////
		/* check for timeout on FLOW topic */
		if ((flow_valid || lidar_valid) && t > (flow_time + flow_topic_timeout)) {
			flow_valid = false;
			warnx("FLOW timeout");
			mavlink_log_info(&mavlink_log_pub, "[inav] FLOW timeout");
		}

		/* check for timeout on GPS topic */
		if (gps_valid && (t > (gps.timestamp_position + gps_topic_timeout))) {
			gps_valid = false;
			warnx("GPS timeout");
			mavlink_log_info(&mavlink_log_pub, "[inav] GPS timeout");
		}

		/* check for timeout on vision topic */
		if (vision_valid && (t > (vision.timestamp_boot + vision_topic_timeout))) {
			vision_valid = false;
			warnx("VISION timeout");
			mavlink_log_info(&mavlink_log_pub, "[inav] VISION timeout");
		}

		/* check for timeout on mocap topic */
		if (mocap_valid && (t > (mocap.timestamp_boot + mocap_topic_timeout))) {
			mocap_valid = false;
			warnx("MOCAP timeout");
			mavlink_log_info(&mavlink_log_pub, "[inav] MOCAP timeout");
		}

		/* check for lidar measurement timeout */
		if (lidar_valid && (t > (lidar_time + lidar_timeout))) {
			lidar_valid = false;
			warnx("LIDAR timeout");
			mavlink_log_info(&mavlink_log_pub, "[inav] LIDAR timeout");
		}

		float dt = t_prev > 0 ? (t - t_prev) / 1000000.0f : 0.0f;
		dt = fmaxf(fminf(0.02, dt), 0.0002);		// constrain dt from 0.2 to 20 ms
		t_prev = t;

		/* increase EPH/EPV on each step */
		if (eph < 0.000001f) { //get case where eph is 0 -> would stay 0
			eph = 0.001;
		}

		if (eph < max_eph_epv) {
			eph *= 1.0f + dt;
		}

		if (epv < 0.000001f) { //get case where epv is 0 -> would stay 0
			epv = 0.001;
		}

		if (epv < max_eph_epv) {
			epv += 0.005f * dt;	// add 1m to EPV each 200s (baro drift)
		}
//////////////////////////////////////根据具体情况,将决策赋给标志位//////////////////////////////////////
		/* use GPS if it's valid and reference position initialized */使用全球定位系统,如果它是有效的,并参考位置初始化
		bool use_gps_xy = ref_inited && gps_valid && params.w_xy_gps_p > MIN_VALID_W;
		bool use_gps_z = ref_inited && gps_valid && params.w_z_gps_p > MIN_VALID_W;
		/* use VISION if it's valid and has a valid weight parameter */使用VISION,如果它是有效的,并具有有效的权重参数
		bool use_vision_xy = vision_valid && params.w_xy_vision_p > MIN_VALID_W;
		bool use_vision_z = vision_valid && params.w_z_vision_p > MIN_VALID_W;
		/* use MOCAP if it's valid and has a valid weight parameter */使用MOCAP如果它是有效的,并具有有效的权重参数
		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

		if (params.disable_mocap) { //disable mocap if fake gps is used
			use_mocap = false;
		}

		/* use flow if it's valid and (accurate or no GPS available) */
		bool use_flow = flow_valid && (flow_accurate || !use_gps_xy);

		/* use LIDAR if it's valid and lidar altitude estimation is enabled */
		use_lidar = lidar_valid && params.enable_lidar_alt_est;

		bool can_estimate_xy = (eph < max_eph_epv) || use_gps_xy || use_flow || use_vision_xy || use_mocap;

		bool dist_bottom_valid = (t < lidar_valid_time + lidar_valid_timeout);

		float w_xy_gps_p = params.w_xy_gps_p * w_gps_xy;
		float w_xy_gps_v = params.w_xy_gps_v * w_gps_xy;
		float w_z_gps_p = params.w_z_gps_p * w_gps_z;
		float w_z_gps_v = params.w_z_gps_v * w_gps_z;

		float w_xy_vision_p = params.w_xy_vision_p;
		float w_xy_vision_v = params.w_xy_vision_v;
		float w_z_vision_p = params.w_z_vision_p;

		float w_mocap_p = params.w_mocap_p;
//////////////////////////////////根据之前的决策,开始校准数据处理///////////////////////////////////
		/* reduce GPS weight if optical flow is good */
		if (use_flow && flow_accurate) {
			w_xy_gps_p *= params.w_gps_flow;
			w_xy_gps_v *= params.w_gps_flow;
		}

		/* baro offset correction */
		if (use_gps_z) {
			float offs_corr = corr_gps[2][0] * w_z_gps_p * dt;
			baro_offset += offs_corr;
			corr_baro += offs_corr;
		}

		/* accelerometer bias correction for GPS (use buffered rotation matrix) */
		float accel_bias_corr[3] = { 0.0f, 0.0f, 0.0f };

		if (use_gps_xy) {
			accel_bias_corr[0] -= corr_gps[0][0] * w_xy_gps_p * w_xy_gps_p;
			accel_bias_corr[0] -= corr_gps[0][1] * w_xy_gps_v;
			accel_bias_corr[1] -= corr_gps[1][0] * w_xy_gps_p * w_xy_gps_p;
			accel_bias_corr[1] -= corr_gps[1][1] * w_xy_gps_v;
		}

		if (use_gps_z) {
			accel_bias_corr[2] -= corr_gps[2][0] * w_z_gps_p * w_z_gps_p;
			accel_bias_corr[2] -= corr_gps[2][1] * w_z_gps_v;
		}

		/* transform error vector from NED frame to body frame */
		for (int i = 0; i < 3; i++) {
			float c = 0.0f;

			for (int j = 0; j < 3; j++) {
				c += R_gps[j][i] * accel_bias_corr[j];
			}

			if (PX4_ISFINITE(c)) {
				acc_bias[i] += c * params.w_acc_bias * dt;
			}
		}

		/* accelerometer bias correction for VISION (use buffered rotation matrix) */
		accel_bias_corr[0] = 0.0f;
		accel_bias_corr[1] = 0.0f;
		accel_bias_corr[2] = 0.0f;

		if (use_vision_xy) {
			accel_bias_corr[0] -= corr_vision[0][0] * w_xy_vision_p * w_xy_vision_p;
			accel_bias_corr[0] -= corr_vision[0][1] * w_xy_vision_v;
			accel_bias_corr[1] -= corr_vision[1][0] * w_xy_vision_p * w_xy_vision_p;
			accel_bias_corr[1] -= corr_vision[1][1] * w_xy_vision_v;
		}

		if (use_vision_z) {
			accel_bias_corr[2] -= corr_vision[2][0] * w_z_vision_p * w_z_vision_p;
		}

		/* accelerometer bias correction for MOCAP (use buffered rotation matrix) */
		accel_bias_corr[0] = 0.0f;
		accel_bias_corr[1] = 0.0f;
		accel_bias_corr[2] = 0.0f;

		if (use_mocap) {
			accel_bias_corr[0] -= corr_mocap[0][0] * w_mocap_p * w_mocap_p;
			accel_bias_corr[1] -= corr_mocap[1][0] * w_mocap_p * w_mocap_p;
			accel_bias_corr[2] -= corr_mocap[2][0] * w_mocap_p * w_mocap_p;
		}

		/* transform error vector from NED frame to body frame */
		for (int i = 0; i < 3; i++) {
			float c = 0.0f;

			for (int j = 0; j < 3; j++) {
				c += PX4_R(att.R, j, i) * accel_bias_corr[j];
			}

			if (PX4_ISFINITE(c)) {
				acc_bias[i] += c * params.w_acc_bias * dt;
			}
		}

		/* accelerometer bias correction for flow and baro (assume that there is no delay) */
		accel_bias_corr[0] = 0.0f;
		accel_bias_corr[1] = 0.0f;
		accel_bias_corr[2] = 0.0f;

		if (use_flow) {
			accel_bias_corr[0] -= corr_flow[0] * params.w_xy_flow;
			accel_bias_corr[1] -= corr_flow[1] * params.w_xy_flow;
		}

		if (use_lidar) {
			accel_bias_corr[2] -= corr_lidar * params.w_z_lidar * params.w_z_lidar;
		} else {
			accel_bias_corr[2] -= corr_baro * params.w_z_baro * params.w_z_baro;
		}

		/* transform error vector from NED frame to body frame */
		for (int i = 0; i < 3; i++) {
			float c = 0.0f;

			for (int j = 0; j < 3; j++) {
				c += PX4_R(att.R, j, i) * accel_bias_corr[j];
			}

			if (PX4_ISFINITE(c)) {
				acc_bias[i] += c * params.w_acc_bias * dt;
			}
		}
//////////////////////////////////滤波处理///////////////////////////////////
		/* inertial filter prediction for altitude */
		inertial_filter_predict(dt, z_est, acc[2]);

		if (!(PX4_ISFINITE(z_est[0]) && PX4_ISFINITE(z_est[1]))) {
			write_debug_log("BAD ESTIMATE AFTER Z PREDICTION", dt, x_est, y_est, z_est, x_est_prev, y_est_prev, z_est_prev,
					acc, corr_gps, w_xy_gps_p, w_xy_gps_v, corr_mocap, w_mocap_p,
					corr_vision, w_xy_vision_p, w_z_vision_p, w_xy_vision_v);
			memcpy(z_est, z_est_prev, sizeof(z_est));
		}

		/* inertial filter correction for altitude */
		if (use_lidar) {
			inertial_filter_correct(corr_lidar, dt, z_est, 0, params.w_z_lidar);

		} else {
			inertial_filter_correct(corr_baro, dt, z_est, 0, params.w_z_baro);
		}

		if (use_gps_z) {
			epv = fminf(epv, gps.epv);

			inertial_filter_correct(corr_gps[2][0], dt, z_est, 0, w_z_gps_p);
			inertial_filter_correct(corr_gps[2][1], dt, z_est, 1, w_z_gps_v);
		}

		if (use_vision_z) {
			epv = fminf(epv, epv_vision);
			inertial_filter_correct(corr_vision[2][0], dt, z_est, 0, w_z_vision_p);
		}

		if (use_mocap) {
			epv = fminf(epv, epv_mocap);
			inertial_filter_correct(corr_mocap[2][0], dt, z_est, 0, w_mocap_p);
		}

		if (!(PX4_ISFINITE(z_est[0]) && PX4_ISFINITE(z_est[1]))) {
			write_debug_log("BAD ESTIMATE AFTER Z CORRECTION", dt, x_est, y_est, z_est, x_est_prev, y_est_prev, z_est_prev,
					acc, corr_gps, w_xy_gps_p, w_xy_gps_v, corr_mocap, w_mocap_p,
					corr_vision, w_xy_vision_p, w_z_vision_p, w_xy_vision_v);
			memcpy(z_est, z_est_prev, sizeof(z_est));
			memset(corr_gps, 0, sizeof(corr_gps));
			memset(corr_vision, 0, sizeof(corr_vision));
			memset(corr_mocap, 0, sizeof(corr_mocap));
			corr_baro = 0;

		} else {
			memcpy(z_est_prev, z_est, sizeof(z_est));
		}

		if (can_estimate_xy) {
			/* inertial filter prediction for position */
			inertial_filter_predict(dt, x_est, acc[0]);
			inertial_filter_predict(dt, y_est, acc[1]);

			if (!(PX4_ISFINITE(x_est[0]) && PX4_ISFINITE(x_est[1]) && PX4_ISFINITE(y_est[0]) && PX4_ISFINITE(y_est[1]))) {
				write_debug_log("BAD ESTIMATE AFTER PREDICTION", dt, x_est, y_est, z_est, x_est_prev, y_est_prev, z_est_prev,
						acc, corr_gps, w_xy_gps_p, w_xy_gps_v, corr_mocap, w_mocap_p,
						corr_vision, w_xy_vision_p, w_z_vision_p, w_xy_vision_v);
				memcpy(x_est, x_est_prev, sizeof(x_est));
				memcpy(y_est, y_est_prev, sizeof(y_est));
			}

			/* inertial filter correction for position */
			if (use_flow) {
				eph = fminf(eph, eph_flow);

				inertial_filter_correct(corr_flow[0], dt, x_est, 1, params.w_xy_flow * w_flow);
				inertial_filter_correct(corr_flow[1], dt, y_est, 1, params.w_xy_flow * w_flow);
			}

			if (use_gps_xy) {
				eph = fminf(eph, gps.eph);

				inertial_filter_correct(corr_gps[0][0], dt, x_est, 0, w_xy_gps_p);
				inertial_filter_correct(corr_gps[1][0], dt, y_est, 0, w_xy_gps_p);

				if (gps.vel_ned_valid && t < gps.timestamp_velocity + gps_topic_timeout) {
					inertial_filter_correct(corr_gps[0][1], dt, x_est, 1, w_xy_gps_v);
					inertial_filter_correct(corr_gps[1][1], dt, y_est, 1, w_xy_gps_v);
				}
			}

			if (use_vision_xy) {
				eph = fminf(eph, eph_vision);

				inertial_filter_correct(corr_vision[0][0], dt, x_est, 0, w_xy_vision_p);
				inertial_filter_correct(corr_vision[1][0], dt, y_est, 0, w_xy_vision_p);

				if (w_xy_vision_v > MIN_VALID_W) {
					inertial_filter_correct(corr_vision[0][1], dt, x_est, 1, w_xy_vision_v);
					inertial_filter_correct(corr_vision[1][1], dt, y_est, 1, w_xy_vision_v);
				}
			}

			if (use_mocap) {
				eph = fminf(eph, eph_mocap);

				inertial_filter_correct(corr_mocap[0][0], dt, x_est, 0, w_mocap_p);
				inertial_filter_correct(corr_mocap[1][0], dt, y_est, 0, w_mocap_p);
			}

			if (!(PX4_ISFINITE(x_est[0]) && PX4_ISFINITE(x_est[1]) && PX4_ISFINITE(y_est[0]) && PX4_ISFINITE(y_est[1]))) {
				write_debug_log("BAD ESTIMATE AFTER CORRECTION", dt, x_est, y_est, z_est, x_est_prev, y_est_prev, z_est_prev,
						acc, corr_gps, w_xy_gps_p, w_xy_gps_v, corr_mocap, w_mocap_p,
						corr_vision, w_xy_vision_p, w_z_vision_p, w_xy_vision_v);
				memcpy(x_est, x_est_prev, sizeof(x_est));
				memcpy(y_est, y_est_prev, sizeof(y_est));
				memset(corr_gps, 0, sizeof(corr_gps));
				memset(corr_vision, 0, sizeof(corr_vision));
				memset(corr_mocap, 0, sizeof(corr_mocap));
				memset(corr_flow, 0, sizeof(corr_flow));

			} else {
				memcpy(x_est_prev, x_est, sizeof(x_est));
				memcpy(y_est_prev, y_est, sizeof(y_est));
			}

		} else {
			/* gradually reset xy velocity estimates */
			inertial_filter_correct(-x_est[1], dt, x_est, 1, params.w_xy_res_v);
			inertial_filter_correct(-y_est[1], dt, y_est, 1, params.w_xy_res_v);
		}
///////////////////////////////运行地形估计//////////////////////////////////////
		/* run terrain estimator */
		terrain_estimator->predict(dt, &att, &sensor, &lidar);
		terrain_estimator->measurement_update(hrt_absolute_time(), &gps, &lidar, &att);

		if (inav_verbose_mode) {
			/* print updates rate */
			if (t > updates_counter_start + updates_counter_len) {
				float updates_dt = (t - updates_counter_start) * 0.000001f;
				warnx(
					"updates rate: accelerometer = %.1f/s, baro = %.1f/s, gps = %.1f/s, attitude = %.1f/s, flow = %.1f/s, vision = %.1f/s, mocap = %.1f/s",
					(double)(accel_updates / updates_dt),
					(double)(baro_updates / updates_dt),
					(double)(gps_updates / updates_dt),
					(double)(attitude_updates / updates_dt),
					(double)(flow_updates / updates_dt),
					(double)(vision_updates / updates_dt),
					(double)(mocap_updates / updates_dt));
				updates_counter_start = t;
				accel_updates = 0;
				baro_updates = 0;
				gps_updates = 0;
				attitude_updates = 0;
				flow_updates = 0;
				vision_updates = 0;
				mocap_updates = 0;
			}
		}

		if (t > pub_last + PUB_INTERVAL) {
			pub_last = t;

			/* push current estimate to buffer */
			est_buf[buf_ptr][0][0] = x_est[0];
			est_buf[buf_ptr][0][1] = x_est[1];
			est_buf[buf_ptr][1][0] = y_est[0];
			est_buf[buf_ptr][1][1] = y_est[1];
			est_buf[buf_ptr][2][0] = z_est[0];
			est_buf[buf_ptr][2][1] = z_est[1];

			/* push current rotation matrix to buffer */
			memcpy(R_buf[buf_ptr], att.R, sizeof(att.R));

			buf_ptr++;

			if (buf_ptr >= EST_BUF_SIZE) {
				buf_ptr = 0;
			}

///////////////////////////////////////发布位置信息///////////////////////////////////
			/* publish local position */
			local_pos.xy_valid = can_estimate_xy;
			local_pos.v_xy_valid = can_estimate_xy;
			local_pos.xy_global = local_pos.xy_valid && use_gps_xy;
			local_pos.z_global = local_pos.z_valid && use_gps_z;
			local_pos.x = x_est[0];
			local_pos.vx = x_est[1];
			local_pos.y = y_est[0];
			local_pos.vy = y_est[1];
			local_pos.z = z_est[0];
			local_pos.vz = z_est[1];
			local_pos.yaw = att.yaw;
			local_pos.dist_bottom_valid = dist_bottom_valid;
			local_pos.eph = eph;
			local_pos.epv = epv;

			if (local_pos.dist_bottom_valid) {
				local_pos.dist_bottom = dist_ground;
				local_pos.dist_bottom_rate = - z_est[1];
			}

			local_pos.timestamp = t;

			orb_publish(ORB_ID(vehicle_local_position), vehicle_local_position_pub, &local_pos);

			if (local_pos.xy_global && local_pos.z_global) {
				/* publish global position */
				global_pos.timestamp = t;
				global_pos.time_utc_usec = gps.time_utc_usec;

				double est_lat, est_lon;
				map_projection_reproject(&ref, local_pos.x, local_pos.y, &est_lat, &est_lon);

				global_pos.lat = est_lat;
				global_pos.lon = est_lon;
				global_pos.alt = local_pos.ref_alt - local_pos.z;

				global_pos.vel_n = local_pos.vx;
				global_pos.vel_e = local_pos.vy;
				global_pos.vel_d = local_pos.vz;

				global_pos.yaw = local_pos.yaw;

				global_pos.eph = eph;
				global_pos.epv = epv;

				if (terrain_estimator->is_valid()) {
					global_pos.terrain_alt = global_pos.alt - terrain_estimator->get_distance_to_ground();
					global_pos.terrain_alt_valid = true;

				} else {
					global_pos.terrain_alt_valid = false;
				}

				global_pos.pressure_alt = sensor.baro_alt_meter[0];

				if (vehicle_global_position_pub == NULL) {
					vehicle_global_position_pub = orb_advertise(ORB_ID(vehicle_global_position), &global_pos);

				} else {
					orb_publish(ORB_ID(vehicle_global_position), vehicle_global_position_pub, &global_pos);
				}
			}
		}
	}
/***************************************大循环结束********************************/
	warnx("stopped");
	mavlink_log_info(&mavlink_log_pub, "[inav] stopped");
	thread_running = false;
	return 0;
}

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

#ifdef __cplusplus
struct __EXPORT vehicle_local_position_s {
#else
struct vehicle_local_position_s {
#endif
	uint64_t timestamp;
	bool xy_valid;
	bool z_valid;
	bool v_xy_valid;
	bool v_z_valid;
	float x;
	float y;
	float z;
	float vx;
	float vy;
	float vz;
	float yaw;
	bool xy_global;
	bool z_global;
	uint64_t ref_timestamp;
	double ref_lat;
	double ref_lon;
	float ref_alt;
	float dist_bottom;
	float dist_bottom_rate;
	uint64_t surface_bottom_timestamp;
	bool dist_bottom_valid;
	float eph;
	float epv;
#ifdef __cplusplus
#endif
};
#ifdef __cplusplus
struct __EXPORT vehicle_global_position_s {
#else
struct vehicle_global_position_s {
#endif
	uint64_t timestamp;
	uint64_t time_utc_usec;
	double lat;
	double lon;
	float alt;
	float vel_n;
	float vel_e;
	float vel_d;
	float yaw;
	float eph;
	float epv;
	float terrain_alt;
	bool terrain_alt_valid;
	bool dead_reckoning;
	float pressure_alt;
#ifdef __cplusplus
#endif
};

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

#ifdef __cplusplus
struct __EXPORT sensor_combined_s {
#else
struct sensor_combined_s {
#endif
	uint64_t timestamp;
	uint64_t gyro_timestamp[3];
	int16_t gyro_raw[9];
	float gyro_rad_s[9];
	uint32_t gyro_priority[3];
	float gyro_integral_rad[9];
	uint64_t gyro_integral_dt[3];
	uint32_t gyro_errcount[3];
	float gyro_temp[3];
	int16_t accelerometer_raw[9];
	float accelerometer_m_s2[9];
	float accelerometer_integral_m_s[9];
	uint64_t accelerometer_integral_dt[3];
	int16_t accelerometer_mode[3];
	float accelerometer_range_m_s2[3];
	uint64_t accelerometer_timestamp[3];
	uint32_t accelerometer_priority[3];
	uint32_t accelerometer_errcount[3];
	float accelerometer_temp[3];
	int16_t magnetometer_raw[9];
	float magnetometer_ga[9];
	int16_t magnetometer_mode[3];
	float magnetometer_range_ga[3];
	float magnetometer_cuttoff_freq_hz[3];
	uint64_t magnetometer_timestamp[3];
	uint32_t magnetometer_priority[3];
	uint32_t magnetometer_errcount[3];
	float magnetometer_temp[3];
	float baro_pres_mbar[3];
	float baro_alt_meter[3];
	float baro_temp_celcius[3];
	uint64_t baro_timestamp[3];
	uint32_t baro_priority[3];
	uint32_t baro_errcount[3];
	float adc_voltage_v[10];
	uint16_t adc_mapping[10];
	float mcu_temp_celcius;
	float differential_pressure_pa[3];
	uint64_t differential_pressure_timestamp[3];
	float differential_pressure_filtered_pa[3];
	uint32_t differential_pressure_priority[3];
	uint32_t differential_pressure_errcount[3];
#ifdef __cplusplus
	static const int32_t MAGNETOMETER_MODE_NORMAL = 0;
	static const int32_t MAGNETOMETER_MODE_POSITIVE_BIAS = 1;
	static const int32_t MAGNETOMETER_MODE_NEGATIVE_BIAS = 2;
	static const uint32_t SENSOR_PRIO_MIN = 0;
	static const uint32_t SENSOR_PRIO_VERY_LOW = 25;
	static const uint32_t SENSOR_PRIO_LOW = 50;
	static const uint32_t SENSOR_PRIO_DEFAULT = 75;
	static const uint32_t SENSOR_PRIO_HIGH = 100;
	static const uint32_t SENSOR_PRIO_VERY_HIGH = 125;
	static const uint32_t SENSOR_PRIO_MAX = 255;
#endif
};
#ifdef __cplusplus
struct __EXPORT parameter_update_s {
#else
struct parameter_update_s {
#endif
	uint64_t timestamp;
	bool saved;
#ifdef __cplusplus
#endif
};
#ifdef __cplusplus
struct __EXPORT actuator_controls_s {
#else
struct actuator_controls_s {
#endif
	uint64_t timestamp;
	uint64_t timestamp_sample;
	float control[8];
#ifdef __cplusplus
	static const uint8_t NUM_ACTUATOR_CONTROLS = 8;
	static const uint8_t NUM_ACTUATOR_CONTROL_GROUPS = 4;
	static const uint8_t INDEX_ROLL = 0;
	static const uint8_t INDEX_PITCH = 1;
	static const uint8_t INDEX_YAW = 2;
	static const uint8_t INDEX_THROTTLE = 3;
	static const uint8_t INDEX_FLAPS = 4;
	static const uint8_t INDEX_SPOILERS = 5;
	static const uint8_t INDEX_AIRBRAKES = 6;
	static const uint8_t INDEX_LANDING_GEAR = 7;
	static const uint8_t GROUP_INDEX_ATTITUDE = 0;
	static const uint8_t GROUP_INDEX_ATTITUDE_ALTERNATE = 1;
#endif
};
#ifdef __cplusplus
struct __EXPORT actuator_armed_s {
#else
struct actuator_armed_s {
#endif
	uint64_t timestamp;
	bool armed;
	bool prearmed;
	bool ready_to_arm;
	bool lockdown;
	bool force_failsafe;
	bool in_esc_calibration_mode;
#ifdef __cplusplus
#endif
#ifdef __cplusplus
struct __EXPORT vision_position_estimate_s {
#else
struct vision_position_estimate_s {
#endif
	uint32_t id;
	uint64_t timestamp_boot;
	uint64_t timestamp_computer;
	float x;
	float y;
	float z;
	float vx;
	float vy;
	float vz;
	float q[4];
#ifdef __cplusplus
#endif
};

#ifdef __cplusplus
struct __EXPORT att_pos_mocap_s {
#else
struct att_pos_mocap_s {
#endif
	uint32_t id;
	uint64_t timestamp_boot;
	uint64_t timestamp_computer;
	float q[4];
	float x;
	float y;
	float z;
#ifdef __cplusplus
#endif
};

#ifdef __cplusplus
struct __EXPORT vehicle_gps_position_s {
#else
struct vehicle_gps_position_s {
#endif
	uint64_t timestamp_position;
	int32_t lat;
	int32_t lon;
	int32_t alt;
	int32_t alt_ellipsoid;
	uint64_t timestamp_variance;
	float s_variance_m_s;
	float c_variance_rad;
	uint8_t fix_type;
	float eph;
	float epv;
	float hdop;
	float vdop;
	int32_t noise_per_ms;
	int32_t jamming_indicator;
	uint64_t timestamp_velocity;
	float vel_m_s;
	float vel_n_m_s;
	float vel_e_m_s;
	float vel_d_m_s;
	float cog_rad;
	bool vel_ned_valid;
	uint64_t timestamp_time;
	uint64_t time_utc_usec;
	uint8_t satellites_used;
#ifdef __cplusplus

#endif
};

把光流部分单独拿出来

/**************************************这里就是光流的处理*********************************/
			/* optical flow */
			orb_check(optical_flow_sub, &updated);

			if (updated && lidar_valid) {
				orb_copy(ORB_ID(optical_flow), optical_flow_sub, &flow);//光流数据跟新

				flow_time = t;
				float flow_q = flow.quality / 255.0f;
				float dist_bottom = lidar.current_distance;//高度信息

				if (dist_bottom > flow_min_dist && flow_q > params.flow_q_min && PX4_R(att.R, 2, 2) > 0.7f) {
					/* distance to surface */
					//float flow_dist = dist_bottom / PX4_R(att.R, 2, 2); //use this if using sonar
					float flow_dist = dist_bottom; //use this if using lidar

					/* check if flow if too large for accurate measurements */检查速度是否太大,影响光流精确测量
/*****************************计算机架估计的速度*****************************/
					/* calculate estimated velocity in body frame */
					float body_v_est[2] = { 0.0f, 0.0f };

					for (int i = 0; i < 2; i++) {
						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];
					}
/*************设置这个标志,如果光流按照当前速度和姿态率的估计是准确的*************/
					/* set this flag if flow should be accurate according to current velocity and attitude rate estimate */
					flow_accurate = fabsf(body_v_est[1] / flow_dist - att.rollspeed) < max_flow &&
							fabsf(body_v_est[0] / flow_dist + att.pitchspeed) < max_flow;
/*************利用自动驾驶仪(飞控)已经校准的陀螺仪计算光流用的陀螺仪的偏移*************/
					/*calculate offset of flow-gyro using already calibrated gyro from autopilot*/
					flow_gyrospeed[0] = flow.gyro_x_rate_integral / (float)flow.integration_timespan * 1000000.0f;
					flow_gyrospeed[1] = flow.gyro_y_rate_integral / (float)flow.integration_timespan * 1000000.0f;
					flow_gyrospeed[2] = flow.gyro_z_rate_integral / (float)flow.integration_timespan * 1000000.0f;

					//moving average
					if (n_flow >= 100) {
						gyro_offset_filtered[0] = flow_gyrospeed_filtered[0] - att_gyrospeed_filtered[0];
						gyro_offset_filtered[1] = flow_gyrospeed_filtered[1] - att_gyrospeed_filtered[1];
						gyro_offset_filtered[2] = flow_gyrospeed_filtered[2] - att_gyrospeed_filtered[2];
						n_flow = 0;
						flow_gyrospeed_filtered[0] = 0.0f;
						flow_gyrospeed_filtered[1] = 0.0f;
						flow_gyrospeed_filtered[2] = 0.0f;
						att_gyrospeed_filtered[0] = 0.0f;
						att_gyrospeed_filtered[1] = 0.0f;
						att_gyrospeed_filtered[2] = 0.0f;

					} else {
						flow_gyrospeed_filtered[0] = (flow_gyrospeed[0] + n_flow * flow_gyrospeed_filtered[0]) / (n_flow + 1);
						flow_gyrospeed_filtered[1] = (flow_gyrospeed[1] + n_flow * flow_gyrospeed_filtered[1]) / (n_flow + 1);
						flow_gyrospeed_filtered[2] = (flow_gyrospeed[2] + n_flow * flow_gyrospeed_filtered[2]) / (n_flow + 1);
						att_gyrospeed_filtered[0] = (att.pitchspeed + n_flow * att_gyrospeed_filtered[0]) / (n_flow + 1);
						att_gyrospeed_filtered[1] = (att.rollspeed + n_flow * att_gyrospeed_filtered[1]) / (n_flow + 1);
						att_gyrospeed_filtered[2] = (att.yawspeed + n_flow * att_gyrospeed_filtered[2]) / (n_flow + 1);
						n_flow++;
					}

/*******************************偏航补偿(光流传感器不处于旋转中心)->参数在QGC中***********************/
					/*yaw compensation (flow sensor is not in center of rotation) -> params in QGC*/
					yaw_comp[0] = - params.flow_module_offset_y * (flow_gyrospeed[2] - gyro_offset_filtered[2]);
					yaw_comp[1] = params.flow_module_offset_x * (flow_gyrospeed[2] - gyro_offset_filtered[2]);
/***************************将原始流转换成角度流**********************************/
					/* convert raw flow to angular flow (rad/s) */
					float flow_ang[2];
/***************************检查机体速度设定值->它低于阈值->不减去->更好悬停*********************/
					/* check for vehicle rates setpoint - it below threshold -> dont subtract -> better hover */
					orb_check(vehicle_rate_sp_sub, &updated);
					if (updated)
						orb_copy(ORB_ID(vehicle_rates_setpoint), vehicle_rate_sp_sub, &rates_setpoint);

					double rate_threshold = 0.15f;

					if (fabs(rates_setpoint.pitch) < rate_threshold) {
						//warnx("[inav] test ohne comp");
						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)
					}
					else {
						//warnx("[inav] test mit comp");
						//calculate flow [rad/s] and compensate for rotations (and offset of flow-gyro)
						//计算光流的速度[rad/s]并补偿旋转(和光流的陀螺仪的偏置)
						flow_ang[0] = ((flow.pixel_flow_x_integral - flow.gyro_x_rate_integral) / (float)flow.integration_timespan * 1000000.0f
							       + gyro_offset_filtered[0]) * params.flow_k;//for now the flow has to be scaled (to small)
					}

					if (fabs(rates_setpoint.roll) < rate_threshold) {
						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)
					}
					else {
						//calculate flow [rad/s] and compensate for rotations (and offset of flow-gyro)
						//计算光流的速度[rad/s]并补偿旋转(和光流的陀螺仪的偏置)
						flow_ang[1] = ((flow.pixel_flow_y_integral - flow.gyro_y_rate_integral) / (float)flow.integration_timespan * 1000000.0f
							       + gyro_offset_filtered[1]) * params.flow_k;//for now the flow has to be scaled (to small)
					}
/************************光流测量向量********************************/
					/* flow measurements vector */
					float flow_m[3];
					if (fabs(rates_setpoint.yaw) < rate_threshold) {
						flow_m[0] = -flow_ang[0] * flow_dist;
						flow_m[1] = -flow_ang[1] * flow_dist;
					} else {
						flow_m[0] = -flow_ang[0] * flow_dist - yaw_comp[0] * params.flow_k;
						flow_m[1] = -flow_ang[1] * flow_dist - yaw_comp[1] * params.flow_k;
					}
					flow_m[2] = z_est[1];

					/* velocity in NED */NED是北东地,还有一种旋转方式是东北天,都是属于右手系
					float flow_v[2] = { 0.0f, 0.0f };
/************************基于NED工程测量向量,跳过Z分量*********************/
					/* project measurements vector to NED basis, skip Z component */
					for (int i = 0; i < 2; i++) {
						for (int j = 0; j < 3; j++) {
							flow_v[i] += PX4_R(att.R, i, j) * flow_m[j];
						}
					}
/********************向量矫正***********************/
					/* velocity correction */
					corr_flow[0] = flow_v[0] - x_est[1];
					corr_flow[1] = flow_v[1] - y_est[1];
/********************调整矫正权重***********************/					
					/* adjust correction weight */
					float flow_q_weight = (flow_q - params.flow_q_min) / (1.0f - params.flow_q_min);
					w_flow = PX4_R(att.R, 2, 2) * flow_q_weight / fmaxf(1.0f, flow_dist);

/********************如果光流是不准确的,那么减少他的权重***********************/
					/* if flow is not accurate, reduce weight for it */
					// TODO make this more fuzzy
					if (!flow_accurate) {
						w_flow *= 0.05f;
					}
/**********************在理想条件下,在1m的距离EPH =10厘米****************/
					/* under ideal conditions, on 1m distance assume EPH = 10cm */
					eph_flow = 0.1f / w_flow;

					flow_valid = true;

				} else {
					w_flow = 0.0f;
					flow_valid = false;
				}

				flow_updates++;
			}
/**************************************光流结束*********************************/
接下来就到了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[]);
int mc_pos_control_main(int argc, char *argv[])
{
	if (argc < 2) {
		warnx("usage: mc_pos_control {start|stop|status}");
		return 1;
	}

	if (!strcmp(argv[1], "start")) {

		if (pos_control::g_control != nullptr) {
			warnx("already running");
			return 1;
		}

		pos_control::g_control = new MulticopterPositionControl;

		if (pos_control::g_control == nullptr) {
			warnx("alloc failed");
			return 1;
		}

		if (OK != pos_control::g_control->start()) {
			delete pos_control::g_control;
			pos_control::g_control = nullptr;
			warnx("start failed");
			return 1;
		}

		return 0;
	}

	if (!strcmp(argv[1], "stop")) {
		if (pos_control::g_control == nullptr) {
			warnx("not running");
			return 1;
		}

		delete pos_control::g_control;
		pos_control::g_control = nullptr;
		return 0;
	}

	if (!strcmp(argv[1], "status")) {
		if (pos_control::g_control) {
			warnx("running");
			return 0;

		} else {
			warnx("not running");
			return 1;
		}
	}

	warnx("unrecognized command");
	return 1;
}
这个里面就start和status需要看

int
MulticopterPositionControl::start()
{
	ASSERT(_control_task == -1);

	/* start the task */
	_control_task = px4_task_spawn_cmd("mc_pos_control",
					   SCHED_DEFAULT,
					   SCHED_PRIORITY_MAX - 5,
					   1900,
					   (px4_main_t)&MulticopterPositionControl::task_main_trampoline,
					   nullptr);

	if (_control_task < 0) {
		warn("task start failed");
		return -errno;
	}

	return OK;
}
void
MulticopterPositionControl::task_main_trampoline(int argc, char *argv[])
{
	pos_control::g_control->task_main();
}
void
MulticopterPositionControl::task_main()
{
/***************************订阅各种信息***********************/
	/*
	 * do subscriptions
	 */
	_vehicle_status_sub = orb_subscribe(ORB_ID(vehicle_status));
	_ctrl_state_sub = orb_subscribe(ORB_ID(control_state));
	_att_sp_sub = orb_subscribe(ORB_ID(vehicle_attitude_setpoint));
	_control_mode_sub = orb_subscribe(ORB_ID(vehicle_control_mode));
	_params_sub = orb_subscribe(ORB_ID(parameter_update));
	_manual_sub = orb_subscribe(ORB_ID(manual_control_setpoint));
	_arming_sub = orb_subscribe(ORB_ID(actuator_armed));
	_local_pos_sub = orb_subscribe(ORB_ID(vehicle_local_position));
	//这个就是从position_estimator_inav_main.cpp中来的,里面包含光流数据
	_pos_sp_triplet_sub = orb_subscribe(ORB_ID(position_setpoint_triplet));
	_local_pos_sp_sub = orb_subscribe(ORB_ID(vehicle_local_position_setpoint));
	_global_vel_sp_sub = orb_subscribe(ORB_ID(vehicle_global_velocity_setpoint));


	parameters_update(true);//参数跟新
/*****************初始化关键结构的值,直到第一次定期更新*****************/
	/* initialize values of critical structs until first regular update */
	_arming.armed = false;
/*****************初始跟新,获取所有传感器的数据和状态数据*****************/
	/* get an initial update for all sensor and status data */
	poll_subscriptions();//具体在下面展开了

	bool reset_int_z = true;
	bool reset_int_z_manual = false;
	bool reset_int_xy = true;
	bool reset_yaw_sp = true;
	bool was_armed = false;

	hrt_abstime t_prev = 0;

	math::Vector<3> thrust_int;
	thrust_int.zero();


	math::Matrix<3, 3> R;
	R.identity();

	/* wakeup source */
	px4_pollfd_struct_t fds[1];

	fds[0].fd = _local_pos_sub;
	fds[0].events = POLLIN;
/***********************大循环***************************/
	while (!_task_should_exit) {
		/* wait for up to 500ms for data */等待500ms来获取数据
		int pret = px4_poll(&fds[0], (sizeof(fds) / sizeof(fds[0])), 500);

		/* timed out - periodic check for _task_should_exit */
		if (pret == 0) {
			continue;
		}

		/* this is undesirable but not much we can do */
		if (pret < 0) {
			warn("poll error %d, %d", pret, errno);
			continue;
		}
//////////////获取所有传感器的数据和状态数据//////////////////////
		poll_subscriptions();
///////////////////////////参数跟新////////////////////////
		parameters_update(false);

		hrt_abstime t = hrt_absolute_time();
		float dt = t_prev != 0 ? (t - t_prev) * 0.000001f : 0.0f;//dt是控制周期吗?
		t_prev = t;

		// set dt for control blocks
		setDt(dt);
/////////////////////////判断是否锁定////////////////////////////
		if (_control_mode.flag_armed && !was_armed) {
			/* reset setpoints and integrals on arming */
			_reset_pos_sp = true;
			_reset_alt_sp = true;
			_vel_sp_prev.zero();
			reset_int_z = true;
			reset_int_xy = true;
			reset_yaw_sp = true;
		}
//////////////////复位yaw和姿态的设定    为垂直起降     fw模式/////////////////////
		/* reset yaw and altitude setpoint for VTOL which are in fw mode */
		if (_vehicle_status.is_vtol) {
			if (!_vehicle_status.is_rotary_wing) {
				reset_yaw_sp = true;
				_reset_alt_sp = true;
			}
		}
////////////////////跟新前一时刻锁定的状态///////////////////////
		//Update previous arming state
		was_armed = _control_mode.flag_armed;

		update_ref();//跟新参考点,是不是期望目标?该函数在下面会展开
////////////////////跟新速度导数(加速度)  独立于当前飞行模式///////////
		/* Update velocity derivative,
		 * independent of the current flight mode
		 */
		if (_local_pos.timestamp > 0) {

			if (PX4_ISFINITE(_local_pos.x) && //ISFINITE是测试数据是否是个有限数的函数
					PX4_ISFINITE(_local_pos.y) &&
					PX4_ISFINITE(_local_pos.z)) {

				_pos(0) = _local_pos.x;
				_pos(1) = _local_pos.y;
				_pos(2) = _local_pos.z;
			}

			if (PX4_ISFINITE(_local_pos.vx) &&
					PX4_ISFINITE(_local_pos.vy) &&
					PX4_ISFINITE(_local_pos.vz)) {

				_vel(0) = _local_pos.vx;
				_vel(1) = _local_pos.vy;
				_vel(2) = _local_pos.vz;
			}

			_vel_err_d(0) = _vel_x_deriv.update(-_vel(0));
			_vel_err_d(1) = _vel_y_deriv.update(-_vel(1));
			_vel_err_d(2) = _vel_z_deriv.update(-_vel(2));
		}
///////////////复位在非手动模式下的水平垂直位置保持标志位,让位置和姿态不被控制//////////////////////
		// reset the horizontal and vertical position hold flags for non-manual modes
		// or if position / altitude is not controlled
		if (!_control_mode.flag_control_position_enabled || !_control_mode.flag_control_manual_enabled) {
			_pos_hold_engaged = false;
		}

		if (!_control_mode.flag_control_altitude_enabled || !_control_mode.flag_control_manual_enabled) {
			_alt_hold_engaged = false;
		}
//////////////////根据控制模式选择对应的控制函数//////////////////////
		if (_control_mode.flag_control_altitude_enabled ||
				_control_mode.flag_control_position_enabled ||
				_control_mode.flag_control_climb_rate_enabled ||
				_control_mode.flag_control_velocity_enabled) {

			_vel_ff.zero();
			/////////默认运行位置和姿态控制,在control_*函数中可以失能这种模式而直接在循环中运行速度控制模式/////////
			/* by default, run position/altitude controller. the control_* functions
			 * can disable this and run velocity controllers directly in this cycle */
			_run_pos_control = true;
			_run_alt_control = true;
			/////////选择控制源/////////////
			/* select control source */
			if (_control_mode.flag_control_manual_enabled) {
				/* manual control */
				control_manual(dt);//手动控制,在下面会展开
				_mode_auto = false;

			} else if (_control_mode.flag_control_offboard_enabled) {
				/* offboard control */
				control_offboard(dt);//和control_manual(dt);貌似都是根据具体情况设定期望值
				_mode_auto = false;

			} else {
				/* AUTO */
				control_auto(dt);//自动控制
			}
/********感觉这3个函数功能是实现根据实际复杂情况调整期望值,之后用控制函数一个一个的达到期望值*************/
			/* weather-vane mode for vtol: disable yaw control */风向标模式垂直起降:禁用偏航控制
			if (!_control_mode.flag_control_manual_enabled && _pos_sp_triplet.current.disable_mc_yaw_control == true) {
				_att_sp.disable_mc_yaw_control = true;

			} else {
				/* reset in case of setpoint updates */
				_att_sp.disable_mc_yaw_control = false;
			}
/////////////////////////判断///////////////////////
			if (!_control_mode.flag_control_manual_enabled && _pos_sp_triplet.current.valid
					&& _pos_sp_triplet.current.type == position_setpoint_s::SETPOINT_TYPE_IDLE) {
				/* idle state, don't run controller and set zero thrust */
				//////////空闲状态,不运行控制,并设置为零推力//////////
				R.identity();
				memcpy(&_att_sp.R_body[0], R.data, sizeof(_att_sp.R_body));
				_att_sp.R_valid = true;

				_att_sp.roll_body = 0.0f;
				_att_sp.pitch_body = 0.0f;
				_att_sp.yaw_body = _yaw;
				_att_sp.thrust = 0.0f;

				_att_sp.timestamp = hrt_absolute_time();
/////////////////////发布姿态设定值///////////////////////
				/* publish attitude setpoint */
				if (_att_sp_pub != nullptr) {
					orb_publish(_attitude_setpoint_id, _att_sp_pub, &_att_sp);

				} else if (_attitude_setpoint_id) {
					_att_sp_pub = orb_advertise(_attitude_setpoint_id, &_att_sp);
				}
/////////////////////////另一个判断///////////////////////
			} else if (_control_mode.flag_control_manual_enabled
					&& _vehicle_status.condition_landed) {
				/* don't run controller when landed */着陆不要运行控制器
				_reset_pos_sp = true;
				_reset_alt_sp = true;
				_mode_auto = false;
				reset_int_z = true;
				reset_int_xy = true;

				R.identity();
				memcpy(&_att_sp.R_body[0], R.data, sizeof(_att_sp.R_body));
				_att_sp.R_valid = true;

				_att_sp.roll_body = 0.0f;
				_att_sp.pitch_body = 0.0f;
				_att_sp.yaw_body = _yaw;
				_att_sp.thrust = 0.0f;

				_att_sp.timestamp = hrt_absolute_time();

				/* publish attitude setpoint */
				if (_att_sp_pub != nullptr) {
					orb_publish(_attitude_setpoint_id, _att_sp_pub, &_att_sp);

				} else if (_attitude_setpoint_id) {
					_att_sp_pub = orb_advertise(_attitude_setpoint_id, &_att_sp);
				}
/////////////////////////另一个判断///////////////////////
			} else {
				/* run position & altitude controllers, if enabled (otherwise use already computed velocity setpoints) */
				if (_run_pos_control) {
					_vel_sp(0) = (_pos_sp(0) - _pos(0)) * _params.pos_p(0);
					_vel_sp(1) = (_pos_sp(1) - _pos(1)) * _params.pos_p(1);
				}

				// do not go slower than the follow target velocity when position tracking is active (set to valid)

				if(_pos_sp_triplet.current.type == position_setpoint_s::SETPOINT_TYPE_FOLLOW_TARGET &&
					_pos_sp_triplet.current.velocity_valid &&
					_pos_sp_triplet.current.position_valid) {

					math::Vector<3> ft_vel(_pos_sp_triplet.current.vx, _pos_sp_triplet.current.vy, 0);

					float cos_ratio = (ft_vel*_vel_sp)/(ft_vel.length()*_vel_sp.length());

					// only override velocity set points when uav is traveling in same direction as target and vector component
					// is greater than calculated position set point velocity component

					if(cos_ratio > 0) {
						ft_vel *= (cos_ratio);
						// min speed a little faster than target vel
						ft_vel += ft_vel.normalized()*1.5f;
					} else {
						ft_vel.zero();
					}

					_vel_sp(0) = fabs(ft_vel(0)) > fabs(_vel_sp(0)) ? ft_vel(0) : _vel_sp(0);
					_vel_sp(1) = fabs(ft_vel(1)) > fabs(_vel_sp(1)) ? ft_vel(1) : _vel_sp(1);

					// track target using velocity only

				} else if (_pos_sp_triplet.current.type == position_setpoint_s::SETPOINT_TYPE_FOLLOW_TARGET &&
					_pos_sp_triplet.current.velocity_valid) {

					_vel_sp(0) = _pos_sp_triplet.current.vx;
					_vel_sp(1) = _pos_sp_triplet.current.vy;
				}

				if (_run_alt_control) {
					_vel_sp(2) = (_pos_sp(2) - _pos(2)) * _params.pos_p(2);
				}

				/* make sure velocity setpoint is saturated in xy*/
				float vel_norm_xy = sqrtf(_vel_sp(0) * _vel_sp(0) +
							  _vel_sp(1) * _vel_sp(1));

				if (vel_norm_xy > _params.vel_max(0)) {
					/* note assumes vel_max(0) == vel_max(1) */
					_vel_sp(0) = _vel_sp(0) * _params.vel_max(0) / vel_norm_xy;
					_vel_sp(1) = _vel_sp(1) * _params.vel_max(1) / vel_norm_xy;
				}

				/* make sure velocity setpoint is saturated in z*/
				float vel_norm_z = sqrtf(_vel_sp(2) * _vel_sp(2));

				if (vel_norm_z > _params.vel_max(2)) {
					_vel_sp(2) = _vel_sp(2) * _params.vel_max(2) / vel_norm_z;
				}

				if (!_control_mode.flag_control_position_enabled) {
					_reset_pos_sp = true;
				}

				if (!_control_mode.flag_control_altitude_enabled) {
					_reset_alt_sp = true;
				}

				if (!_control_mode.flag_control_velocity_enabled) {
					_vel_sp_prev(0) = _vel(0);
					_vel_sp_prev(1) = _vel(1);
					_vel_sp(0) = 0.0f;
					_vel_sp(1) = 0.0f;
					control_vel_enabled_prev = false;
				}

				if (!_control_mode.flag_control_climb_rate_enabled) {
					_vel_sp(2) = 0.0f;
				}

				/* use constant descend rate when landing, ignore altitude setpoint */
				if (!_control_mode.flag_control_manual_enabled && _pos_sp_triplet.current.valid
						&& _pos_sp_triplet.current.type == position_setpoint_s::SETPOINT_TYPE_LAND) {
					_vel_sp(2) = _params.land_speed;
				}

				/* special thrust setpoint generation for takeoff from ground */
				if (!_control_mode.flag_control_manual_enabled && _pos_sp_triplet.current.valid
						&& _pos_sp_triplet.current.type == position_setpoint_s::SETPOINT_TYPE_TAKEOFF
						&& _control_mode.flag_armed) {

					// check if we are not already in air.
					// if yes then we don't need a jumped takeoff anymore
					if (!_takeoff_jumped && !_vehicle_status.condition_landed && fabsf(_takeoff_thrust_sp) < FLT_EPSILON) {
						_takeoff_jumped = true;
					}

					if (!_takeoff_jumped) {
						// ramp thrust setpoint up
						if (_vel(2) > -(_params.tko_speed / 2.0f)) {
							_takeoff_thrust_sp += 0.5f * dt;
							_vel_sp.zero();
							_vel_prev.zero();

						} else {
							// copter has reached our takeoff speed. split the thrust setpoint up
							// into an integral part and into a P part
							thrust_int(2) = _takeoff_thrust_sp - _params.vel_p(2) * fabsf(_vel(2));
							thrust_int(2) = -math::constrain(thrust_int(2), _params.thr_min, _params.thr_max);
							_vel_sp_prev(2) = -_params.tko_speed;
							_takeoff_jumped = true;
							reset_int_z = false;
						}
					}

					if (_takeoff_jumped) {
						_vel_sp(2) = -_params.tko_speed;
					}

				} else {
					_takeoff_jumped = false;
					_takeoff_thrust_sp = 0.0f;
				}



				// limit total horizontal acceleration
				math::Vector<2> acc_hor;
				acc_hor(0) = (_vel_sp(0) - _vel_sp_prev(0)) / dt;
				acc_hor(1) = (_vel_sp(1) - _vel_sp_prev(1)) / dt;

				if (acc_hor.length() > _params.acc_hor_max) {
					acc_hor.normalize();
					acc_hor *= _params.acc_hor_max;
					math::Vector<2> vel_sp_hor_prev(_vel_sp_prev(0), _vel_sp_prev(1));
					math::Vector<2> vel_sp_hor = acc_hor * dt + vel_sp_hor_prev;
					_vel_sp(0) = vel_sp_hor(0);
					_vel_sp(1) = vel_sp_hor(1);
				}

				// limit vertical acceleration
				float acc_v = (_vel_sp(2) - _vel_sp_prev(2)) / dt;

				if (fabsf(acc_v) > 2 * _params.acc_hor_max) {
					acc_v /= fabsf(acc_v);
					_vel_sp(2) = acc_v * 2 * _params.acc_hor_max * dt + _vel_sp_prev(2);
				}

				_vel_sp_prev = _vel_sp;

				_global_vel_sp.vx = _vel_sp(0);
				_global_vel_sp.vy = _vel_sp(1);
				_global_vel_sp.vz = _vel_sp(2);

				/* publish velocity setpoint */
				if (_global_vel_sp_pub != nullptr) {
					orb_publish(ORB_ID(vehicle_global_velocity_setpoint), _global_vel_sp_pub, &_global_vel_sp);

				} else {
					_global_vel_sp_pub = orb_advertise(ORB_ID(vehicle_global_velocity_setpoint), &_global_vel_sp);
				}

				if (_control_mode.flag_control_climb_rate_enabled || _control_mode.flag_control_velocity_enabled) {
					/* reset integrals if needed */
					if (_control_mode.flag_control_climb_rate_enabled) {
						if (reset_int_z) {
							reset_int_z = false;
							float i = _params.thr_min;

							if (reset_int_z_manual) {
								i = _params.thr_hover;

								if (i < _params.thr_min) {
									i = _params.thr_min;

								} else if (i > _params.thr_max) {
									i = _params.thr_max;
								}
							}

							thrust_int(2) = -i;
						}

					} else {
						reset_int_z = true;
					}

					if (_control_mode.flag_control_velocity_enabled) {
						if (reset_int_xy) {
							reset_int_xy = false;
							thrust_int(0) = 0.0f;
							thrust_int(1) = 0.0f;
						}

					} else {
						reset_int_xy = true;
					}

					/* velocity error */
					math::Vector<3> vel_err = _vel_sp - _vel;
					////////////检查是否从非速度模式转到速度控制模式,如果是的话,校正xy的速度设定值以便姿态设定值保持不变/////
					// check if we have switched from a non-velocity controlled mode into a velocity controlled mode
					// if yes, then correct xy velocity setpoint such that the attitude setpoint is continuous
					if (!control_vel_enabled_prev && _control_mode.flag_control_velocity_enabled) {

						// choose velocity xyz setpoint such that the resulting thrust setpoint has the direction
						// given by the last attitude setpoint
						_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);
						_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);
						_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);
						_vel_sp_prev(0) = _vel_sp(0);
						_vel_sp_prev(1) = _vel_sp(1);
						_vel_sp_prev(2) = _vel_sp(2);
						control_vel_enabled_prev = true;

						// compute updated velocity error
						vel_err = _vel_sp - _vel;
					}

					/* thrust vector in NED frame */
					math::Vector<3> thrust_sp = vel_err.emult(_params.vel_p) + _vel_err_d.emult(_params.vel_d) + thrust_int;

					if (_pos_sp_triplet.current.type == position_setpoint_s::SETPOINT_TYPE_TAKEOFF
							&& !_takeoff_jumped && !_control_mode.flag_control_manual_enabled) {
						// for jumped takeoffs use special thrust setpoint calculated above
						thrust_sp.zero();
						thrust_sp(2) = -_takeoff_thrust_sp;
					}

					if (!_control_mode.flag_control_velocity_enabled) {
						thrust_sp(0) = 0.0f;
						thrust_sp(1) = 0.0f;
					}

					if (!_control_mode.flag_control_climb_rate_enabled) {
						thrust_sp(2) = 0.0f;
					}

					/* limit thrust vector and check for saturation */
					bool saturation_xy = false;
					bool saturation_z = false;

					/* limit min lift */
					float thr_min = _params.thr_min;

					if (!_control_mode.flag_control_velocity_enabled && thr_min < 0.0f) {
						/* don't allow downside thrust direction in manual attitude mode */
						thr_min = 0.0f;
					}

					float thrust_abs = thrust_sp.length();
					float tilt_max = _params.tilt_max_air;
					float thr_max = _params.thr_max;
					/* filter vel_z over 1/8sec */
					_vel_z_lp = _vel_z_lp * (1.0f - dt * 8.0f) + dt * 8.0f * _vel(2);
					/* filter vel_z change over 1/8sec */
					float vel_z_change = (_vel(2) - _vel_prev(2)) / dt;
					_acc_z_lp = _acc_z_lp * (1.0f - dt * 8.0f) + dt * 8.0f * vel_z_change;

					/* adjust limits for landing mode */调整限制  着陆模式
					if (!_control_mode.flag_control_manual_enabled && _pos_sp_triplet.current.valid &&
							_pos_sp_triplet.current.type == position_setpoint_s::SETPOINT_TYPE_LAND) {
						/* limit max tilt and min lift when landing */
						tilt_max = _params.tilt_max_land;

						if (thr_min < 0.0f) {
							thr_min = 0.0f;
						}

						/* descend stabilized, we're landing */
						if (!_in_landing && !_lnd_reached_ground
								&& (float)fabs(_acc_z_lp) < 0.1f
								&& _vel_z_lp > 0.5f * _params.land_speed) {
							_in_landing = true;
						}

						/* assume ground, cut thrust */
						if (_in_landing
								&& _vel_z_lp < 0.1f) {
							thr_max = 0.0f;
							_in_landing = false;
							_lnd_reached_ground = true;
						}

						/* once we assumed to have reached the ground always cut the thrust.
							Only free fall detection below can revoke this
						*/
						if (!_in_landing && _lnd_reached_ground) {
							thr_max = 0.0f;
						}

						/* if we suddenly fall, reset landing logic and remove thrust limit */
						if (_lnd_reached_ground
								/* XXX: magic value, assuming free fall above 4m/s2 acceleration */
								&& (_acc_z_lp > 4.0f
								    || _vel_z_lp > 2.0f * _params.land_speed)) {
							thr_max = _params.thr_max;
							_in_landing = false;
							_lnd_reached_ground = false;
						}

					} else {
						_in_landing = false;
						_lnd_reached_ground = false;
					}

					/* limit min lift */
					if (-thrust_sp(2) < thr_min) {
						thrust_sp(2) = -thr_min;
						saturation_z = true;
					}

					if (_control_mode.flag_control_velocity_enabled) {

						/* limit max tilt */
						if (thr_min >= 0.0f && tilt_max < M_PI_F / 2 - 0.05f) {
							/* absolute horizontal thrust */
							float thrust_sp_xy_len = math::Vector<2>(thrust_sp(0), thrust_sp(1)).length();

							if (thrust_sp_xy_len > 0.01f) {
								/* max horizontal thrust for given vertical thrust*/
								float thrust_xy_max = -thrust_sp(2) * tanf(tilt_max);

								if (thrust_sp_xy_len > thrust_xy_max) {
									float k = thrust_xy_max / thrust_sp_xy_len;
									thrust_sp(0) *= k;
									thrust_sp(1) *= k;
									saturation_xy = true;
								}
							}
						}
					}

					if (_control_mode.flag_control_altitude_enabled) {
						/* thrust compensation for altitude only control modes */推力的补偿  高度控制模式
						float att_comp;

						if (_R(2, 2) > TILT_COS_MAX) {
							att_comp = 1.0f / _R(2, 2);

						} else if (_R(2, 2) > 0.0f) {
							att_comp = ((1.0f / TILT_COS_MAX - 1.0f) / TILT_COS_MAX) * _R(2, 2) + 1.0f;
							saturation_z = true;

						} else {
							att_comp = 1.0f;
							saturation_z = true;
						}

						thrust_sp(2) *= att_comp;
					}
					///////////////////限制最大推力///////////////////
					/* limit max thrust */
					thrust_abs = thrust_sp.length(); /* recalculate because it might have changed */

					if (thrust_abs > thr_max) {
						if (thrust_sp(2) < 0.0f) {
							if (-thrust_sp(2) > thr_max) {
								/* thrust Z component is too large, limit it */
								thrust_sp(0) = 0.0f;
								thrust_sp(1) = 0.0f;
								thrust_sp(2) = -thr_max;
								saturation_xy = true;
								saturation_z = true;

							} else {
								/* preserve thrust Z component and lower XY, keeping altitude is more important than position */
								float thrust_xy_max = sqrtf(thr_max * thr_max - thrust_sp(2) * thrust_sp(2));
								float thrust_xy_abs = math::Vector<2>(thrust_sp(0), thrust_sp(1)).length();
								float k = thrust_xy_max / thrust_xy_abs;
								thrust_sp(0) *= k;
								thrust_sp(1) *= k;
								saturation_xy = true;
							}

						} else {
							/* Z component is negative, going down, simply limit thrust vector */
							float k = thr_max / thrust_abs;
							thrust_sp *= k;
							saturation_xy = true;
							saturation_z = true;
						}

						thrust_abs = thr_max;
					}
					///////////跟新积分//////////
					/* update integrals */
					if (_control_mode.flag_control_velocity_enabled && !saturation_xy) {
						thrust_int(0) += vel_err(0) * _params.vel_i(0) * dt;
						thrust_int(1) += vel_err(1) * _params.vel_i(1) * dt;
					}

					if (_control_mode.flag_control_climb_rate_enabled && !saturation_z) {
						thrust_int(2) += vel_err(2) * _params.vel_i(2) * dt;

						/* protection against flipping on ground when landing */
						if (thrust_int(2) > 0.0f) {
							thrust_int(2) = 0.0f;
						}
					}
					/////////////根据推力矢量计算的姿态设定值//////////////
					/* calculate attitude setpoint from thrust vector */
					if (_control_mode.flag_control_velocity_enabled) {
						/* desired body_z axis = -normalize(thrust_vector) */
						math::Vector<3> body_x;
						math::Vector<3> body_y;
						math::Vector<3> body_z;

						if (thrust_abs > SIGMA) {
							body_z = -thrust_sp / thrust_abs;

						} else {
							/* no thrust, set Z axis to safe value */
							body_z.zero();
							body_z(2) = 1.0f;
						}

						/* vector of desired yaw direction in XY plane, rotated by PI/2 */
						math::Vector<3> y_C(-sinf(_att_sp.yaw_body), cosf(_att_sp.yaw_body), 0.0f);

						if (fabsf(body_z(2)) > SIGMA) {
							/* desired body_x axis, orthogonal to body_z */
							body_x = y_C % body_z;

							/* keep nose to front while inverted upside down */
							if (body_z(2) < 0.0f) {
								body_x = -body_x;
							}

							body_x.normalize();

						} else {
							/* desired thrust is in XY plane, set X downside to construct correct matrix,
							 * but yaw component will not be used actually */
							body_x.zero();
							body_x(2) = 1.0f;
						}

						/* desired body_y axis */
						body_y = body_z % body_x;

						/* fill rotation matrix */
						for (int i = 0; i < 3; i++) {
							R(i, 0) = body_x(i);
							R(i, 1) = body_y(i);
							R(i, 2) = body_z(i);
						}
						////////////////////复制旋转矩阵到姿态设定值主题中////////////////////
						/* copy rotation matrix to attitude setpoint topic */
						memcpy(&_att_sp.R_body[0], R.data, sizeof(_att_sp.R_body));
						_att_sp.R_valid = true;
						/////////////////////复制四元数设定值到姿态设定值主题中///////////////////////////
						/* copy quaternion setpoint to attitude setpoint topic */
						math::Quaternion q_sp;
						q_sp.from_dcm(R);
						memcpy(&_att_sp.q_d[0], &q_sp.data[0], sizeof(_att_sp.q_d));
						//////////////计算欧拉角,只记录,不得用于控制//////////////////
						/* calculate euler angles, for logging only, must not be used for control */
						math::Vector<3> euler = R.to_euler();
						_att_sp.roll_body = euler(0);
						_att_sp.pitch_body = euler(1);
						/* yaw already used to construct rot matrix, but actual rotation matrix can have different yaw near singularity */
						/////////偏航已经用于构建rot矩阵,但实际旋转矩阵可以有奇点附近的不同偏航///////////
					} else if (!_control_mode.flag_control_manual_enabled) {
						/////////////没有位置控制的自主高度控制(故障安全降落),强制水平姿态,不改变yaw///////////////////
						/* autonomous altitude control without position control (failsafe landing),
						 * force level attitude, don't change yaw */
						R.from_euler(0.0f, 0.0f, _att_sp.yaw_body);
						////////////////////复制旋转矩阵到姿态设定值主题中////////////////////
						/* copy rotation matrix to attitude setpoint topic */
						memcpy(&_att_sp.R_body[0], R.data, sizeof(_att_sp.R_body));
						_att_sp.R_valid = true;
						/////////////////////复制四元数设定值到姿态设定值主题中///////////////////////////
						/* copy quaternion setpoint to attitude setpoint topic */
						math::Quaternion q_sp;
						q_sp.from_dcm(R);
						memcpy(&_att_sp.q_d[0], &q_sp.data[0], sizeof(_att_sp.q_d));

						_att_sp.roll_body = 0.0f;
						_att_sp.pitch_body = 0.0f;
					}

					_att_sp.thrust = thrust_abs;

					/* save thrust setpoint for logging */保存推理设定值在logging里
					_local_pos_sp.acc_x = thrust_sp(0) * ONE_G;
					_local_pos_sp.acc_y = thrust_sp(1) * ONE_G;
					_local_pos_sp.acc_z = thrust_sp(2) * ONE_G;

					_att_sp.timestamp = hrt_absolute_time();


				} else {
					reset_int_z = true;
				}
			}

			/* fill local position, velocity and thrust setpoint */
			_local_pos_sp.timestamp = hrt_absolute_time();
			_local_pos_sp.x = _pos_sp(0);
			_local_pos_sp.y = _pos_sp(1);
			_local_pos_sp.z = _pos_sp(2);
			_local_pos_sp.yaw = _att_sp.yaw_body;
			_local_pos_sp.vx = _vel_sp(0);
			_local_pos_sp.vy = _vel_sp(1);
			_local_pos_sp.vz = _vel_sp(2);
////////////////////////发布当地位置设定值////////////////////
/***************这个应该是这段程序处理的最后结果********************/
			/* publish local position setpoint */
			if (_local_pos_sp_pub != nullptr) {
				orb_publish(ORB_ID(vehicle_local_position_setpoint), _local_pos_sp_pub, &_local_pos_sp);

			} else {
				_local_pos_sp_pub = orb_advertise(ORB_ID(vehicle_local_position_setpoint), &_local_pos_sp);
			}

		} else {位置控制模式失能,那么复位设定值
			/* position controller disabled, reset setpoints */
			_reset_alt_sp = true;
			_reset_pos_sp = true;
			_mode_auto = false;
			reset_int_z = true;
			reset_int_xy = true;
			control_vel_enabled_prev = false;

			/* store last velocity in case a mode switch to position control occurs */
			_vel_sp_prev = _vel;
		}
//////////////////////从手动控制中产生姿态设定值//////////////////////////
		/* generate attitude setpoint from manual controls */
		if (_control_mode.flag_control_manual_enabled && _control_mode.flag_control_attitude_enabled) {

			/* reset yaw setpoint to current position if needed */
			if (reset_yaw_sp) {
				reset_yaw_sp = false;
				_att_sp.yaw_body = _yaw;
			}
//////////////////在地上时,不动偏航方向/////////////////////////
			/* do not move yaw while sitting on the ground */
			else if (!_vehicle_status.condition_landed &&
					!(!_control_mode.flag_control_altitude_enabled && _manual.z < 0.1f)) {

				/* we want to know the real constraint, and global overrides manual */
				const float yaw_rate_max = (_params.man_yaw_max < _params.global_yaw_max) ? _params.man_yaw_max :
							   _params.global_yaw_max;
				const float yaw_offset_max = yaw_rate_max / _params.mc_att_yaw_p;

				_att_sp.yaw_sp_move_rate = _manual.r * yaw_rate_max;
				float yaw_target = _wrap_pi(_att_sp.yaw_body + _att_sp.yaw_sp_move_rate * dt);
				float yaw_offs = _wrap_pi(yaw_target - _yaw);
////////////////////如果对于系统跟踪,yaw偏移变得过大,那么停止其转移////////////////////
				// If the yaw offset became too big for the system to track stop
				// shifting it

				// XXX this needs inspection - probably requires a clamp, not an if
				if (fabsf(yaw_offs) < yaw_offset_max) {
					_att_sp.yaw_body = yaw_target;
				}
			}
//////////////////////直接控制侧倾和俯仰,如果协助速度控制器没有被激活////////////////////
			/* control roll and pitch directly if we no aiding velocity controller is active */
			if (!_control_mode.flag_control_velocity_enabled) {
				_att_sp.roll_body = _manual.y * _params.man_roll_max;
				_att_sp.pitch_body = -_manual.x * _params.man_pitch_max;
			}
//////////////////////控制油门直接,如果没有爬升率控制器被激活/////////////////////
			/* control throttle directly if no climb rate controller is active */
			if (!_control_mode.flag_control_climb_rate_enabled) {
				float thr_val = throttle_curve(_manual.z, _params.thr_hover);
				_att_sp.thrust = math::min(thr_val, _manual_thr_max.get());

				/* enforce minimum throttle if not landed */
				if (!_vehicle_status.condition_landed) {
					_att_sp.thrust = math::max(_att_sp.thrust, _manual_thr_min.get());
				}
			}

			math::Matrix<3, 3> R_sp;
//////////////////////构建姿态设定值的旋转矩阵///////////////////////
			/* construct attitude setpoint rotation matrix */
			R_sp.from_euler(_att_sp.roll_body, _att_sp.pitch_body, _att_sp.yaw_body);
			memcpy(&_att_sp.R_body[0], R_sp.data, sizeof(_att_sp.R_body));
////////////////////为所有非姿态飞行模式      复位加速度设定值/////////////////
			/* reset the acceleration set point for all non-attitude flight modes */
			if (!(_control_mode.flag_control_offboard_enabled &&
					!(_control_mode.flag_control_position_enabled ||
					  _control_mode.flag_control_velocity_enabled))) {

				_thrust_sp_prev = R_sp * math::Vector<3>(0, 0, -_att_sp.thrust);
			}
/////////////////////将四元数设定值复制到姿态设定值的主题中/////////////////////////
			/* copy quaternion setpoint to attitude setpoint topic */
			math::Quaternion q_sp;
			q_sp.from_dcm(R_sp);
			memcpy(&_att_sp.q_d[0], &q_sp.data[0], sizeof(_att_sp.q_d));
			_att_sp.timestamp = hrt_absolute_time();

		} else {
			reset_yaw_sp = true;
		}

		/* update previous velocity for velocity controller D part */
		_vel_prev = _vel;
/////////////////////////发布设定值//////////////////////////////
		/* publish attitude setpoint
		 * Do not publish if offboard is enabled but position/velocity control is disabled,
		 * in this case the attitude setpoint is published by the mavlink app. Also do not publish
		 * if the vehicle is a VTOL and it's just doing a transition (the VTOL attitude control module will generate
		 * attitude setpoints for the transition).
		 */
		if (!(_control_mode.flag_control_offboard_enabled &&
				!(_control_mode.flag_control_position_enabled ||
				  _control_mode.flag_control_velocity_enabled))) {

			if (_att_sp_pub != nullptr) {
				orb_publish(_attitude_setpoint_id, _att_sp_pub, &_att_sp);

			} else if (_attitude_setpoint_id) {
				_att_sp_pub = orb_advertise(_attitude_setpoint_id, &_att_sp);
			}
		}
/////////////手动油门控制后,将高度控制器积分(悬停油门)复位到手动油门///////////////
		/* reset altitude controller integral (hovering throttle) to manual throttle after manual throttle control */
		reset_int_z_manual = _control_mode.flag_armed && _control_mode.flag_control_manual_enabled
				     && !_control_mode.flag_control_climb_rate_enabled;
	}

	mavlink_log_info(&_mavlink_log_pub, "[mpc] stopped");

	_control_task = -1;
}
这些设定值的确定是不是就是控制策略的体现?
void
MulticopterPositionControl::poll_subscriptions()
{
	bool updated;
/**********************机体状态跟新**************************/
	orb_check(_vehicle_status_sub, &updated);

	if (updated) {
		orb_copy(ORB_ID(vehicle_status), _vehicle_status_sub, &_vehicle_status);

		/* set correct uORB ID, depending on if vehicle is VTOL or not */
		if (!_attitude_setpoint_id) {
			if (_vehicle_status.is_vtol) {
				_attitude_setpoint_id = ORB_ID(mc_virtual_attitude_setpoint);

			} else {
				_attitude_setpoint_id = ORB_ID(vehicle_attitude_setpoint);
			}
		}
	}
/**********************控制状态跟新**************************/
	orb_check(_ctrl_state_sub, &updated);

	if (updated) {
		orb_copy(ORB_ID(control_state), _ctrl_state_sub, &_ctrl_state);

		/* get current rotation matrix and euler angles from control state quaternions */
		math::Quaternion q_att(_ctrl_state.q[0], _ctrl_state.q[1], _ctrl_state.q[2], _ctrl_state.q[3]);
		_R = q_att.to_dcm();
		math::Vector<3> euler_angles;
		euler_angles = _R.to_euler();
		_yaw = euler_angles(2);
	}
/**********************setpoint状态跟新**************************/
	orb_check(_att_sp_sub, &updated);

	if (updated) {
		orb_copy(ORB_ID(vehicle_attitude_setpoint), _att_sp_sub, &_att_sp);
	}

	orb_check(_control_mode_sub, &updated);

	if (updated) {
		orb_copy(ORB_ID(vehicle_control_mode), _control_mode_sub, &_control_mode);
	}
/**********************手动控制状态跟新**************************/
	orb_check(_manual_sub, &updated); 

	if (updated) {
		orb_copy(ORB_ID(manual_control_setpoint), _manual_sub, &_manual);
	}
/**********************解锁状态跟新**************************/
	orb_check(_arming_sub, &updated);

	if (updated) {
		orb_copy(ORB_ID(actuator_armed), _arming_sub, &_arming);
	}
/**********************位置跟新**************************/
	orb_check(_local_pos_sub, &updated);

	if (updated) {
		orb_copy(ORB_ID(vehicle_local_position), _local_pos_sub, &_local_pos);
	}
}
void
MulticopterPositionControl::update_ref()
{
	if (_local_pos.ref_timestamp != _ref_timestamp) {
		double lat_sp, lon_sp;//维度设定、经度设定
		float alt_sp = 0.0f;//姿态设定

		if (_ref_timestamp != 0) { 
		/*************在整体框架下计算当前位置设定值***************/
			/* calculate current position setpoint in global frame */
			map_projection_reproject(&_ref_pos, _pos_sp(0), _pos_sp(1), &lat_sp, &lon_sp);
			alt_sp = _ref_alt - _pos_sp(2);
		}
		/*************跟新投影参考*********************/
		/* update local projection reference */
		map_projection_init(&_ref_pos, _local_pos.ref_lat, _local_pos.ref_lon);
		_ref_alt = _local_pos.ref_alt;

		if (_ref_timestamp != 0) {
		/*********************设置新的位置设定值*******************/
			/* reproject position setpoint to new reference */
			map_projection_project(&_ref_pos, lat_sp, lon_sp, &_pos_sp.data[0], &_pos_sp.data[1]);
			_pos_sp(2) = -(alt_sp - _ref_alt);
		}

		_ref_timestamp = _local_pos.ref_timestamp;
	}
}
void
MulticopterPositionControl::control_manual(float dt)
{
	math::Vector<3> req_vel_sp; // X,Y in local frame and Z in global (D), in [-1,1] normalized range
	req_vel_sp.zero();

	if (_control_mode.flag_control_altitude_enabled) {
		/* set vertical velocity setpoint with throttle stick */用油门棒设置垂直速度
		req_vel_sp(2) = -scale_control(_manual.z - 0.5f, 0.5f, _params.alt_ctl_dz, _params.alt_ctl_dy); // D
	}

	if (_control_mode.flag_control_position_enabled) {
		/* set horizontal velocity setpoint with roll/pitch stick */用roll/pitch棒设置水平速度
		req_vel_sp(0) = _manual.x;
		req_vel_sp(1) = _manual.y;
	}

	if (_control_mode.flag_control_altitude_enabled) {
		/* reset alt setpoint to current altitude if needed */复位姿态设定值
		reset_alt_sp();
	}

	if (_control_mode.flag_control_position_enabled) {
		/* reset position setpoint to current position if needed */复位位置设定值
		reset_pos_sp();
	}

	/* limit velocity setpoint */限制速度设定值
	float req_vel_sp_norm = req_vel_sp.length();

	if (req_vel_sp_norm > 1.0f) {
		req_vel_sp /= req_vel_sp_norm;//相当于是向量除以它的模值
	}
///////////_req_vel_sp的范围是0到1,围绕yaw旋转
	/* _req_vel_sp scaled to 0..1, scale it to max speed and rotate around yaw */
	math::Matrix<3, 3> R_yaw_sp;
	R_yaw_sp.from_euler(0.0f, 0.0f, _att_sp.yaw_body);
	math::Vector<3> req_vel_sp_scaled = R_yaw_sp * req_vel_sp.emult(
			_params.vel_cruise); // in NED and scaled to actual velocity在NED旋转方式下缩放到实际速度
/////////用户辅助模式:用户控制速度,但是,如果速度足够小,相应的轴的位置就会保持////////////////
	/*
	 * assisted velocity mode: user controls velocity, but if	velocity is small enough, position
	 * hold is activated for the corresponding axis
	 */

	/* horizontal axes */水平轴
	if (_control_mode.flag_control_position_enabled) {
		/* check for pos. hold */检测位置的保持状态
		if (fabsf(req_vel_sp(0)) < _params.hold_xy_dz && fabsf(req_vel_sp(1)) < _params.hold_xy_dz) {
			if (!_pos_hold_engaged) {
				if (_params.hold_max_xy < FLT_EPSILON || (fabsf(_vel(0)) < _params.hold_max_xy
						&& fabsf(_vel(1)) < _params.hold_max_xy)) {
					_pos_hold_engaged = true;

				} else {
					_pos_hold_engaged = false;
				}
			}

		} else {
			_pos_hold_engaged = false;
		}

		/* set requested velocity setpoint */设置所需的速度设定值
		if (!_pos_hold_engaged) {
			_pos_sp(0) = _pos(0);
			_pos_sp(1) = _pos(1);
			//////////速度设定值会被利用,位置设定值不会被用/////////////
			_run_pos_control = false; /* request velocity setpoint to be used, instead of position setpoint */
			_vel_sp(0) = req_vel_sp_scaled(0);
			_vel_sp(1) = req_vel_sp_scaled(1);
		}
	}

	/* vertical axis */垂直轴
	if (_control_mode.flag_control_altitude_enabled) {
		/* check for pos. hold */
		if (fabsf(req_vel_sp(2)) < FLT_EPSILON) {
			if (!_alt_hold_engaged) {
				if (_params.hold_max_z < FLT_EPSILON || fabsf(_vel(2)) < _params.hold_max_z) {
					_alt_hold_engaged = true;

				} else {
					_alt_hold_engaged = false;
				}
			}

		} else {
			_alt_hold_engaged = false;
		}

		/* set requested velocity setpoint */
		if (!_alt_hold_engaged) {
			_run_alt_control = false; /* request velocity setpoint to be used, instead of altitude setpoint */
			_vel_sp(2) = req_vel_sp_scaled(2);
			_pos_sp(2) = _pos(2);
		}
	}
}
#ifdef __cplusplus
struct __EXPORT position_setpoint_triplet_s {
#else
struct position_setpoint_triplet_s {
#endif
	struct position_setpoint_s previous;
	struct position_setpoint_s current;
	struct position_setpoint_s next;
	uint8_t nav_state;
#ifdef __cplusplus
#endif
};

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

orb_publish(_attitude_setpoint_id, _att_sp_pub, &_att_sp);
orb_publish(ORB_ID(vehicle_global_velocity_setpoint), _global_vel_sp_pub, &_global_vel_sp);
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 

if (!_attitude_setpoint_id) {
	if (_vehicle_status.is_vtol) {
		_attitude_setpoint_id = ORB_ID(mc_virtual_attitude_setpoint);

	} else {
		_attitude_setpoint_id = ORB_ID(vehicle_attitude_setpoint);
	}
}
再看看
void
MulticopterPositionControl::task_main()
{

	/*
	 * do subscriptions
	 */
	_vehicle_status_sub = orb_subscribe(ORB_ID(vehicle_status));
	_ctrl_state_sub = orb_subscribe(ORB_ID(control_state));
	_att_sp_sub = orb_subscribe(ORB_ID(vehicle_attitude_setpoint));
	_control_mode_sub = orb_subscribe(ORB_ID(vehicle_control_mode));
	_params_sub = orb_subscribe(ORB_ID(parameter_update));
	_manual_sub = orb_subscribe(ORB_ID(manual_control_setpoint));
	_arming_sub = orb_subscribe(ORB_ID(actuator_armed));
	_local_pos_sub = orb_subscribe(ORB_ID(vehicle_local_position));
	_pos_sp_triplet_sub = orb_subscribe(ORB_ID(position_setpoint_triplet));
	_local_pos_sp_sub = orb_subscribe(ORB_ID(vehicle_local_position_setpoint));
	_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);
if (!_attitude_setpoint_id) {
	if (_vehicle_status.is_vtol) {
		_attitude_setpoint_id = ORB_ID(mc_virtual_attitude_setpoint);

	} else {
		_attitude_setpoint_id = ORB_ID(vehicle_attitude_setpoint);
	}
}
//ardupilot/modules/PX4Firmware/src/modules/mc_att_control/mc_att_control_main.cpp
_v_att_sp_sub = orb_subscribe(ORB_ID(vehicle_attitude_setpoint));
void
MulticopterAttitudeControl::vehicle_attitude_setpoint_poll()
{
	/* check if there is a new setpoint */
	bool updated;
	orb_check(_v_att_sp_sub, &updated);

	if (updated) {
		orb_copy(ORB_ID(vehicle_attitude_setpoint), _v_att_sp_sub, &_v_att_sp);
	}
}
突然发现//ardupilot/modules/PX4Firmware/src/modules/vtol_att_control//vtol_att_control_main.cpp中也有orb_copy(ORB_ID(vehicle_attitude_setpoint)
void
VtolAttitudeControl::vehicle_attitude_setpoint_poll()
{
	/* check if there is a new setpoint */
	bool updated;
	orb_check(_v_att_sp_sub, &updated);

	if (updated) {
		orb_copy(ORB_ID(vehicle_attitude_setpoint), _v_att_sp_sub, &_v_att_sp);
	}
}
void
VtolAttitudeControl::mc_virtual_att_sp_poll()
{
	bool updated;

	orb_check(_mc_virtual_att_sp_sub, &updated);

	if (updated) {
		orb_copy(ORB_ID(mc_virtual_attitude_setpoint), _mc_virtual_att_sp_sub , &_mc_virtual_att_sp);
	}

}
void VtolAttitudeControl::publish_att_sp()
{
	if (_v_att_sp_pub != nullptr) {
		/* publish the attitude setpoint */
		orb_publish(ORB_ID(vehicle_attitude_setpoint), _v_att_sp_pub, &_v_att_sp);

	} else {
		/* advertise and publish */
		_v_att_sp_pub = orb_advertise(ORB_ID(vehicle_attitude_setpoint), &_v_att_sp);
	}
}
好奇葩啊,//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[]);
int mc_att_control_main(int argc, char *argv[])
{
	if (argc < 2) {
		warnx("usage: mc_att_control {start|stop|status}");
		return 1;
	}

	if (!strcmp(argv[1], "start")) {

		if (mc_att_control::g_control != nullptr) {
			warnx("already running");
			return 1;
		}

		mc_att_control::g_control = new MulticopterAttitudeControl;

		if (mc_att_control::g_control == nullptr) {
			warnx("alloc failed");
			return 1;
		}

		if (OK != mc_att_control::g_control->start()) {
			delete mc_att_control::g_control;
			mc_att_control::g_control = nullptr;
			warnx("start failed");
			return 1;
		}

		return 0;
	}

	if (!strcmp(argv[1], "stop")) {
		if (mc_att_control::g_control == nullptr) {
			warnx("not running");
			return 1;
		}

		delete mc_att_control::g_control;
		mc_att_control::g_control = nullptr;
		return 0;
	}

	if (!strcmp(argv[1], "status")) {
		if (mc_att_control::g_control) {
			warnx("running");
			return 0;

		} else {
			warnx("not running");
			return 1;
		}
	}

	warnx("unrecognized command");
	return 1;
}
这里MulticopterAttitudeControl*g_control;*g_control是最大的一个类的实例
mc_att_control::g_control = new MulticopterAttitudeControl;相当于是分配空间
里面包含
/* fetch initial parameter values */
	parameters_update();
里面好多参数,不知道是不是上位机配置的参数,就是说是不是把上位机的各种关于飞行的参数存到了sd卡中,在通过这个函数跟新到程序中用于飞行器的控制??
接着就进了start
int
MulticopterAttitudeControl::start()
{
	ASSERT(_control_task == -1);

	/* start the task */
	_control_task = px4_task_spawn_cmd("mc_att_control",
					   SCHED_DEFAULT,
					   SCHED_PRIORITY_MAX - 5,
					   1500,
					   (px4_main_t)&MulticopterAttitudeControl::task_main_trampoline,
					   nullptr);

	if (_control_task < 0) {
		warn("task start failed");
		return -errno;
	}

	return OK;
}
void
MulticopterAttitudeControl::task_main_trampoline(int argc, char *argv[])
{
	mc_att_control::g_control->task_main();
}
void
MulticopterAttitudeControl::task_main()
{
/**************************订阅各种信息**************************/
	/*
	 * do subscriptions
	 */
	_v_att_sp_sub = orb_subscribe(ORB_ID(vehicle_attitude_setpoint));
	_v_rates_sp_sub = orb_subscribe(ORB_ID(vehicle_rates_setpoint));
	_ctrl_state_sub = orb_subscribe(ORB_ID(control_state));
	_v_control_mode_sub = orb_subscribe(ORB_ID(vehicle_control_mode));
	_params_sub = orb_subscribe(ORB_ID(parameter_update));
	_manual_control_sp_sub = orb_subscribe(ORB_ID(manual_control_setpoint));
	_armed_sub = orb_subscribe(ORB_ID(actuator_armed));
	_vehicle_status_sub = orb_subscribe(ORB_ID(vehicle_status));
	_motor_limits_sub = orb_subscribe(ORB_ID(multirotor_motor_limits));
/**************************初始化各种参数**************************/
	/* initialize parameters cache */
	parameters_update();
/**************************启动机体姿态原函数**************************/
	/* wakeup source: vehicle attitude */
	px4_pollfd_struct_t fds[1];

	fds[0].fd = _ctrl_state_sub;
	fds[0].events = POLLIN;
/**************************大循环**************************/
	while (!_task_should_exit) {
/////////////////////////////等待100ms   为了获取数据/////////////////////////
		/* wait for up to 100ms for data */
		int pret = px4_poll(&fds[0], (sizeof(fds) / sizeof(fds[0])), 100);

		/* timed out - periodic check for _task_should_exit */
		if (pret == 0) {
			continue;
		}

		/* this is undesirable but not much we can do - might want to flag unhappy status */
		if (pret < 0) {
			warn("mc att ctrl: poll error %d, %d", pret, errno);
			/* sleep a bit before next try */
			usleep(100000);
			continue;
		}

		perf_begin(_loop_perf);
///////////////////////运行姿态变化控制器//////////////////////////
		/* run controller on attitude changes */
		if (fds[0].revents & POLLIN) {
			static uint64_t last_run = 0;
			float dt = (hrt_absolute_time() - last_run) / 1000000.0f;
			last_run = hrt_absolute_time();
/////////////////////防止dt太小或太大   2ms<dt<20ms////////////////////
			/* guard against too small (< 2ms) and too large (> 20ms) dt's */
			if (dt < 0.002f) {
				dt = 0.002f;

			} else if (dt > 0.02f) {
				dt = 0.02f;
			}

			/* copy attitude and control state topics */
			orb_copy(ORB_ID(control_state), _ctrl_state_sub, &_ctrl_state);

			/* check for updates in other topics */
			parameter_update_poll();
			vehicle_control_mode_poll();
			arming_status_poll();
			vehicle_manual_poll();
			vehicle_status_poll();
			vehicle_motor_limits_poll();
////////////////若xy轴的设定值大于阈值,那么不运行姿态控制/////////////////
			/* Check if we are in rattitude mode and the pilot is above the threshold on pitch
			 * or roll (yaw can rotate 360 in normal att control).  If both are true don't
			 * even bother running the attitude controllers */
			if (_vehicle_status.main_state == vehicle_status_s::MAIN_STATE_RATTITUDE) {
				if (fabsf(_manual_control_sp.y) > _params.rattitude_thres ||
				    fabsf(_manual_control_sp.x) > _params.rattitude_thres) {
					_v_control_mode.flag_control_attitude_enabled = false;
				}
			}
/////////////////////////姿态控制使能////////////////////////////
			if (_v_control_mode.flag_control_attitude_enabled) {

				if (_ts_opt_recovery == nullptr) {
					// the  tailsitter recovery instance has not been created, thus, the vehicle
					// is not a tailsitter, do normal attitude control
					control_attitude(dt);

				} else {
					vehicle_attitude_setpoint_poll();
					_thrust_sp = _v_att_sp.thrust;
					math::Quaternion q(_ctrl_state.q[0], _ctrl_state.q[1], _ctrl_state.q[2], _ctrl_state.q[3]);
					math::Quaternion q_sp(&_v_att_sp.q_d[0]);
					_ts_opt_recovery->setAttGains(_params.att_p, _params.yaw_ff);
					_ts_opt_recovery->calcOptimalRates(q, q_sp, _v_att_sp.yaw_sp_move_rate, _rates_sp);

					/* limit rates */
					for (int i = 0; i < 3; i++) {
						_rates_sp(i) = math::constrain(_rates_sp(i), -_params.mc_rate_max(i), _params.mc_rate_max(i));
					}
				}

				/* publish attitude rates setpoint */
				_v_rates_sp.roll = _rates_sp(0);
				_v_rates_sp.pitch = _rates_sp(1);
				_v_rates_sp.yaw = _rates_sp(2);
				_v_rates_sp.thrust = _thrust_sp;
				_v_rates_sp.timestamp = hrt_absolute_time();

				if (_v_rates_sp_pub != nullptr) {
					orb_publish(_rates_sp_id, _v_rates_sp_pub, &_v_rates_sp);

				} else if (_rates_sp_id) {
					_v_rates_sp_pub = orb_advertise(_rates_sp_id, &_v_rates_sp);
				}

				//}

			} else {
/////////////////////////////////////手动控制使能//////////////////////////////
				/* attitude controller disabled, poll rates setpoint topic */
				if (_v_control_mode.flag_control_manual_enabled) {
				///////////////////////特技模式/////////////////////////
					/* manual rates control - ACRO mode */
					_rates_sp = math::Vector<3>(_manual_control_sp.y, -_manual_control_sp.x,
								    _manual_control_sp.r).emult(_params.acro_rate_max);
					_thrust_sp = math::min(_manual_control_sp.z, MANUAL_THROTTLE_MAX_MULTICOPTER);
				/////////////////////////发布角速度设定值////////////////////
					/* publish attitude rates setpoint */
					_v_rates_sp.roll = _rates_sp(0);
					_v_rates_sp.pitch = _rates_sp(1);
					_v_rates_sp.yaw = _rates_sp(2);
					_v_rates_sp.thrust = _thrust_sp;
					_v_rates_sp.timestamp = hrt_absolute_time();

					if (_v_rates_sp_pub != nullptr) {
						orb_publish(_rates_sp_id, _v_rates_sp_pub, &_v_rates_sp);

					} else if (_rates_sp_id) {
						_v_rates_sp_pub = orb_advertise(_rates_sp_id, &_v_rates_sp);
					}

				} 
				//////////////非特技模式//////////////////
				else {
				//////////////速度设定值来自于vehicle_rates_setpoint//////////////////
				//////////////vehicle_rates_setpoint来自于哪里??///////////////////
					/* attitude controller disabled, poll rates setpoint topic */
					vehicle_rates_setpoint_poll();
					_rates_sp(0) = _v_rates_sp.roll;
					_rates_sp(1) = _v_rates_sp.pitch;
					_rates_sp(2) = _v_rates_sp.yaw;
					_thrust_sp = _v_rates_sp.thrust;
				}
			}
///////////////////////////////速度控制/////////////////////////////////////
			if (_v_control_mode.flag_control_rates_enabled) {
			//////////////////角速度控制////////////////////
				control_attitude_rates(dt);
			//////////////////发布执行器控制///////////////
			//////////////////这个就是pwm吗??////////////
				/* publish actuator controls */
				_actuators.control[0] = (PX4_ISFINITE(_att_control(0))) ? _att_control(0) : 0.0f;
				_actuators.control[1] = (PX4_ISFINITE(_att_control(1))) ? _att_control(1) : 0.0f;
				_actuators.control[2] = (PX4_ISFINITE(_att_control(2))) ? _att_control(2) : 0.0f;
				_actuators.control[3] = (PX4_ISFINITE(_thrust_sp)) ? _thrust_sp : 0.0f;
				_actuators.timestamp = hrt_absolute_time();
				_actuators.timestamp_sample = _ctrl_state.timestamp;

				_controller_status.roll_rate_integ = _rates_int(0);
				_controller_status.pitch_rate_integ = _rates_int(1);
				_controller_status.yaw_rate_integ = _rates_int(2);
				_controller_status.timestamp = hrt_absolute_time();

				if (!_actuators_0_circuit_breaker_enabled) {
					if (_actuators_0_pub != nullptr) {

						orb_publish(_actuators_id, _actuators_0_pub, &_actuators);
						perf_end(_controller_latency_perf);

					} else if (_actuators_id) {
						_actuators_0_pub = orb_advertise(_actuators_id, &_actuators);
					}

				}

				/* publish controller status */
				if (_controller_status_pub != nullptr) {
					orb_publish(ORB_ID(mc_att_ctrl_status), _controller_status_pub, &_controller_status);

				} else {
					_controller_status_pub = orb_advertise(ORB_ID(mc_att_ctrl_status), &_controller_status);
				}
			}
		}

		perf_end(_loop_perf);
	}

	_control_task = -1;
	return;
}
看看这3段控制程序的输入输出,大概能猜想他们的运行逻辑并不是并列的,很有可能_v_control_mode.flag_control_rates_enabled跟在_v_control_mode.flag_control_attitude_enabled之后,这样就形成了常用的外环角度(也就是这里的姿态)内环角速度(姿态速度)的控制结构,这个还需要细看里面的输入输出和标志位如何赋值控制运行逻辑。
/**
 * Attitude controller.
 * Input: 'vehicle_attitude_setpoint' topics (depending on mode)
 * Output: '_rates_sp' vector, '_thrust_sp'
 */
/*************************注意看注释!!********************/
/***********输入是'vehicle_attitude_setpoint'主题***********/
/************输出是角速度设定值向量和油门设定值*************/
void
MulticopterAttitudeControl::control_attitude(float dt)
{
	vehicle_attitude_setpoint_poll();//获取'vehicle_attitude_setpoint'主题

	_thrust_sp = _v_att_sp.thrust;
//////////构建姿态设定值的旋转矩阵就是vehicle_attitude_setpoint_poll()获取的//////////
	/* construct attitude setpoint rotation matrix */
	math::Matrix<3, 3> R_sp;
	R_sp.set(_v_att_sp.R_body);//这里做的事只是把订阅_v_att_sp复制到一个新的地方
/****		void set(const float *d) {
 ****		memcpy(data, d, sizeof(data));
 ****		}
 ****/
//////////////////////定义一个控制状态的四元数和相应的方向余弦矩阵(dcm)/////////////////
	/* get current rotation matrix from control state quaternions */
	math::Quaternion q_att(_ctrl_state.q[0], _ctrl_state.q[1], _ctrl_state.q[2], _ctrl_state.q[3]);
	math::Matrix<3, 3> R = q_att.to_dcm();
/////////////////输入就准备好了,就是姿态设定值,并转变成能用的形式四元数和dcm///////////////
	/* all input data is ready, run controller itself */

	/* try to move thrust vector shortest way, because yaw response is slower than roll/pitch */
	math::Vector<3> R_z(R(0, 2), R(1, 2), R(2, 2));//控制状态的z轴
	math::Vector<3> R_sp_z(R_sp(0, 2), R_sp(1, 2), R_sp(2, 2));//设定姿态的z轴

	/* axis and sin(angle) of desired rotation */轴与角的理想旋转
	math::Vector<3> e_R = R.transposed() * (R_z % R_sp_z);//transposed()换位函数,%是求叉积的意思
/***		ardupilot/modules/PX4Firmware/src/lib/matrix/matrix/Matrix.hpp
 ***		Matrix<Type, N, M> transpose() const
 ***		{
 ***			Matrix<Type, N, M> res;
 ***			const Matrix<Type, M, N> &self = *this;
 ***
 ***			for (size_t i = 0; i < M; i++) {
 ***				for (size_t j = 0; j < N; j++) {
 ***					res(j, i) = self(i, j);
 ***				}
 ***			}
 ***
 ***			return res;
 ***		}
 ***/
	/* calculate angle error */计算角的误差
	float e_R_z_sin = e_R.length();
	float e_R_z_cos = R_z * R_sp_z;
//计算姿态角度误差(姿态误差),一个数学知识背景:由公式a×b=︱a︱︱b︱sinθ,a•b=︱a︱︱b︱cosθ
//这里的R_z和R_sp_z都是单位向量,模值为1,因此误差向量e_R(a×b叉积就是误差)的模就是sinθ,点积就是cosθ。
/***	ardupilot/modules/PX4Firmware/src/lib/mathlib/math/Vector.hpp
 ***	float length() const {
 ***		float res = 0.0f;
 ***
 ***		for (unsigned int i = 0; i < N; i++)
 ***			res += data[i] * data[i];
 ***
 ***		return sqrtf(res);
 ***	}
 ***/
	/* calculate weight for yaw control */计算yaw控制的权重
	float yaw_w = R_sp(2, 2) * R_sp(2, 2);//姿态设定值矩阵的第三行第三列元素的平方

	/* calculate rotation matrix after roll/pitch only rotation */
	math::Matrix<3, 3> R_rp;

	if (e_R_z_sin > 0.0f) {
		/* get axis-angle representation */
		float e_R_z_angle = atan2f(e_R_z_sin, e_R_z_cos);//得到z轴的误差角度
		math::Vector<3> e_R_z_axis = e_R / e_R_z_sin;
/***	  R.transposed() * (R_z % R_sp_z)
 ***	-----------------------------------
 ***	||R.transposed() * (R_z % R_sp_z)||
 ***/
/***********这些式子的数据具体怎么算的可以看得出来,具体含义应该与理论知识有关吧**********/
		e_R = e_R_z_axis * e_R_z_angle;

		/* cross product matrix for e_R_axis */
		math::Matrix<3, 3> e_R_cp;
		e_R_cp.zero();
		e_R_cp(0, 1) = -e_R_z_axis(2);
		e_R_cp(0, 2) = e_R_z_axis(1);
		e_R_cp(1, 0) = e_R_z_axis(2);
		e_R_cp(1, 2) = -e_R_z_axis(0);
		e_R_cp(2, 0) = -e_R_z_axis(1);
		e_R_cp(2, 1) = e_R_z_axis(0);

		/* rotation matrix for roll/pitch only rotation */
		R_rp = R * (_I + e_R_cp * e_R_z_sin + e_R_cp * e_R_cp * (1.0f - e_R_z_cos));

	} else {
		/* zero roll/pitch rotation */
		R_rp = R;
	}

	/* R_rp and R_sp has the same Z axis, calculate yaw error */
	math::Vector<3> R_sp_x(R_sp(0, 0), R_sp(1, 0), R_sp(2, 0));
	math::Vector<3> R_rp_x(R_rp(0, 0), R_rp(1, 0), R_rp(2, 0));
	e_R(2) = atan2f((R_rp_x % R_sp_x) * R_sp_z, R_rp_x * R_sp_x) * yaw_w;

	if (e_R_z_cos < 0.0f) {
		/* for large thrust vector rotations use another rotation method:
		 * calculate angle and axis for R -> R_sp rotation directly */
		math::Quaternion q;
		q.from_dcm(R.transposed() * R_sp);
		math::Vector<3> e_R_d = q.imag();
		e_R_d.normalize();
		e_R_d *= 2.0f * atan2f(e_R_d.length(), q(0));

		/* use fusion of Z axis based rotation and direct rotation */
		float direct_w = e_R_z_cos * e_R_z_cos * yaw_w;//计算权重
		e_R = e_R * (1.0f - direct_w) + e_R_d * direct_w;
	}
///////////////////计算角速度设定值  也是矩阵表达的1*3矩阵///////////////////////
	/* calculate angular rates setpoint */
	_rates_sp = _params.att_p.emult(e_R);

	/* limit rates */限制角速度大小
	for (int i = 0; i < 3; i++) {
		if (_v_control_mode.flag_control_velocity_enabled && !_v_control_mode.flag_control_manual_enabled) {
			_rates_sp(i) = math::constrain(_rates_sp(i), -_params.auto_rate_max(i), _params.auto_rate_max(i));
		} else {
			_rates_sp(i) = math::constrain(_rates_sp(i), -_params.mc_rate_max(i), _params.mc_rate_max(i));
		}
	}
//////////////////风向标模式,抑制yaw角速度(风向标模式是定航向的意思吗??)///////////////
//////////////////yaw控制失能、速度模式使能、非手动模式/////////////////////////
	/* weather-vane mode, dampen yaw rate */
	if (_v_att_sp.disable_mc_yaw_control == true && _v_control_mode.flag_control_velocity_enabled && !_v_control_mode.flag_control_manual_enabled) {
		float wv_yaw_rate_max = _params.auto_rate_max(2) * _params.vtol_wv_yaw_rate_scale;
		_rates_sp(2) = math::constrain(_rates_sp(2), -wv_yaw_rate_max, wv_yaw_rate_max);
		// prevent integrator winding up in weathervane mode
		_rates_int(2) = 0.0f;
	}
///////////////////反馈角速度设定值(2)  矩阵中第三个元素/////////////////
	/* feed forward yaw setpoint rate */
	_rates_sp(2) += _v_att_sp.yaw_sp_move_rate * yaw_w * _params.yaw_ff;
///////////////////风向标模式,减小yaw角速度///////////////////
	/* weather-vane mode, scale down yaw rate */
	if (_v_att_sp.disable_mc_yaw_control == true && _v_control_mode.flag_control_velocity_enabled && !_v_control_mode.flag_control_manual_enabled) {
		float wv_yaw_rate_max = _params.auto_rate_max(2) * _params.vtol_wv_yaw_rate_scale;
		_rates_sp(2) = math::constrain(_rates_sp(2), -wv_yaw_rate_max, wv_yaw_rate_max);
		// prevent integrator winding up in weathervane mode
		_rates_int(2) = 0.0f;
	}

}
/*
 * Attitude rates controller.
 * Input: '_rates_sp' vector, '_thrust_sp'
 * Output: '_att_control' vector
 */
/*************************注意看注释!!********************/
/************输入是角速度设定值向量、油门设定值*************/
/**************************姿态控制值***********************/
void
MulticopterAttitudeControl::control_attitude_rates(float dt)
{
	/* reset integral if disarmed */如果锁定,则复位角速度积分
	if (!_armed.armed || !_vehicle_status.is_rotary_wing) {
		_rates_int.zero();
	}
//////////////////当前机体角速度设定值//////////////////////
/////////由此可知_ctrl_state表示的传感器测得的机体状态////////////
//_ctrl_state来源应该是ardupilot/modules/PX4Firmware/src/modules/attitude_estimator_q/attitude_estimator_q.main.cpp//
	/* current body angular rates */
	math::Vector<3> rates;
	rates(0) = _ctrl_state.roll_rate;
	rates(1) = _ctrl_state.pitch_rate;
	rates(2) = _ctrl_state.yaw_rate;
////////////角速度误差=角速度设定值-机体角速度///////////////
	/* angular rates error */
	math::Vector<3> rates_err = _rates_sp - rates;
	/////////////////_att_control的三个量分别为pitch、roll、yaw方向的pwm值////////////////
/***	_att_control的三个量就是下面的Roll、pitch、yaw
 ***	Motor1=(int)(COMD.THR + Roll - pitch + yaw);
 ***	Motor2=(int)(COMD.THR + Roll + pitch - yaw);
 ***	Motor3=(int)(COMD.THR - Roll + pitch + yaw);
 ***	Motor4=(int)(COMD.THR - Roll - pitch - yaw);
 ***/
	_att_control = _params.rate_p.emult(rates_err) + _params.rate_d.emult(_rates_prev - rates) / dt + _rates_int +
		       _params.rate_ff.emult(_rates_sp - _rates_sp_prev) / dt;
	_rates_sp_prev = _rates_sp;
	_rates_prev = rates;
//注意这里用的算法pwm=P*error+D*(角速度_last-角速度_now)/dt+角速度积分+(角速度设定值_now-角速度设定值_last)/dt
/***    Matrix<Type, M, N> emult(const Matrix<Type, M, N> &other) const
 ***    {
 ***        Matrix<Type, M, N> res;
 ***        const Matrix<Type, M, N> &self = *this;
 ***
 ***        for (size_t i = 0; i < M; i++) {
 ***            for (size_t j = 0; j < N; j++) {
 ***                res(i , j) = self(i, j)*other(i, j);
 ***            }
 ***        }
 ***
 ***        return res;
 ***    }
 ***/
	/* update integral only if not saturated on low limit and if motor commands are not saturated */
	/////////////////////下面是积分的计算,注意积分饱和问题////////////////
	if (_thrust_sp > MIN_TAKEOFF_THRUST && !_motor_limits.lower_limit && !_motor_limits.upper_limit) {
		for (int i = 0; i < 3; i++) {
			if (fabsf(_att_control(i)) < _thrust_sp) {
				float rate_i = _rates_int(i) + _params.rate_i(i) * rates_err(i) * dt;

				if (PX4_ISFINITE(rate_i) && rate_i > -RATES_I_LIMIT && rate_i < RATES_I_LIMIT &&
				    _att_control(i) > -RATES_I_LIMIT && _att_control(i) < RATES_I_LIMIT) {
					_rates_int(i) = rate_i;
				}
			}
		}
	}
}
<pre name="code" class="cpp">	ardupilot/modules/PX4Firmware/src/modules/position_estimator_inav/position_estimator_inav_main.cpp
	int parameter_update_sub = orb_subscribe(ORB_ID(parameter_update));
	int actuator_sub = orb_subscribe(ORB_ID_VEHICLE_ATTITUDE_CONTROLS);
	int armed_sub = orb_subscribe(ORB_ID(actuator_armed));
	int sensor_combined_sub = orb_subscribe(ORB_ID(sensor_combined));
	int vehicle_attitude_sub = orb_subscribe(ORB_ID(vehicle_attitude));
	int optical_flow_sub = orb_subscribe(ORB_ID(optical_flow));
	int vehicle_gps_position_sub = orb_subscribe(ORB_ID(vehicle_gps_position));
	int vision_position_estimate_sub = orb_subscribe(ORB_ID(vision_position_estimate));
	int att_pos_mocap_sub = orb_subscribe(ORB_ID(att_pos_mocap));
	int distance_sensor_sub = orb_subscribe(ORB_ID(distance_sensor));
	int vehicle_rate_sp_sub = orb_subscribe(ORB_ID(vehicle_rates_setpoint));
	
	orb_copy(ORB_ID(sensor_combined), sensor_combined_sub, &sensor);
	orb_copy(ORB_ID(vehicle_attitude), vehicle_attitude_sub, &att);
	orb_copy(ORB_ID(parameter_update), parameter_update_sub, &update);
	orb_copy(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, actuator_sub, &actuator);//actuator_controls_0
	orb_copy(ORB_ID(actuator_armed), armed_sub, &armed);
	orb_copy(ORB_ID(distance_sensor), distance_sensor_sub, &lidar);
	orb_copy(ORB_ID(optical_flow), optical_flow_sub, &flow);
	orb_copy(ORB_ID(vehicle_rates_setpoint), vehicle_rate_sp_sub, &rates_setpoint);
	orb_copy(ORB_ID(vision_position_estimate), vision_position_estimate_sub, &vision);
	orb_copy(ORB_ID(att_pos_mocap), att_pos_mocap_sub, &mocap);
	orb_copy(ORB_ID(vehicle_gps_position), vehicle_gps_position_sub, &gps);
	
	orb_advert_t vehicle_local_position_pub = orb_advertise(ORB_ID(vehicle_local_position), &local_pos);
	orb_publish(ORB_ID(vehicle_local_position), vehicle_local_position_pub, &local_pos);
	vehicle_global_position_pub = orb_advertise(ORB_ID(vehicle_global_position), &global_pos);
	orb_publish(ORB_ID(vehicle_global_position), vehicle_global_position_pub, &global_pos);

	
	
	ardupilot/modules/PX4Firmware/src/modules/attitude_estimator_q/attitude_estimator_q_main.cpp
	_sensors_sub = orb_subscribe(ORB_ID(sensor_combined));
	_vision_sub = orb_subscribe(ORB_ID(vision_position_estimate));
	_mocap_sub = orb_subscribe(ORB_ID(att_pos_mocap));
	_airspeed_sub = orb_subscribe(ORB_ID(airspeed));
	_params_sub = orb_subscribe(ORB_ID(parameter_update));
	_global_pos_sub = orb_subscribe(ORB_ID(vehicle_global_position));

	orb_copy(ORB_ID(sensor_combined), _sensors_sub, &sensors)
	orb_copy(ORB_ID(vision_position_estimate), _vision_sub, &_vision);
	orb_copy(ORB_ID(att_pos_mocap), _mocap_sub, &_mocap);
	orb_copy(ORB_ID(airspeed), _airspeed_sub, &_airspeed);
	orb_copy(ORB_ID(vehicle_global_position), _global_pos_sub, &_gpos);
	orb_copy(ORB_ID(att_pos_mocap), _mocap_sub, &_mocap);
	
	orb_publish_auto(ORB_ID(vehicle_attitude), &_att_pub, &att, &att_inst, ORB_PRIO_HIGH);
	orb_publish_auto(ORB_ID(control_state), &_ctrl_state_pub, &ctrl_state, &ctrl_inst, ORB_PRIO_HIGH);
	orb_publish_auto(ORB_ID(estimator_status), &_est_state_pub, &est, &est_inst, ORB_PRIO_HIGH);
	
	
	
	ardupilot/modules/PX4Firmware/src/modules/mc_pos_control/mc_pos_control_main.cpp
	_vehicle_status_sub = orb_subscribe(ORB_ID(vehicle_status));
	_ctrl_state_sub = orb_subscribe(ORB_ID(control_state));
	_att_sp_sub = orb_subscribe(ORB_ID(vehicle_attitude_setpoint));
	_control_mode_sub = orb_subscribe(ORB_ID(vehicle_control_mode));
	_params_sub = orb_subscribe(ORB_ID(parameter_update));
	_manual_sub = orb_subscribe(ORB_ID(manual_control_setpoint));
	_arming_sub = orb_subscribe(ORB_ID(actuator_armed));
	_local_pos_sub = orb_subscribe(ORB_ID(vehicle_local_position));
	_pos_sp_triplet_sub = orb_subscribe(ORB_ID(position_setpoint_triplet));
	_local_pos_sp_sub = orb_subscribe(ORB_ID(vehicle_local_position_setpoint));
	_global_vel_sp_sub = orb_subscribe(ORB_ID(vehicle_global_velocity_setpoint));
	
	orb_copy(ORB_ID(vehicle_status), _vehicle_status_sub, &_vehicle_status);
	orb_copy(ORB_ID(control_state), _ctrl_state_sub, &_ctrl_state);
	orb_copy(ORB_ID(vehicle_attitude_setpoint), _att_sp_sub, &_att_sp);
	orb_copy(ORB_ID(vehicle_control_mode), _control_mode_sub, &_control_mode);
	orb_copy(ORB_ID(manual_control_setpoint), _manual_sub, &_manual);
	orb_copy(ORB_ID(actuator_armed), _arming_sub, &_arming);
	orb_copy(ORB_ID(vehicle_local_position), _local_pos_sub, &_local_pos);
	orb_copy(ORB_ID(position_setpoint_triplet), _pos_sp_triplet_sub, &_pos_sp_triplet);
	
	
	orb_publish(_attitude_setpoint_id, _att_sp_pub, &_att_sp);
	_att_sp_pub = orb_advertise(_attitude_setpoint_id, &_att_sp);
	if (!_attitude_setpoint_id) {
		if (_vehicle_status.is_vtol) {
			_attitude_setpoint_id = ORB_ID(mc_virtual_attitude_setpoint);

		} else {
			_attitude_setpoint_id = ORB_ID(vehicle_attitude_setpoint);
		}
	}
	orb_publish(ORB_ID(vehicle_global_velocity_setpoint), _global_vel_sp_pub, &_global_vel_sp);
	_global_vel_sp_pub = orb_advertise(ORB_ID(vehicle_global_velocity_setpoint), &_global_vel_sp);
	
	
	
	ardupilot/modules/PX4Firmware/src/modules/mc_att_control/mc_att_control_main.cpp
	_v_att_sp_sub = orb_subscribe(ORB_ID(vehicle_attitude_setpoint));
	_v_rates_sp_sub = orb_subscribe(ORB_ID(vehicle_rates_setpoint));
	_ctrl_state_sub = orb_subscribe(ORB_ID(control_state));
	_v_control_mode_sub = orb_subscribe(ORB_ID(vehicle_control_mode));
	_params_sub = orb_subscribe(ORB_ID(parameter_update));
	_manual_control_sp_sub = orb_subscribe(ORB_ID(manual_control_setpoint));
	_armed_sub = orb_subscribe(ORB_ID(actuator_armed));
	_vehicle_status_sub = orb_subscribe(ORB_ID(vehicle_status));
	_motor_limits_sub = orb_subscribe(ORB_ID(multirotor_motor_limits));
	
	orb_copy(ORB_ID(vehicle_attitude_setpoint), _v_att_sp_sub, &_v_att_sp);
	orb_copy(ORB_ID(control_state), _ctrl_state_sub, &_ctrl_state);
	orb_copy(ORB_ID(parameter_update), _params_sub, ¶m_update);
	orb_copy(ORB_ID(vehicle_control_mode), _v_control_mode_sub, &_v_control_mode);
	orb_copy(ORB_ID(actuator_armed), _armed_sub, &_armed);
	orb_copy(ORB_ID(manual_control_setpoint), _manual_control_sp_sub, &_manual_control_sp);
	orb_copy(ORB_ID(vehicle_status), _vehicle_status_sub, &_vehicle_status);
	if (!_rates_sp_id) {
			if (_vehicle_status.is_vtol) {
				_rates_sp_id = ORB_ID(mc_virtual_rates_setpoint);
				_actuators_id = ORB_ID(actuator_controls_virtual_mc);

			} else {
				_rates_sp_id = ORB_ID(vehicle_rates_setpoint);
				_actuators_id = ORB_ID(actuator_controls_0);
			}
		}
	}
	orb_copy(ORB_ID(multirotor_motor_limits), _motor_limits_sub, &_motor_limits);
	orb_copy(ORB_ID(vehicle_rates_setpoint), _v_rates_sp_sub, &_v_rates_sp);
	
	orb_publish(_rates_sp_id, _v_rates_sp_pub, &_v_rates_sp);
	_v_rates_sp_pub = orb_advertise(_rates_sp_id, &_v_rates_sp);
	orb_publish(_actuators_id, _actuators_0_pub, &_actuators);
	_actuators_0_pub = orb_advertise(_actuators_id, &_actuators);
	orb_publish(ORB_ID(mc_att_ctrl_status), _controller_status_pub, &_controller_status);
	_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 
的逻辑图
#ifdef __cplusplus
struct __EXPORT vehicle_local_position_s {
#else
struct vehicle_local_position_s {
#endif
	uint64_t timestamp;
	bool xy_valid;
	bool z_valid;
	bool v_xy_valid;
	bool v_z_valid;
	float x;
	float y;
	float z;
	float vx;
	float vy;
	float vz;
	float yaw;
	bool xy_global;
	bool z_global;
	uint64_t ref_timestamp;
	double ref_lat;
	double ref_lon;
	float ref_alt;
	float dist_bottom;
	float dist_bottom_rate;
	uint64_t surface_bottom_timestamp;
	bool dist_bottom_valid;
	float eph;
	float epv;
#ifdef __cplusplus
#endif
};
#ifdef __cplusplus
struct __EXPORT vehicle_global_position_s {
#else
struct vehicle_global_position_s {
#endif
	uint64_t timestamp;
	uint64_t time_utc_usec;
	double lat;
	double lon;
	float alt;
	float vel_n;
	float vel_e;
	float vel_d;
	float yaw;
	float eph;
	float epv;
	float terrain_alt;
	bool terrain_alt_valid;
	bool dead_reckoning;
	float pressure_alt;
#ifdef __cplusplus
#endif
};
#ifdef __cplusplus
struct __EXPORT vehicle_attitude_s {
#else
struct vehicle_attitude_s {
#endif
	uint64_t timestamp;
	float roll;
	float pitch;
	float yaw;
	float rollspeed;
	float pitchspeed;
	float yawspeed;
	float rollacc;
	float pitchacc;
	float yawacc;
	float rate_vibration;
	float accel_vibration;
	float mag_vibration;
	float rate_offsets[3];
	float R[9];
	float q[4];
	float g_comp[3];
	bool R_valid;
	bool q_valid;
#ifdef __cplusplus

#endif
};
#ifdef __cplusplus
struct __EXPORT control_state_s {
#else
struct control_state_s {
#endif
	uint64_t timestamp;
	float x_acc;
	float y_acc;
	float z_acc;
	float x_vel;
	float y_vel;
	float z_vel;
	float x_pos;
	float y_pos;
	float z_pos;
	float airspeed;
	bool airspeed_valid;
	float vel_variance[3];
	float pos_variance[3];
	float q[4];
	float roll_rate;
	float pitch_rate;
	float yaw_rate;
	float horz_acc_mag;
#ifdef __cplusplus

#endif
};
#ifdef __cplusplus
struct __EXPORT vehicle_attitude_setpoint_s {
#else
struct vehicle_attitude_setpoint_s {
#endif
	uint64_t timestamp;
	float roll_body;
	float pitch_body;
	float yaw_body;
	float yaw_sp_move_rate;
	float R_body[9];
	bool R_valid;
	float q_d[4];
	bool q_d_valid;
	float q_e[4];
	bool q_e_valid;
	float thrust;
	bool roll_reset_integral;
	bool pitch_reset_integral;
	bool yaw_reset_integral;
	bool fw_control_yaw;
	bool disable_mc_yaw_control;
	bool apply_flaps;
#ifdef __cplusplus

#endif
};

#ifdef __cplusplus
struct __EXPORT vehicle_rates_setpoint_s {
#else
struct vehicle_rates_setpoint_s {
#endif
	uint64_t timestamp;
	float roll;
	float pitch;
	float yaw;
	float thrust;
#ifdef __cplusplus

#endif
};
#ifdef __cplusplus
struct __EXPORT actuator_controls_0_s {
#else
struct actuator_controls_0_s {
#endif
	uint64_t timestamp;
	uint64_t timestamp_sample;
	float control[8];
#ifdef __cplusplus
	static const uint8_t NUM_ACTUATOR_CONTROLS = 8;
	static const uint8_t NUM_ACTUATOR_CONTROL_GROUPS = 4;

#endif
};
这是用到的结构体
上述结构还没有包括传感器的数据流,现在还没时间看到每个传感器。
恩,这篇就先到这吧,内容还挺多的。^_^
至于传感器系统、pwm系统,再另外写一篇blog。

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


版权声明:本文为博主原创文章,未经博主允许不得转载。

PX4位置控制offboard模式说明

offboard模式的开发及应用 一、px4固件的模式 px4固件支持10几种飞行模式,从代码结构上分析,分为基本模式、自定义模式和自定义子模式。 1.基本模式 基本模式又分为,位置控...
  • Gen_Ye
  • Gen_Ye
  • 2017年03月13日 17:03
  • 4093

px4位置估计-inav 算法解读(2017/10/26更新)

技术交流:zinghd@163.com,757012902@qq.com 转载标明出处,欢迎转载,因为都是自己的想法,不一定都是对的,欢迎讨论,哪有问题欢迎指点。2017/10/26更新 之前写的有...
  • ZingHd
  • ZingHd
  • 2016年08月27日 08:36
  • 3209

Pixhawk之姿态控制篇(1)_源码算法分析(超级有料)

一、开篇         姿态控制篇终于来了、来了、来了~~~         心情爽不爽?愉悦不愉悦?开心不开心?         喜欢的话就请我吃顿饭吧,哈哈。         其实这篇blog一...

1.Px4控制算法综述

说明 1)文档还处在编辑状态,错误在所难免,后续的文章我是看着记录,有一些错(我是打算以后再次深入的时候再修改) 2)交流联系wechat:2024323408 或email:2024323408...

px4飞控位置估计lpe移植到vs

本文主要内容px4飞控的位置估计有两种方式,一是inav,二是lpe,用到的传感器用加速度计,磁场传感器,gps,超声,激光,气压,视觉,动作捕捉。但是动作捕捉在目前固件的inav中还不支持。本文将进...

Pixhawk室内自动控制:参数设置

Pixhawk室内自动控制:参数设置本文针对使用光流传感器和超声波传感器(或激光雷达)的Pixhawk用户。ArduCopter目前(3.52)已经能够使用光流传感器提供的位置信息以及超声波或雷达提供...

pixhawk position_estimator_inav.cpp思路整理及数据流

写在前面: 这篇blog主要参考pixhawk的高度解算算法解读,并且加以扩展,扩展到其他传感器,其实里面处理好多只是记录了流程,至于里面实际物理意义并不是很清楚,也希望大牛能够指导一下。 概述: 整...
  • czyv587
  • czyv587
  • 2016年07月11日 21:37
  • 4738

PX4在ROS-Kinetic外部控制下的仿真(1)

官方文档是基于ROS-indigo作的介绍,ubuntu16.04下只能安装kinetic版本的ROS,亲身试坑后留下一些经验教训。 主要参考了PX4开发文档1,ROS官方文档2和相关博客步骤如下3...
  • yjy728
  • yjy728
  • 2017年07月16日 13:43
  • 527

pixhawk 姿态与控制部分的记录

此篇是把之前看到的资料总结整理一遍,大部分是搬砖,也加入了自己的一点思考,写的过程中晕了好多次,先大体记录下来,肯定有错误,日后再改正吧。 关于pixhawk程序执行流程,依然还没有实...
  • czyv587
  • czyv587
  • 2016年05月31日 19:29
  • 8584

Pixhawk之姿态解算篇(1)_入门篇(DCM Nomalize)

一、开篇         慢慢的、慢慢的、慢慢的就快要到飞控的主要部分了,飞控飞控就是所谓的飞行控制呗,一个是姿态解算一个是姿态控制,解算是解算,控制是控制,各自负责各自的任务,我也不懂...
内容举报
返回顶部
收藏助手
不良信息举报
您举报文章:pixhawk 光流--位置估计--姿态估计--位置控制--姿态控制
举报原因:
原因补充:

(最多只允许输入30个字)