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

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

前面的是从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.hclass 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 > 0FAR 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;#endiferrout: 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来

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

voidPX4FLOW::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    -工作队列的IDwork  -队列的工作结构worker -工作线程被执行，worker callback 会被调用arg    -参数会传递给worker callbackdelay  -延时多久再执行这个任务，若为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_tcbdispatchint 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，这个函数会返回一个指针指向相应的TCBFAR 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  */#endifstruct 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[]);

intpx4flow_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. */intstart(){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_ESCPX4_I2C_BUS_ESC,#endif#ifdef PX4_I2C_BUS_ONBOARDPX4_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. */voidtest(){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 > 0static 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_tPX4FLOW::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函数中)

boolRingBuffer::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函数中)

intPX4FLOW::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_REG0x16///< 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就是我们要找的函数

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

intPX4FLOW::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 radiansreport.pixel_flow_y_integral = static_cast<float>(f_integral.pixel_flow_y_integral) / 10000.0f;//convert to radiansreport.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 metersreport.quality = f_integral.qual; //0:bad ; 255 max qualityreport.gyro_x_rate_integral = static_cast<float>(f_integral.gyro_x_rate_integral) / 10000.0f; //convert to radiansreport.gyro_y_rate_integral = static_cast<float>(f_integral.gyro_y_rate_integral) / 10000.0f; //convert to radiansreport.gyro_z_rate_integral = static_cast<float>(f_integral.gyro_z_rate_integral) / 10000.0f; //convert to radiansreport.integration_timespan = f_integral.integration_timespan; //microsecondsreport.time_since_last_sonar_update = f_integral.sonar_timestamp;//microsecondsreport.gyro_temperature = f_integral.gyro_temperature;//Temperature * 100 in centi-degrees Celsiusreport.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, velfloat y_est[2] = { 0.0f, 0.0f };// pos, velfloat z_est[2] = { 0.0f, 0.0f };// pos, velfloat est_buf[EST_BUF_SIZE][3][2];// estimated position bufferfloat R_buf[EST_BUF_SIZE][3][3];// rotation matrix bufferfloat R_gps[3][3];// rotation matrix for GPS correction momentmemset(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 calculationstatic const float max_eph_epv = 20.0f;// max EPH/EPV acceptable for estimationfloat 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 adjustedhrt_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 fixstruct 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 Dfloat acc_bias[] = { 0.0f, 0.0f, 0.0f };// body framefloat corr_baro = 0.0f;// Dfloat 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 estimationfloat 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 Efloat 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 validbool lidar_valid = false;// lidar is validbool flow_valid = false;// flow is validbool flow_accurate = false;// flow should be accurate (this flag not updated if flow_valid == false)bool vision_valid = false;// vision is validbool 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 ratehrt_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 dataif (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 distanceif (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 spikecorr_lidar = 0;lidar_valid = false;lidar_offset_count++;if (lidar_offset_count > 3) { //if consecutive bigger/smaller measurements -> new ground offset -> reinitlidar_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 sonarfloat 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 averageif (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 fuzzyif (!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 printwarnx("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 mst_prev = t;/* increase EPH/EPV on each step */if (eph < 0.000001f) { //get case where eph is 0 -> would stay 0eph = 0.001;}if (eph < max_eph_epv) {eph *= 1.0f + dt;}if (epv < 0.000001f) { //get case where epv is 0 -> would stay 0epv = 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 mocapif (params.disable_mocap) { //disable mocap if fake gps is useduse_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 __cplusplusstruct __EXPORT vehicle_local_position_s {#elsestruct vehicle_local_position_s {#endifuint64_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 __cplusplusstruct __EXPORT vehicle_global_position_s {#elsestruct vehicle_global_position_s {#endifuint64_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 __cplusplusstruct __EXPORT sensor_combined_s {#elsestruct sensor_combined_s {#endifuint64_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 __cplusplusstatic 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 __cplusplusstruct __EXPORT parameter_update_s {#elsestruct parameter_update_s {#endifuint64_t timestamp;bool saved;#ifdef __cplusplus#endif};#ifdef __cplusplusstruct __EXPORT actuator_controls_s {#elsestruct actuator_controls_s {#endifuint64_t timestamp;uint64_t timestamp_sample;float control[8];#ifdef __cplusplusstatic 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 __cplusplusstruct __EXPORT actuator_armed_s {#elsestruct actuator_armed_s {#endifuint64_t timestamp;bool armed;bool prearmed;bool ready_to_arm;bool lockdown;bool force_failsafe;bool in_esc_calibration_mode;#ifdef __cplusplus#endif#ifdef __cplusplusstruct __EXPORT vision_position_estimate_s {#elsestruct vision_position_estimate_s {#endifuint32_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 __cplusplusstruct __EXPORT att_pos_mocap_s {#elsestruct att_pos_mocap_s {#endifuint32_t id;uint64_t timestamp_boot;uint64_t timestamp_computer;float q[4];float x;float y;float z;#ifdef __cplusplus#endif};#ifdef __cplusplusstruct __EXPORT vehicle_gps_position_s {#elsestruct vehicle_gps_position_s {#endifuint64_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 sonarfloat 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 averageif (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 fuzzyif (!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需要看

intMulticopterPositionControl::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;}

voidMulticopterPositionControl::task_main_trampoline(int argc, char *argv[]){pos_control::g_control->task_main();}

voidMulticopterPositionControl::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 blockssetDt(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 statewas_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 controlledif (!_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 componentif(cos_ratio > 0) {ft_vel *= (cos_ratio);// min speed a little faster than target velft_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 anymoreif (!_takeoff_jumped && !_vehicle_status.condition_landed && fabsf(_takeoff_thrust_sp) < FLT_EPSILON) {_takeoff_jumped = true;}if (!_takeoff_jumped) {// ramp thrust setpoint upif (_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 partthrust_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 accelerationmath::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 accelerationfloat 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 continuousif (!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 errorvel_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 abovethrust_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 ifif (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;}

这些设定值的确定是不是就是控制策略的体现？

voidMulticopterPositionControl::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);}}

voidMulticopterPositionControl::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;}}

voidMulticopterPositionControl::control_manual(float dt){math::Vector<3> req_vel_sp; // X,Y in local frame and Z in global (D), in [-1,1] normalized rangereq_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 ifvelocity 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 __cplusplusstruct __EXPORT position_setpoint_triplet_s {#elsestruct position_setpoint_triplet_s {#endifstruct 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);}}

再看看

voidMulticopterPositionControl::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));

voidMulticopterAttitudeControl::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)

voidVtolAttitudeControl::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);}}

voidVtolAttitudeControl::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

intMulticopterAttitudeControl::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;}

voidMulticopterAttitudeControl::task_main_trampoline(int argc, char *argv[]){mc_att_control::g_control->task_main();}

voidMulticopterAttitudeControl::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 controlcontrol_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'主题***********//************输出是角速度设定值向量和油门设定值*************/voidMulticopterAttitudeControl::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 *//*************************注意看注释！！********************//************输入是角速度设定值向量、油门设定值*************//**************************姿态控制值***********************/voidMulticopterAttitudeControl::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.cppint 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_0orb_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 __cplusplusstruct __EXPORT vehicle_local_position_s {#elsestruct vehicle_local_position_s {#endifuint64_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 __cplusplusstruct __EXPORT vehicle_global_position_s {#elsestruct vehicle_global_position_s {#endifuint64_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 __cplusplusstruct __EXPORT vehicle_attitude_s {#elsestruct vehicle_attitude_s {#endifuint64_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 __cplusplusstruct __EXPORT control_state_s {#elsestruct control_state_s {#endifuint64_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 __cplusplusstruct __EXPORT vehicle_attitude_setpoint_s {#elsestruct vehicle_attitude_setpoint_s {#endifuint64_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 __cplusplusstruct __EXPORT vehicle_rates_setpoint_s {#elsestruct vehicle_rates_setpoint_s {#endifuint64_t timestamp;float roll;float pitch;float yaw;float thrust;#ifdef __cplusplus#endif};#ifdef __cplusplusstruct __EXPORT actuator_controls_0_s {#elsestruct actuator_controls_0_s {#endifuint64_t timestamp;uint64_t timestamp_sample;float control[8];#ifdef __cplusplusstatic const uint8_t NUM_ACTUATOR_CONTROLS = 8;static const uint8_t NUM_ACTUATOR_CONTROL_GROUPS = 4;#endif};

这是用到的结构体

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

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