本文是边分析边写的,顺便就记录下了分析的思路,并不是按照教材那种思路介绍性的,而是按照程序员分析程序的思路来的。所以读者跟着看有些地方看了意义不大,但是这种程序分析的思路还是可以借鉴的,如果有大神看到了本文,有更好的见解,欢迎指教。
前面的是从APM代码角度看的,后面是从原生码角度看的。这blog写的我自己看了都想吐,翻下去都是代码,就最后一张图人性化一点。
温馨提示:可以从后面点看,程序中有注释。
先从ardupilot/libraries/AP_OpticalFlow/examples/AP_OpticalFlow_test/ AP_OpticalFlow_test.cpp看起
voidsetup() { hal.console->println("OpticalFlowlibrary test ver 1.6"); hal.scheduler->delay(1000); // flowSensor initialization optflow.init(); if (!optflow.healthy()) { hal.console->print("Failed toinitialise PX4Flow "); } hal.scheduler->delay(1000);只需要看 } voidloop() { hal.console->println("this onlytests compilation succeeds"); hal.scheduler->delay(5000); }
因此optflow.init();再进行判断if (!optflow.healthy()),任务就完成了
跳转到optflow.init();
voidOpticalFlow::init(void) { if (backend != NULL) { backend->init(); } else { _enabled = 0; } }
跳转到init();
classOpticalFlow_backend { friend class OpticalFlow; public: // constructor OpticalFlow_backend(OpticalFlow&_frontend) : frontend(_frontend) {} // init - initialise sensor virtual void init() = 0; // read latest values from sensor and fillin x,y and totals. virtual void update() = 0; protected: // access to frontend OpticalFlow &frontend; // update the frontend void _update_frontend(const structOpticalFlow::OpticalFlow_state &state); // get the flow scaling parameters Vector2f _flowScaler(void) const { returnVector2f(frontend._flowScalerX, frontend._flowScalerY); } // get the yaw angle in radians float _yawAngleRad(void) const { returnradians(float(frontend._yawAngle_cd) * 0.01f); } };
看到了吧,virtualvoid init() = 0;就是这个虚函数
怎么找到它的实例化呢?
Ctrl+H全局搜索public OpticalFlow_backend
路径是:ardupilot/libraries/AP_OpticalFlow/ AP_OpticalFlow_PX4.h
路径是:ardupilot/libraries/AP_OpticalFlow/ AP_OpticalFlow_PX4.h class AP_OpticalFlow_PX4 : public OpticalFlow_backend { public: /// constructor AP_OpticalFlow_PX4(OpticalFlow &_frontend); // init - initialise the sensor void init(); // update - read latest values from sensor and fill in x,y and totals. void update(void); private: int _fd; // file descriptor for sensor uint64_t _last_timestamp; // time of last update (used to avoid processing old reports) }; void init();就是实例化,继续搜索AP_OpticalFlow_PX4:: init
void init();就是实例化,继续搜索AP_OpticalFlow_PX4::init
本体就是
voidAP_OpticalFlow_PX4::init(void) { _fd = open(PX4FLOW0_DEVICE_PATH, O_RDONLY);//关于sd卡文件的read // check for failure to open device if (_fd == -1) { return; } // change to 10Hz update if (ioctl(_fd, SENSORIOCSPOLLRATE, 10) != 0) { hal.console->printf("Unable to set flow rate to10Hz\n"); } }
慢慢查看,先看_fd = open(PX4FLOW0_DEVICE_PATH, O_RDONLY);源码如下
路径是ardupilot/modules/PX4NuttX/nuttx/fs/fs_open.c
/**************************************************************************** *Name: open * *Description: * Standard 'open' interface * ****************************************************************************/ int open(const char *path, int oflags, ...) { FARstruct filelist *list; FARstruct inode *inode; FARconst char *relpath = NULL; #if defined(CONFIG_FILE_MODE) ||!defined(CONFIG_DISABLE_MOUNTPOINT) mode_t mode = 0666; #endif int ret; int fd; /*Get the thread-specific file list */得到具体线程列表 list = sched_getfiles(); if(!list) { ret = EMFILE; goto errout; } #ifdef CONFIG_FILE_MODE # ifdef CONFIG_CPP_HAVE_WARNING # warning "File creation not implemented" # endif /*If the file is opened for creation, then get the mode bits */ if(oflags & (O_WRONLY|O_CREAT) != 0) { va_list ap; va_start(ap, oflags); mode = va_arg(ap, mode_t); va_end(ap); } #endif /*Get an inode for this file */得到索引节点 inode = inode_find(path, &relpath); if(!inode) { /* "O_CREAT is not set and the named file does not exist. Or, a * directory component in pathname does not exist or is a dangling * symbolic link." */ ret = ENOENT; goto errout; } /*Verify that the inode is valid and either a "normal" or amountpoint. We *specifically exclude block drivers. */ #ifndef CONFIG_DISABLE_MOUNTPOINT if((!INODE_IS_DRIVER(inode) && !INODE_IS_MOUNTPT(inode)) ||!inode->u.i_ops) #else if(!INODE_IS_DRIVER(inode) || !inode->u.i_ops) #endif { ret = ENXIO; goto errout_with_inode; } /*Make sure that the inode supports the requested access */ ret= inode_checkflags(inode, oflags); if(ret < 0) { ret = -ret; goto errout_with_inode; } /*Associate the inode with a file structure */ fd= files_allocate(inode, oflags, 0, 0); if(fd < 0) { ret = EMFILE; goto errout_with_inode; } /*Perform the driver open operation. NOTEthat the open method may be *called many times. The driver/mountpointlogic should handled this *because it may also be closed that many times. */ ret= OK; if(inode->u.i_ops->open) { #ifndef CONFIG_DISABLE_MOUNTPOINT if (INODE_IS_MOUNTPT(inode)) { ret = inode->u.i_mops->open((FAR structfile*)&list->fl_files[fd], relpath,oflags, mode); } else #endif { ret = inode->u.i_ops->open((FAR structfile*)&list->fl_files[fd]); } } if(ret < 0) { ret = -ret; goto errout_with_fd; } return fd; errout_with_fd: files_release(fd); errout_with_inode: inode_release(inode); errout: set_errno(ret); return ERROR; } 跳转到list = sched_getfiles(); 路径是ardupilot/modules/PX4NuttX/nuttx/sched/sched_getfiles.c /************************************************************************ *Name: sched_getfiles * *Description: * Return a pointer to the file list for this thread * *Parameters: * None * *Return Value: * Apointer to the errno. * *Assumptions: * ************************************************************************/ #if CONFIG_NFILE_DESCRIPTORS > 0 FAR struct filelist *sched_getfiles(void) { FARstruct tcb_s *rtcb = (FAR struct tcb_s*)g_readytorun.head; FARstruct task_group_s *group = rtcb->group; /*The group may be NULL under certain conditions. For example, if *debug output is attempted from the IDLE thead before the group has *been allocated. I have only seen thiscase when memory management *debug is enabled. */ if(group) { return &group->tg_filelist; } /*Higher level logic must handle the NULL gracefully */ return NULL; } #endif /* CONFIG_NFILE_DESCRIPTORS */ 跳转到inode = inode_find(path, &relpath); 路径是ardupilot/modules/PX4NuttX/nuttx/fs/fs_inodefine.c /**************************************************************************** *Name: inode_find * *Description: * Thisis called from the open() logic to get a reference to the inode * associated with a path. * ****************************************************************************/ //得到一个inode与路径相关的引用 FAR struct inode *inode_find(FAR const char*path, FAR const char **relpath) { FARstruct inode *node; if(!*path || path[0] != '/') { return NULL; } /*Find the node matching the path. Iffound, increment the count of *references on the node. */ inode_semtake(); node = inode_search(&path, (FAR struct inode**)NULL, (FAR struct inode**)NULL,relpath); if(node) { node->i_crefs++; } inode_semgive(); return node; }
看到不大懂,姑且认为是关于sd卡文件的read
#definePX4FLOW0_DEVICE_PATH "/dev/px4flow0"路径ardupilot/modules/PX4Firmware/src/drivers/drv_px4flow.h
void AP_OpticalFlow_PX4::init(void) { _fd = open(PX4FLOW0_DEVICE_PATH, O_RDONLY); // check for failure to open device if (_fd == -1) { return; } // change to 10Hz update if (ioctl(_fd, SENSORIOCSPOLLRATE, 10) != 0) { hal.console->printf("Unable to set flow rate to10Hz\n"); } }
继续往下看ioctl(_fd, SENSORIOCSPOLLRATE, 10);这句话的目的是控制光流跟新频率为10Hz
分别看ioctl的输入
_fd = open(PX4FLOW0_DEVICE_PATH, O_RDONLY);_fd用来判断是否有这个路径并读到了东西
SENSORIOCSPOLLRATE设置传感器读取的时间间隔
路径ardupilot/modules/PX4Firmware/src/drivers/drv_sensor.h
#define SENSORIOCSPOLLRATE _SENSORIOC(0)
#define _SENSORIOC(_n) (_PX4_IOC(_SENSORIOCBASE,_n))
#define _SENSORIOCBASE (0x2000)ardupilot/modules/PX4Firmware/src/drivers/drv_sensor.h
#define _PX4_IOC(x,y) _IOC(x,y) ardupilot/modules/PX4Firmware/src/platforms/px4_defines.h
#define_IOC(type,nr) ((type)|(nr))
也就是说SENSORIOCSPOLLRATE等价于_PX4_IOC(0x2000,0)等价于(0x2000)|(0)
ardupilot/modules/PX4Firmware/Build/px4fmu-v2_APM.build/nuttx-export/include/nuttx/fs/ioctl.h
与上面的宏定义相比
#define_PX4FLOWIOCBASE (0x7700)
#define__PX4FLOWIOC(_n) (_IOC(_PX4FLOWIOCBASE,_n))
另外,在ardupilot/modules/PX4Firmware/src/drivers/drv_px4flow.h中找到“px4flow drivers also implement the generic sensordriver interfaces from drv_sensor.h”,表明光流的驱动在通用传感器驱动文件中ardupilot/modules/PX4Firmware/src/drivers/drv_sensor.h
跳转到ioctl(_fd,SENSORIOCSPOLLRATE, 10)
/**************************************************************************** *Name: ioctl * *Description: * Perform device specific operations. * *Parameters: * fd File/socket descriptor ofdevice * req The ioctl command * arg The argument of the ioctlcmd * *Return: * >=0 on success (positive non-zero values are cmd-specific) * -1on failure withi errno set properly: * * EBADF * 'fd' is not a valid descriptor. * EFAULT * 'arg' references an inaccessible memory area. * EINVAL * 'cmd' or 'arg' is not valid. * ENOTTY * 'fd' is not associated with a character special device. * ENOTTY * The specified request does not apply to the kind of object that the * descriptor 'fd' references. * ****************************************************************************/ int ioctl(int fd, int req, unsigned longarg) { interr; #if CONFIG_NFILE_DESCRIPTORS > 0 FARstruct filelist *list; FARstruct file *this_file; FARstruct inode *inode; int ret = OK; /*Did we get a valid file descriptor? */ if((unsigned int)fd >= CONFIG_NFILE_DESCRIPTORS) #endif { /* Perform the socket ioctl */ #if defined(CONFIG_NET) &&CONFIG_NSOCKET_DESCRIPTORS > 0 if ((unsigned int)fd <(CONFIG_NFILE_DESCRIPTORS+CONFIG_NSOCKET_DESCRIPTORS)) { return netdev_ioctl(fd, req, arg); } else #endif { err = EBADF; goto errout; } } #if CONFIG_NFILE_DESCRIPTORS > 0 /*Get the thread-specific file list */ list = sched_getfiles(); if(!list) { err = EMFILE; goto errout; } /*Is a driver registered? Does it support the ioctl method? */ this_file = &list->fl_files[fd]; inode = this_file->f_inode; if(inode && inode->u.i_ops && inode->u.i_ops->ioctl) { /* Yes, then let it perform the ioctl */ ret = (int)inode->u.i_ops->ioctl(this_file,req, arg); if (ret < 0) { err = -ret; goto errout; } } return ret; #endif errout: set_errno(err); return ERROR; }
这句话是这个函数的核心
/* Yes, then let it perform the ioctl */
ret =(int)inode->u.i_ops->ioctl(this_file, req, arg);
跳转至ardupilot/modules/PX4Firmware/Build/px4fmu-v2_APM.build/nuttx-export/include/nuttx/fs/fs.h
int (*ioctl)(FAR struct file *filp, int cmd, unsigned long arg);
无意中跑到ardupilot/libraries/AP_HAL_PX4/GPIO.cpp中,Ctrl+点击ioctl
可以搜索到现在可以总结关于光流的程序有哪些部分了:
ardupilot/libraries/AP_OpticalFlow文件夹
ardupilot/modules/PX4Firmware/src/drivers/drv_px4flow.h
ardupilot/modules/PX4Firmware/src/drivers/px4flow文件夹
顺便再ardupilot/modules/PX4Firmware/src/drivers/px4flow/px4flow.cpp里面找到了
extern "C"__EXPORT int px4flow_main(int argc, char *argv[]);
也就是程序入口,久违的感动
现在有2条思路:一是继续跳转至ioctl实例化,继续看如何执行的;二是顺着main来看,先还是按照老思路example来
int PX4FLOW::ioctl(structfile *filp, int cmd, unsigned long arg) { switch(cmd) { caseSENSORIOCSPOLLRATE: { switch(arg) { /*switching to manual polling */ caseSENSOR_POLLRATE_MANUAL: stop(); _measure_ticks= 0; returnOK; /*external signalling (DRDY) not supported */ case SENSOR_POLLRATE_EXTERNAL: /*zero would be bad */ case0: return-EINVAL; /*set default/max polling rate */ caseSENSOR_POLLRATE_MAX: caseSENSOR_POLLRATE_DEFAULT: { /*do we need to start internal polling? */ boolwant_start = (_measure_ticks == 0); /*set interval for next measurement to minimum legal value */ _measure_ticks= USEC2TICK(PX4FLOW_CONVERSION_INTERVAL); /*if we need to start the poll state machine, do it */ if(want_start) { start(); } returnOK; } /*adjust to a legal polling interval in Hz */ default:{ /*do we need to start internal polling? */ boolwant_start = (_measure_ticks == 0); /*convert hz to tick interval via microseconds */ unsignedticks = USEC2TICK(1000000 / arg); /*check against maximum rate */ if(ticks < USEC2TICK(PX4FLOW_CONVERSION_INTERVAL)) { return-EINVAL; } /*update interval for next measurement */ _measure_ticks= ticks; /*if we need to start the poll state machine, do it */ if(want_start) { start(); } returnOK; } } } caseSENSORIOCGPOLLRATE: if(_measure_ticks == 0) { returnSENSOR_POLLRATE_MANUAL; } return(1000 / _measure_ticks); caseSENSORIOCSQUEUEDEPTH: { /*lower bound is mandatory, upper bound is a sanity check */ if((arg < 1) || (arg > 100)) { return-EINVAL; } irqstate_t flags = irqsave(); if(!_reports->resize(arg)) { irqrestore(flags); return-ENOMEM; } irqrestore(flags); returnOK; } caseSENSORIOCGQUEUEDEPTH: return_reports->size(); caseSENSORIOCSROTATION: _sensor_rotation= (enum Rotation)arg; returnOK; caseSENSORIOCGROTATION: return_sensor_rotation; caseSENSORIOCRESET: /*XXX implement this */ return-EINVAL; default: /*give it to the superclass */ returnI2C::ioctl(filp, cmd, arg); } }
这个函数很好理解,根据传入的参数对号入座,传入的是SENSORIOCSPOLLRATE,那么进入
case SENSORIOCSPOLLRATE: { switch(arg) { /*switching to manual polling */ caseSENSOR_POLLRATE_MANUAL: stop(); _measure_ticks= 0; returnOK; /*external signalling (DRDY) not supported */ case SENSOR_POLLRATE_EXTERNAL: /*zero would be bad */ case0: return-EINVAL; /*set default/max polling rate */ caseSENSOR_POLLRATE_MAX: caseSENSOR_POLLRATE_DEFAULT: { /*do we need to start internal polling? */ boolwant_start = (_measure_ticks == 0); /*set interval for next measurement to minimum legal value */ _measure_ticks= USEC2TICK(PX4FLOW_CONVERSION_INTERVAL); /*if we need to start the poll state machine, do it */ if(want_start) { start(); } returnOK; } /*adjust to a legal polling interval in Hz */ default:{ /*do we need to start internal polling? */ boolwant_start = (_measure_ticks == 0); /*convert hz to tick interval via microseconds */ unsignedticks = USEC2TICK(1000000 / arg); /*check against maximum rate */ if(ticks < USEC2TICK(PX4FLOW_CONVERSION_INTERVAL)) { return-EINVAL; } /*update interval for next measurement */ _measure_ticks= ticks; /* if we need to start thepoll state machine, do it */ if(want_start) { start(); } returnOK; }
Ok,下一个参数是10,那么进入default
看注释能明白default是用来调整合适的间隔
里面需要重点看的是
unsigned ticks =USEC2TICK(1000000 / arg);// 看注释能明白应该是处理时间间隔的
start();//轮询状态机
先看unsigned ticks = USEC2TICK(1000000 / arg);
跳转至ardupilot/modules/PX4Firmware/Build/px4fmu-v2_APM.build/nuttx-export/include/nuttx/clock.h
#define USEC2TICK(usec) (((usec)+(USEC_PER_TICK/2))/USEC_PER_TICK) /* Rounds */
这里并没有寄存器的相关操作,只是把用户所定的时间进行了一些转换,方便单片机之后定时操作
那么跳转至start();
void PX4FLOW::start() { /*reset the report ring and state machine *///复位报告环和状态机 _collect_phase= false; _reports->flush(); /*schedule a cycle to start things *///安排一个周期开始一些事情 work_queue(HPWORK,&_work, (worker_t)&PX4FLOW::cycle_trampoline, this, 1); /*notify about state change */ structsubsystem_info_s info = { true, true, true, subsystem_info_s::SUBSYSTEM_TYPE_OPTICALFLOW }; staticorb_advert_t pub = nullptr; if(pub != nullptr) { orb_publish(ORB_ID(subsystem_info),pub, &info); }else { pub= orb_advertise(ORB_ID(subsystem_info), &info); } }
看函数名字的意思是启动光流
看来最关键的是work_queue(HPWORK, &_work, (worker_t)&PX4FLOW::cycle_trampoline,this, 1)再后面就是发布和公告子系统信息,跳转至work_queue(HPWORK, &_work,(worker_t)&PX4FLOW::cycle_trampoline, this, 1);
/**************************************************************************** *Name: work_queue * *Description: * Queue work to be performed at a later time. All queued work will be * performed on the worker thread of of execution (not the caller's). * * Thework structure is allocated by caller, but completely managed by * thework queue logic. The caller shouldnever modify the contents of * thework queue structure; the caller should not call work_queue() * again until either (1) the previous work has been performed and removed * fromthe queue, or (2) work_cancel() has been called to cancel the work * andremove it from the work queue. * *Input parameters: * qid - The work queue ID (index) * work - The work structure toqueue * worker - The worker callback to be invoked. The callback will invoked * on the worker thread of execution. * arg - The argument that will bepassed to the workder callback when * int is invoked. * delay - Delay (in clock ticks)from the time queue until the worker * is invoked. Zero means to perform the work immediately. * *Returned Value: * Zeroon success, a negated errno on failure * ****************************************************************************/ 注释翻译: 在稍后的时间将要执行的队列的工作。 所有排队工作将在工作线程(不是调用的程序)上进行。 工作结构由调用者分配,but,由工作队列逻辑完全管理。调用者不能修改工作队列结构的内容,也不能再次调用work_queue(),除非任务执行完毕或者任务被取消 输入参数: qid -工作队列的ID work -队列的工作结构 worker -工作线程被执行,worker callback 会被调用 arg -参数会传递给worker callback delay -延时多久再执行这个任务,若为0,则直接执行 返回值: 0,则成功执行;错误号,则失败 int work_queue(int qid, FAR struct work_s*work, worker_t worker, FAR void *arg, uint32_t delay) { FARstruct wqueue_s *wqueue = &g_work[qid]; irqstate_t flags; DEBUGASSERT(work != NULL && (unsigned)qid < NWORKERS);//应该是调试用的debug /* First, initialize the work structure */ work->worker = worker; /* Work callback */ work->arg = arg; /* Callback argument */ work->delay = delay; /* Delay until work performed */ /*Now, time-tag that entry and put it in the work queue. This must be *done with interrupts disabled. Thispermits this function to be called *from with task logic or interrupt handlers. */ flags = irqsave(); work->qtime = clock_systimer(); /* Time work queued */ dq_addlast((FAR dq_entry_t *)work, &wqueue->q); kill(wqueue->pid, SIGWORK); /* Wake up the worker thread */ irqrestore(flags); return OK; }
在看一下调用的函数work_queue(HPWORK, &_work,(worker_t)&PX4FLOW::cycle_trampoline, this, 1);
int work_queue(int qid, FAR struct work_s*work, worker_t worker, FAR void *arg, uint32_t delay)
这个应该是比较关键的函数,一点点看
FAR struct wqueue_s *wqueue =&g_work[qid]; /* This structure defines the state on onework queue. Thisstructure is * used internally by the OS and worker queuelogic and should not be * accessed by application logic. */ 注释翻译: 这个结构体定义了一个工作队列的状态。这种结构是由系统和worker队列逻辑在内部使用,不应被应用程序逻辑访问。 struct wqueue_s { pid_t pid; /* The taskID of the worker thread *///worker线程中的任务ID struct dq_queue_s q; /* Thequeue of pending work *///挂起的工作队列 }; typedef int16_t pid_t; struct dq_queue_s { FARdq_entry_t *head;//头 FARdq_entry_t *tail;//尾 };
也就是说
FARstruct wqueue_s *wqueue = &g_work[qid];
irqstate_t flags;
这2句是声明数据类型
/*First, initialize the work structure *///初始化work结构体
work->worker = worker; /* Work callback */
work->arg = arg; /* Callback argument */
work->delay = delay; /* Delay until work performed */
work的定义struct work_s work;
work_s的定义
/* Defines one entry in the workqueue. The user only needs thisstructure * inorder to declare instances of the work structure. Handling of all *fields is performed by the work APIs */ 注释翻译:定义一个工作队列的入口。使用前声明这个结构,再实例化。所有处理都是有API来完成的 struct work_s { structdq_entry_s dq; /* Implements a doublylinked list */ worker_t worker; /* Work callback */ FARvoid *arg; /* Callback argument*/ uint32_t qtime; /* Time work queued */ uint32_t delay; /* Delay until work performed */ }; work->worker = worker; /* Work callback */ work->arg = arg; /* Callback argument */ work->delay = delay; /* Delay until work performed */ /* Now, time-tag that entry and put it inthe work queue. This must be *done with interrupts disabled. Thispermits this function to be called *from with task logic or interrupt handlers. */ 注释翻译:现在,时间标记该条目,并把它的工作队列。这个需要在中断失能的情况下完成。允许这个函数从任务逻辑或中断处理程序调用。 flags = irqsave();/* Save the current primask state& disable IRQs */保存当前primask的状态,并失能中断 work->qtime = clock_systimer(); /* Time work queued */
跳转至irqsave();
static inline irqstate_t irqsave(void) { #ifdef CONFIG_ARMV7M_USEBASEPRI uint8_t basepri = getbasepri(); setbasepri(NVIC_SYSH_DISABLE_PRIORITY); return (irqstate_t)basepri; #else unsigned short primask; /*Return the current value of primask register and set *bit 0 of the primask register to disable interrupts */ __asm__ __volatile__ ( "\tmrs %0, primask\n" "\tcpsid i\n" : "=r" (primask) : : "memory"); return primask; #endif }
PRIMASK位:只允许NMI和hard fault异常,其他中断/ 异常都被屏蔽(当前CPU优先级=0)。(stm32的一个中断控制寄存器的位)
跳转至clock_systimer()
/* Access to raw system clock*****************/访问原始系统时钟
/* Direct access to the systemtimer/counter is supported only if (1) the
*system timer counter is available (i.e., we are notconfigured to use
* ahardware periodic timer), and (2) the execution environment has direct
*access to kernel global data
*/
注释翻译:下面2种情况可以直接访问系统时钟/计数器,(1)系统时钟是可得的(这就是说,我们没有配置硬件周期定时),(2)执行环境可以直接访问内核全局数据
extern volatile uint32_t g_system_timer
#define clock_systimer() g_system_timer
这个意思是从其他地方传来内核全局数据g_system_timer赋给work->qtime
关于g_system_timer的地方,只有/****************************************************************************
* Name: clock_timer
*
* Description:
* Thisfunction must be called once every time the real time clock
* interrupt occurs. The interval ofthis clock interrupt must be
* MSEC_PER_TICK
*
****************************************************************************/
void clock_timer(void)
{
/* Increment the per-tick system counter */
g_system_timer++;
}
就是每一次时钟中断g_system_timer++一次,另g_system_timer初始化为0
dq_addlast((FAR dq_entry_t *)work,&wqueue->q);
跳转至dq_addlast()//dq_addlast adds 'node' to the end of 'queue' dq_addlast增加'节点'到'队列'的末端
输入参数就是之前声明的work结构体和wqueue结构体
struct dq_queue_s
{
FAR dq_entry_t *head;
FAR dq_entry_t *tail;
};
kill(wqueue->pid,SIGWORK); /* Wake up theworker thread */唤醒工作进程
跳转至kill()
/************************************************************************ * Name: kill * * Description: * Thekill() system call can be used to send any signal to any task. * * Limitation: Sending of signals to 'process groups' is not * supported in NuttX * * Parameters: * pid- The id of the task to receive the signal. The POSIX kill * specification encodes process group information as zero and * negative pid values. Onlypositive, non-zero values of pid are * supported by this implementation. * signo - The signal number to send. If signo is zero, no signal is * sent, but all error checking is performed. * * Returned Value: * Onsuccess (at least one signal was sent), zero is returned. On * error, -1 is returned, and errno is set appropriately: * * EINVAL An invalid signal was specified. * EPERM The process does not havepermission to send the * signal to any of the target processes. * ESRCH The pid or process groupdoes not exist. * ENOSYS Do not support sending signals to process groups. * * Assumptions: * ************************************************************************/ 注释翻译: 这个kill()系统调用可用于任何信号发送到任何任务。 限制:信号发送到“进程组”在NuttX不支持 参数: pid -任务的ID用来接收信号。如果pid的值是0或负的,POSIX的kill规范会编码进程组信息 只有正的pid才会被执行 signo -需发送的信号数量。如果signo为0,就是没有信号发送,但是所有错误检查会被执行 返回值: 0, 成功;-1,发生错误,并且会返回错误码: EINVAL 无效的信号指定。 EPERM 进程没有权限将信号发送到任何目标进程。 ESRCH PID或进程组不存在。 ENOSYS 不支持发送的信号来处理组。 int kill(pid_t pid, int signo) { #ifdef CONFIG_SCHED_HAVE_PARENT FAR struct tcb_s *rtcb = (FAR struct tcb_s *)g_readytorun.head; #endif siginfo_t info; int ret; /* We do not support sending signals to process groups */ if (pid <= 0) { ret = -ENOSYS; goto errout; } /* Make sure that the signal is valid */ if (!GOOD_SIGNO(signo)) { ret = -EINVAL; goto errout; } //至此都是保证pid大于0 /* Keep things stationary through the following */ sched_lock(); /* Create the siginfo structure */创建siginfo结构体 info.si_signo = signo; info.si_code = SI_USER; info.si_value.sival_ptr = NULL; #ifdef CONFIG_SCHED_HAVE_PARENT info.si_pid =rtcb->pid; info.si_status = OK; #endif /* Send the signal */ ret = sig_dispatch(pid, &info); sched_unlock(); if (ret < 0) { goto errout; } return OK; errout: set_errno(-ret); return ERROR; }
sched_lock();/* Keep thingsstationary through the following */通过这个让事情变得平稳,
sched_unlock();//也就是一个保护作用,里面的内容不让其他程序触碰
重点就是info结构体赋值,赋值的内容就是之前的任务的相关信息,然后就是ret =sig_dispatch(pid,&info);把任务发出去(这一层一层包的- -!)
跳转至sig_dispatch()
/**************************************************************************** * Name: sig_dispatch * * Description: * Thisis the front-end for sig_tcbdispatch that should be typically * beused to dispatch a signal. If HAVE_GROUP_MEMBERS is defined, * thenfunction will follow the group signal delivery algorthrims: * * Thisfront-end does the following things before calling * sig_tcbdispatch. * * With HAVE_GROUP_MEMBERS defined: * -Get the TCB associated with the pid. * -If the TCB was found, get the group from the TCB. * - If the PID has already exited, lookup thegroup that that was * started by this task. * -Use the group to pick the TCB to receive the signal * -Call sig_tcbdispatch with the TCB * * With HAVE_GROUP_MEMBERS *not* defined * -Get the TCB associated with the pid. * -Call sig_tcbdispatch with the TCB * * Returned Value: * Returns 0 (OK) on success or a negated errno value on failure. * ****************************************************************************/ 注释翻译:(TCB是线程控制块的意思) 这个函数是用作派遣任务 如果HAVE_GROUP_MEMBERS被定义了,那么要完成 获取与PID相关的TCB 如果TCB被发现,请从TCB里获取组 如果PID已经退出,查找被此任务开始的组 使用组pick TCB来接收信号 用TCB来调用sig_tcbdispatch 如果HAVE_GROUP_MEMBERS没有被定义,那么 获取与PID相关的TCB 用TCB来调用sig_tcbdispatch int sig_dispatch(pid_t pid, FARsiginfo_t *info) { #ifdef HAVE_GROUP_MEMBERS FAR struct tcb_s *stcb; FAR struct task_group_s *group; /* Get the TCB associated with the pid */ stcb = sched_gettcb(pid);//获取一个任务ID,这个函数会返回一个指针指向相应的TCB if (stcb) { /* The task/thread associated with thisPID is still active. Get its * task group. */ //与PID相关的任务/线程还是激活的,得到它的任务组 group = stcb->group; } else { /* The task/thread associated with thisPID has exited. In the normal * usage model, the PID should correspondto the PID of the task that * created the task group. Try looking it up. */ group = group_findbypid(pid); } /* Didwe locate the group? */ if (group) { /* Yes.. call group_signal() to send thesignal to the correct group * member. */ return group_signal(group, info); } else { return -ESRCH; } #else FAR struct tcb_s *stcb; /* Get the TCB associated with the pid */ stcb = sched_gettcb(pid); if (!stcb) { return -ESRCH; } return sig_tcbdispatch(stcb, info); #endif }
ret = sig_dispatch(pid,&info);
快到内核了,东西越来越多,一点点看吧
stcb = sched_gettcb(pid); /* Get the TCB associated with the pid */获取pid相关的TCB
跳转至sched_gettcb()
/**************************************************************************** * Name: sched_gettcb * * Description: * Given a task ID, this function will return * thea pointer to the corresponding TCB (or NULL if there * isno such task ID). * ****************************************************************************/ 注释翻译:获取一个任务ID,这个函数会返回一个指针指向相应的TCB FAR struct tcb_s*sched_gettcb(pid_t pid) { FAR struct tcb_s *ret = NULL; int hash_ndx; /* Verify that the PID is within range */ if (pid >= 0 ) { /* Get the hash_ndx associated with thepid */ hash_ndx = PIDHASH(pid); /* Verify that the correct TCB was found.*/ if (pid == g_pidhash[hash_ndx].pid) { /* Return the TCB associated withthis pid (if any) */ ret = g_pidhash[hash_ndx].tcb; } } /* Return the TCB. */ return ret; } 现在应该到了任务分配层了,应该也算内核了吧,下一层就是物理驱动层 细细看来,跳转至hash_ndx =PIDHASH(pid); group = stcb->group;看看这个group是什么东西 #ifdef HAVE_TASK_GROUP FAR struct task_group_s *group; /* Pointer to shared task group data */ #endif struct task_group_s { #ifdef HAVE_GROUP_MEMBERS struct task_group_s *flink; /* Supports a singly linked list */ gid_t tg_gid; /* The ID of this task group */ gid_t tg_pgid; /* The ID of the parent task group */ #endif #if!defined(CONFIG_DISABLE_PTHREAD) && defined(CONFIG_SCHED_HAVE_PARENT) pid_t tg_task; /* The ID of the task within the group */ #endif uint8_t tg_flags; /* See GROUP_FLAG_* definitions */ /* Group membership ***********************************************************/ uint8_t tg_nmembers; /* Number of members in thegroup */ #ifdef HAVE_GROUP_MEMBERS uint8_t tg_mxmembers; /* Number of members inallocation */ FAR pid_t *tg_members; /* Members of the group */ #endif /* atexit/on_exit support ****************************************************/ #if defined(CONFIG_SCHED_ATEXIT)&& !defined(CONFIG_SCHED_ONEXIT) # ifdefined(CONFIG_SCHED_ATEXIT_MAX) && CONFIG_SCHED_ATEXIT_MAX > 1 atexitfunc_t tg_atexitfunc[CONFIG_SCHED_ATEXIT_MAX]; # else atexitfunc_t tg_atexitfunc; /* Called when exit is called. */ # endif #endif #ifdef CONFIG_SCHED_ONEXIT # ifdefined(CONFIG_SCHED_ONEXIT_MAX) && CONFIG_SCHED_ONEXIT_MAX > 1 onexitfunc_t tg_onexitfunc[CONFIG_SCHED_ONEXIT_MAX]; FAR void *tg_onexitarg[CONFIG_SCHED_ONEXIT_MAX]; # else onexitfunc_t tg_onexitfunc; /* Called when exit is called. */ FAR void *tg_onexitarg; /* The argument passed to the function */ # endif #endif /* Child exit status **********************************************************/ #ifdefined(CONFIG_SCHED_HAVE_PARENT) && defined(CONFIG_SCHED_CHILD_STATUS) FAR struct child_status_s *tg_children; /* Head of a list of childstatus */ #endif /* waitpid support************************************************************/ /*Simple mechanism used only when there is no support for SIGCHLD */ #if defined(CONFIG_SCHED_WAITPID)&& !defined(CONFIG_SCHED_HAVE_PARENT) sem_t tg_exitsem; /* Support for waitpid */ int *tg_statloc; /* Location to return exitstatus */ #endif /* Pthreads *******************************************************************/ #ifndef CONFIG_DISABLE_PTHREAD /* Pthreadjoin Info: */ sem_t tg_joinsem; /* Mutually exclusive access tojoin data */ FAR struct join_s *tg_joinhead; /* Head of a list of joindata */ FAR struct join_s *tg_jointail; /* Tail of a list of joindata */ uint8_t tg_nkeys; /* Number pthread keys allocated */ #endif /* POSIX Signal Control Fields ************************************************/ #ifndef CONFIG_DISABLE_SIGNALS sq_queue_t sigpendingq; /* List of pending signals */ #endif /* Environment variables ******************************************************/ #ifndef CONFIG_DISABLE_ENVIRON size_t tg_envsize; /* Size of environment stringallocation */ FAR char *tg_envp; /* Allocated environmentstrings */ #endif /* PIC data space and address environments************************************/ /* Logically the PIC data space belongs here (see struct dspace_s). The * current logic needs review: There are differences in the away that the * life of the PIC data is managed. */ /* File descriptors ***********************************************************/ #if CONFIG_NFILE_DESCRIPTORS > 0 struct filelist tg_filelist; /* Maps file descriptor to file */ #endif /* FILE streams***************************************************************/ /* In a flat, single-heap build. The stream list is allocated with this * structure. But kernel mode witha kernel allocator, it must be separately * allocated using a user-space allocator. */ #if CONFIG_NFILE_STREAMS > 0 #if defined(CONFIG_NUTTX_KERNEL)&& defined(CONFIG_MM_KERNEL_HEAP) FAR struct streamlist *tg_streamlist; #else struct streamlist tg_streamlist; /* Holds C buffered I/O info */ #endif #endif /* Sockets ********************************************************************/ #if CONFIG_NSOCKET_DESCRIPTORS >0 struct socketlist tg_socketlist; /* Maps socket descriptor to socket */ #endif /* POSIX Named Message Queue Fields *******************************************/ #ifndef CONFIG_DISABLE_MQUEUE sq_queue_t tg_msgdesq; /* List of opened message queues */ #endif }; 好吧,还以为是个什么结构体呢,好长一段啊!!
这些都是不需要改动的地方,太多太杂了,先放着,重点看光流读取并且如何用于控制的部分,这些部分需要根据实际应用改动。
那么第二条思路开始,去main里面看看
(应该是这样前面看到属于ardupilot/libraries/AP_OpticalFlow文件夹里面的,属于APM的那套,应该有个虚拟层把硬件层抽象了,最后根据具体硬件来选择对应的硬件驱动;接下来要看的属于ardupilot/modules/PX4Firmware/src/drivers/px4flow文件夹里面的,属于PX4原生码)
extern "C" __EXPORT int px4flow_main(int argc, char *argv[]);
int px4flow_main(int argc, char *argv[]) { /* * Start/load the driver. */ if (!strcmp(argv[1], "start")) { return px4flow::start(); } /* * Stop the driver */ if (!strcmp(argv[1], "stop")) { px4flow::stop(); } /* * Test the driver/device. */ if (!strcmp(argv[1], "test")) { px4flow::test(); } /* * Reset the driver. */ if (!strcmp(argv[1], "reset")) { px4flow::reset(); } /* * Print driver information. */ if (!strcmp(argv[1], "info") || !strcmp(argv[1], "status")) { px4flow::info(); } errx(1, "unrecognized command, try 'start', 'test', 'reset' or 'info'"); }跳转至start()
/** * Start the driver. */ int start() { int fd; /* entry check: */ if (start_in_progress) { warnx("start already in progress"); return 1; } start_in_progress = true; if (g_dev != nullptr) { start_in_progress = false; warnx("already started"); return 1; } warnx("scanning I2C buses for device.."); int retry_nr = 0; while (1) { const int busses_to_try[] = { PX4_I2C_BUS_EXPANSION, #ifdef PX4_I2C_BUS_ESC PX4_I2C_BUS_ESC, #endif #ifdef PX4_I2C_BUS_ONBOARD PX4_I2C_BUS_ONBOARD, #endif -1 }; const int *cur_bus = busses_to_try; while (*cur_bus != -1) { /* create the driver */ /* warnx("trying bus %d", *cur_bus); */ g_dev = new PX4FLOW(*cur_bus); if (g_dev == nullptr) { /* this is a fatal error */ break; } /* init the driver: */ if (OK == g_dev->init()) { /* success! */ break; } /* destroy it again because it failed. */ delete g_dev; g_dev = nullptr; /* try next! */ cur_bus++; } /* check whether we found it: */ if (*cur_bus != -1) { /* check for failure: */ if (g_dev == nullptr) { break; } /* set the poll rate to default, starts automatic data collection */ fd = open(PX4FLOW0_DEVICE_PATH, O_RDONLY); if (fd < 0) { break; } if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_MAX) < 0) { break; } /* success! */ start_in_progress = false; return 0; } if (retry_nr < START_RETRY_COUNT) { /* lets not be too verbose */ // warnx("PX4FLOW not found on I2C busses. Retrying in %d ms. Giving up in %d retries.", START_RETRY_TIMEOUT, START_RETRY_COUNT - retry_nr); usleep(START_RETRY_TIMEOUT * 1000); retry_nr++; } else { break; } } if (g_dev != nullptr) { delete g_dev; g_dev = nullptr; } start_in_progress = false; return 1; }
之后又会深入到底层,暂时先放着
读数据在test()里面
/** * Perform some basic functional tests on the driver; * make sure we can collect data from the sensor in polled * and automatic modes. */ void test() { struct optical_flow_s report; ssize_t sz; int ret; int fd = open(PX4FLOW0_DEVICE_PATH, O_RDONLY); if (fd < 0) { err(1, "%s open failed (try 'px4flow start' if the driver is not running", PX4FLOW0_DEVICE_PATH); } /* do a simple demand read */ sz = read(fd, &report, sizeof(report)); if (sz != sizeof(report)) { warnx("immediate read failed"); } warnx("single read"); warnx("pixel_flow_x_integral: %i", f_integral.pixel_flow_x_integral); warnx("pixel_flow_y_integral: %i", f_integral.pixel_flow_y_integral); warnx("framecount_integral: %u", f_integral.frame_count_since_last_readout); /* start the sensor polling at 10Hz */ if (OK != ioctl(fd, SENSORIOCSPOLLRATE, 10)) { errx(1, "failed to set 10Hz poll rate"); } /* read the sensor 5x and report each value */ for (unsigned i = 0; i < 10; i++) { struct pollfd fds; /* wait for data to be ready */ fds.fd = fd; fds.events = POLLIN; ret = poll(&fds, 1, 2000); if (ret != 1) { errx(1, "timed out waiting for sensor data"); } /* now go get it */ sz = read(fd, &report, sizeof(report)); if (sz != sizeof(report)) { err(1, "periodic read failed"); } warnx("periodic read %u", i); warnx("framecount_total: %u", f.frame_count); warnx("framecount_integral: %u", f_integral.frame_count_since_last_readout); warnx("pixel_flow_x_integral: %i", f_integral.pixel_flow_x_integral); warnx("pixel_flow_y_integral: %i", f_integral.pixel_flow_y_integral); warnx("gyro_x_rate_integral: %i", f_integral.gyro_x_rate_integral); warnx("gyro_y_rate_integral: %i", f_integral.gyro_y_rate_integral); warnx("gyro_z_rate_integral: %i", f_integral.gyro_z_rate_integral); warnx("integration_timespan [us]: %u", f_integral.integration_timespan); warnx("ground_distance: %0.2f m", (double) f_integral.ground_distance / 1000); warnx("time since last sonar update [us]: %i", f_integral.sonar_timestamp); warnx("quality integration average : %i", f_integral.qual); warnx("quality : %i", f.qual); } errx(0, "PASS"); }看注释能明白sz = read(fd, &report, sizeof(report));是读的操作
ardupilot/modules/PX4Nuttx/nuttx/fs/fs_read.c这个路径应该是系统的路径,已经进了系统吗?系统是什么概念啊?
ssize_t read(int fd, FAR void *buf, size_t nbytes) { /* Did we get a valid file descriptor? */ #if CONFIG_NFILE_DESCRIPTORS > 0 if ((unsigned int)fd >= CONFIG_NFILE_DESCRIPTORS) #endif { /* No.. If networking is enabled, read() is the same as recv() with * the flags parameter set to zero. */ #if defined(CONFIG_NET) && CONFIG_NSOCKET_DESCRIPTORS > 0 return recv(fd, buf, nbytes, 0); #else /* No networking... it is a bad descriptor in any event */ set_errno(EBADF); return ERROR; #endif } /* The descriptor is in a valid range to file descriptor... do the read */ #if CONFIG_NFILE_DESCRIPTORS > 0 return file_read(fd, buf, nbytes); #endif }看注释能明白return file_read(fd, buf, nbytes);是读操作
/**************************************************************************** * Private Functions ****************************************************************************/ #if CONFIG_NFILE_DESCRIPTORS > 0 static inline ssize_t file_read(int fd, FAR void *buf, size_t nbytes) { FAR struct filelist *list; int ret = -EBADF; /* Get the thread-specific file list */ list = sched_getfiles(); if (!list) { /* Failed to get the file list */ ret = -EMFILE; } /* Were we given a valid file descriptor? */ else if ((unsigned int)fd < CONFIG_NFILE_DESCRIPTORS) { FAR struct file *this_file = &list->fl_files[fd]; FAR struct inode *inode = this_file->f_inode; /* Yes.. Was this file opened for read access? */ if ((this_file->f_oflags & O_RDOK) == 0) { /* No.. File is not read-able */ ret = -EACCES; } /* Is a driver or mountpoint registered? If so, does it support * the read method? */ else if (inode && inode->u.i_ops && inode->u.i_ops->read) { /* Yes.. then let it perform the read. NOTE that for the case * of the mountpoint, we depend on the read methods bing * identical in signature and position in the operations vtable. */ ret = (int)inode->u.i_ops->read(this_file, (char*)buf, (size_t)nbytes); } } /* If an error occurred, set errno and return -1 (ERROR) */ if (ret < 0) { set_errno(-ret); return ERROR; } /* Otherwise, return the number of bytes read */ return ret; } #endif跟踪buf可知ret = (int)inode->u.i_ops->read(this_file, (char*)buf, (size_t)nbytes);是读操作
于是进入了“文件操作”
struct file_operations { /* The device driver open method differs from the mountpoint open method */ int (*open)(FAR struct file *filp); /* The following methods must be identical in signature and position because * the struct file_operations and struct mountp_operations are treated like * unions. */ int (*close)(FAR struct file *filp); ssize_t (*read)(FAR struct file *filp, FAR char *buffer, size_t buflen); ssize_t (*write)(FAR struct file *filp, FAR const char *buffer, size_t buflen); off_t (*seek)(FAR struct file *filp, off_t offset, int whence); int (*ioctl)(FAR struct file *filp, int cmd, unsigned long arg); #ifndef CONFIG_DISABLE_POLL int (*poll)(FAR struct file *filp, struct pollfd *fds, bool setup); #endif /* The two structures need not be common after this point */ };怎么继续找下去呢?C++好麻烦啊
看注释This structure is provided by devices when they are registered with the system. It is used to call back to perform device specific operations.
这种结构由设备当它们与系统中注册提供。 它是用来回调来执行设备的特定操作。
就是说首先要注册,再根据具体的设备(控制芯片)选用具体的操作。
ardupilot/modules/PX4Firmware/src/drivers/px4flow.cpp中找到了相应的read函数
ssize_t PX4FLOW::read(struct file *filp, char *buffer, size_t buflen) { unsigned count = buflen / sizeof(struct optical_flow_s); struct optical_flow_s *rbuf = reinterpret_cast<struct optical_flow_s *>(buffer); int ret = 0; /* buffer must be large enough */ if (count < 1) { return -ENOSPC; } /* if automatic measurement is enabled */ if (_measure_ticks > 0) { /* * While there is space in the caller's buffer, and reports, copy them. * Note that we may be pre-empted by the workq thread while we are doing this; * we are careful to avoid racing with them. */ while (count--) { if (_reports->get(rbuf)) { ret += sizeof(*rbuf); rbuf++; } } /* if there was no data, warn the caller */ return ret ? ret : -EAGAIN; } /* manual measurement - run one conversion */ do { _reports->flush(); /* trigger a measurement */ if (OK != measure()) { ret = -EIO; break; } /* run the collection phase */ if (OK != collect()) { ret = -EIO; break; } /* state machine will have generated a report, copy it out */ if (_reports->get(rbuf)) { ret = sizeof(*rbuf); } } while (0); return ret; }get(rbuf) (read函数中)
bool RingBuffer::get(void *val, size_t val_size) { if (_tail != _head) { unsigned candidate; unsigned next; if ((val_size == 0) || (val_size > _item_size)) { val_size = _item_size; } do { /* decide which element we think we're going to read */ candidate = _tail; /* and what the corresponding next index will be */ next = _next(candidate); /* go ahead and read from this index */ if (val != NULL) { memcpy(val, &_buf[candidate * _item_size], val_size); } /* if the tail pointer didn't change, we got our item */ } while (!__PX4_SBCAP(&_tail, candidate, next)); return true; } else { return false; } }字面意思的从缓冲环中得到数据,并存到rbuf中
int PX4FLOW::measure() { int ret; /* * Send the command to begin a measurement. */ uint8_t cmd = PX4FLOW_REG; ret = transfer(&cmd, 1, nullptr, 0); if (OK != ret) { perf_count(_comms_errors); DEVICE_DEBUG("i2c::transfer returned %d", ret); return ret; } ret = OK; return ret; }看到 PX4FLOW_REG
/* PX4FLOW Registers addresses */ #define PX4FLOW_REG 0x16 ///< Measure Register 22
根据DEVICE_DEBUG("i2c::transfer returned %d", ret);可以得知使用的transfer为i2c::transfer
Ctrl+H全局搜索i2c::transfer
i2c函数很多,综合输入参数和DEVICE_DEBUG("i2c::transfer returned %d", ret);信息,可知上图中的i2c::transfer就是我们要找的函数
int I2C::transfer(const uint8_t *send, unsigned send_len, uint8_t *recv, unsigned recv_len) { struct i2c_msg_s msgv[2]; unsigned msgs; int ret; unsigned retry_count = 0; do { // DEVICE_DEBUG("transfer out %p/%u in %p/%u", send, send_len, recv, recv_len); msgs = 0; if (send_len > 0) { msgv[msgs].addr = _address; msgv[msgs].flags = 0; msgv[msgs].buffer = const_cast<uint8_t *>(send); msgv[msgs].length = send_len; msgs++; } if (recv_len > 0) { msgv[msgs].addr = _address; msgv[msgs].flags = I2C_M_READ; msgv[msgs].buffer = recv; msgv[msgs].length = recv_len; msgs++; } if (msgs == 0) { return -EINVAL; } ret = I2C_TRANSFER(_dev, &msgv[0], msgs); /* success */ if (ret == OK) { break; } /* if we have already retried once, or we are going to give up, then reset the bus */ if ((retry_count >= 1) || (retry_count >= _retries)) { up_i2creset(_dev); } } while (retry_count++ < _retries); return ret; }
struct i2c_msg_s msgv[2];
struct i2c_msg_s { uint16_t addr; /* Slave address */ uint16_t flags; /* See I2C_M_* definitions */ uint8_t *buffer; int length; };ret = I2C_TRANSFER(_dev, &msgv[0], msgs);找I2C_TRANSFER函数
#define I2C_TRANSFER(d,m,c) ((d)->ops->transfer(d,m,c)),找transfer函数
已无能为力了,找不下去了。好吧到此断了,找不到硬件层了,数据还没读回来
继续看collect()
int PX4FLOW::collect() { int ret = -EIO; /* read from the sensor */ uint8_t val[I2C_FRAME_SIZE + I2C_INTEGRAL_FRAME_SIZE] = { 0 }; perf_begin(_sample_perf); if (PX4FLOW_REG == 0x00) { ret = transfer(nullptr, 0, &val[0], I2C_FRAME_SIZE + I2C_INTEGRAL_FRAME_SIZE); } if (PX4FLOW_REG == 0x16) { ret = transfer(nullptr, 0, &val[0], I2C_INTEGRAL_FRAME_SIZE); } if (ret < 0) { DEVICE_DEBUG("error reading from sensor: %d", ret); perf_count(_comms_errors); perf_end(_sample_perf); return ret; } if (PX4FLOW_REG == 0) { memcpy(&f, val, I2C_FRAME_SIZE); memcpy(&f_integral, &(val[I2C_FRAME_SIZE]), I2C_INTEGRAL_FRAME_SIZE); } if (PX4FLOW_REG == 0x16) { memcpy(&f_integral, val, I2C_INTEGRAL_FRAME_SIZE); } struct optical_flow_s report; report.timestamp = hrt_absolute_time(); report.pixel_flow_x_integral = static_cast<float>(f_integral.pixel_flow_x_integral) / 10000.0f;//convert to radians report.pixel_flow_y_integral = static_cast<float>(f_integral.pixel_flow_y_integral) / 10000.0f;//convert to radians report.frame_count_since_last_readout = f_integral.frame_count_since_last_readout; report.ground_distance_m = static_cast<float>(f_integral.ground_distance) / 1000.0f;//convert to meters report.quality = f_integral.qual; //0:bad ; 255 max quality report.gyro_x_rate_integral = static_cast<float>(f_integral.gyro_x_rate_integral) / 10000.0f; //convert to radians report.gyro_y_rate_integral = static_cast<float>(f_integral.gyro_y_rate_integral) / 10000.0f; //convert to radians report.gyro_z_rate_integral = static_cast<float>(f_integral.gyro_z_rate_integral) / 10000.0f; //convert to radians report.integration_timespan = f_integral.integration_timespan; //microseconds report.time_since_last_sonar_update = f_integral.sonar_timestamp;//microseconds report.gyro_temperature = f_integral.gyro_temperature;//Temperature * 100 in centi-degrees Celsius report.sensor_id = 0; /* rotate measurements according to parameter */根据参数测量旋转 float zeroval = 0.0f; rotate_3f(_sensor_rotation, report.pixel_flow_x_integral, report.pixel_flow_y_integral, zeroval); rotate_3f(_sensor_rotation, report.gyro_x_rate_integral, report.gyro_y_rate_integral, report.gyro_z_rate_integral); if (_px4flow_topic == nullptr) { _px4flow_topic = orb_advertise(ORB_ID(optical_flow), &report); } else { /* publish it */ orb_publish(ORB_ID(optical_flow), _px4flow_topic, &report); } /* publish to the distance_sensor topic as well */将distance_sensor主题发布出去 struct distance_sensor_s distance_report; distance_report.timestamp = report.timestamp; distance_report.min_distance = PX4FLOW_MIN_DISTANCE; distance_report.max_distance = PX4FLOW_MAX_DISTANCE; distance_report.current_distance = report.ground_distance_m; distance_report.covariance = 0.0f; distance_report.type = distance_sensor_s::MAV_DISTANCE_SENSOR_ULTRASOUND; /* TODO: the ID needs to be properly set */ distance_report.id = 0; distance_report.orientation = 8; orb_publish(ORB_ID(distance_sensor), _distance_sensor_topic, &distance_report); /* post a report to the ring */发布到环的报告 if (_reports->force(&report)) { perf_count(_buffer_overflows); } /* notify anyone waiting for data */通知别人等待数据 poll_notify(POLLIN); ret = OK; perf_end(_sample_perf); return ret; }
注意看
if (PX4FLOW_REG == 0x00) { ret = transfer(nullptr, 0, &val[0], I2C_FRAME_SIZE + I2C_INTEGRAL_FRAME_SIZE); } if (PX4FLOW_REG == 0x16) { ret = transfer(nullptr, 0, &val[0], I2C_INTEGRAL_FRAME_SIZE); }
这个是把数据读回
意思就是measure()是写指令操作,collect()是读传回来的数据,有一点奇怪PX4FLOW_REG什么时候会赋值为0x00
memcpy(&f, val, I2C_FRAME_SIZE);
memcpy(&f_integral, &(val[I2C_FRAME_SIZE]), I2C_INTEGRAL_FRAME_SIZE);
这2句是讲读取的数据存到f和f_integral结构体中
struct i2c_frame f; struct i2c_integral_frame f_integral; typedef struct i2c_frame { uint16_t frame_count; int16_t pixel_flow_x_sum; int16_t pixel_flow_y_sum; int16_t flow_comp_m_x; int16_t flow_comp_m_y; int16_t qual; int16_t gyro_x_rate; int16_t gyro_y_rate; int16_t gyro_z_rate; uint8_t gyro_range; uint8_t sonar_timestamp; int16_t ground_distance; } i2c_frame; #define I2C_FRAME_SIZE (sizeof(i2c_frame)) typedef struct i2c_integral_frame { uint16_t frame_count_since_last_readout; int16_t pixel_flow_x_integral; int16_t pixel_flow_y_integral; int16_t gyro_x_rate_integral; int16_t gyro_y_rate_integral; int16_t gyro_z_rate_integral; uint32_t integration_timespan; uint32_t sonar_timestamp; uint16_t ground_distance; int16_t gyro_temperature; uint8_t qual; } i2c_integral_frame;接着optical_flow_s report用来存储处理后的数据,之后控制用的数据就是来自这里
再换个地方,看read数据之后如何用于控制的
全局搜索ORB_ID(optical_flow),看哪个地方订阅了光流消息
有2个地方orb_copy(ORB_ID(optical_flow), optical_flow_sub, &flow);,再根据rc.mc_apps
默认采用的是
姿态估计 Attitude_estimator_q
位置估计 position_estimator_inav位置控制 mc_pos_control
EKF2暂时先不考虑
接下来要看position_estimator_inav _main.cpp(从常识也知道光流是用来获取空间xy方向的位置信息,所以之后肯定是用于位置估计上)
那么先宏观上分析下这个 position_estimator_inav _main.cpp#include <px4_posix.h> #include <unistd.h> #include <stdlib.h> #include <stdio.h> #include <stdbool.h> #include <fcntl.h> #include <string.h> #include <px4_config.h> #include <math.h> #include <float.h> #include <uORB/uORB.h> #include <uORB/topics/parameter_update.h> #include <uORB/topics/actuator_controls.h> #include <uORB/topics/actuator_controls_0.h> #include <uORB/topics/actuator_controls_1.h> #include <uORB/topics/actuator_controls_2.h> #include <uORB/topics/actuator_controls_3.h> #include <uORB/topics/actuator_armed.h> #include <uORB/topics/sensor_combined.h> #include <uORB/topics/vehicle_attitude.h> #include <uORB/topics/vehicle_local_position.h> #include <uORB/topics/vehicle_global_position.h> #include <uORB/topics/vehicle_gps_position.h> #include <uORB/topics/vision_position_estimate.h> #include <uORB/topics/att_pos_mocap.h> #include <uORB/topics/optical_flow.h> #include <uORB/topics/distance_sensor.h> #include <poll.h> #include <systemlib/err.h> #include <systemlib/mavlink_log.h> #include <geo/geo.h> #include <systemlib/systemlib.h> #include <drivers/drv_hrt.h> #include <platforms/px4_defines.h> #include <terrain_estimation/terrain_estimator.h> #include "position_estimator_inav_params.h" #include "inertial_filter.h"添加了这么多头文件,大概知道会用于控制、传感器融合、位置定位、gps也用上了、光流、惯性导航滤波等
extern "C" __EXPORT int position_estimator_inav_main(int argc, char *argv[]);开始工作了,进入 main
这里顺便猜想下,每个这样的功能函数或者文件,都有extern "C" __EXPORT int position_estimator_inav_main(int argc, char *argv[]);,是不是在哪个地方直接调用了,所以启动了该项功能,是不是在rcS中呢?估计有个总的流程,分别调用启动这些功能“函数”
/**************************************************************************** * main ****************************************************************************/ int position_estimator_inav_thread_main(int argc, char *argv[]) { /**************************类似于初始化或者声明******************************/ orb_advert_t mavlink_log_pub = nullptr; float x_est[2] = { 0.0f, 0.0f }; // pos, vel float y_est[2] = { 0.0f, 0.0f }; // pos, vel float z_est[2] = { 0.0f, 0.0f }; // pos, vel float est_buf[EST_BUF_SIZE][3][2]; // estimated position buffer float R_buf[EST_BUF_SIZE][3][3]; // rotation matrix buffer float R_gps[3][3]; // rotation matrix for GPS correction moment memset(est_buf, 0, sizeof(est_buf)); memset(R_buf, 0, sizeof(R_buf)); memset(R_gps, 0, sizeof(R_gps)); int buf_ptr = 0; static const float min_eph_epv = 2.0f; // min EPH/EPV, used for weight calculation static const float max_eph_epv = 20.0f; // max EPH/EPV acceptable for estimation float eph = max_eph_epv; float epv = 1.0f; float eph_flow = 1.0f; float eph_vision = 0.2f; float epv_vision = 0.2f; float eph_mocap = 0.05f; float epv_mocap = 0.05f; float x_est_prev[2], y_est_prev[2], z_est_prev[2]; memset(x_est_prev, 0, sizeof(x_est_prev)); memset(y_est_prev, 0, sizeof(y_est_prev)); memset(z_est_prev, 0, sizeof(z_est_prev)); int baro_init_cnt = 0; int baro_init_num = 200; float baro_offset = 0.0f; // baro offset for reference altitude, initialized on start, then adjusted hrt_abstime accel_timestamp = 0; hrt_abstime baro_timestamp = 0; bool ref_inited = false; hrt_abstime ref_init_start = 0; const hrt_abstime ref_init_delay = 1000000; // wait for 1s after 3D fix struct map_projection_reference_s ref; memset(&ref, 0, sizeof(ref)); uint16_t accel_updates = 0; uint16_t baro_updates = 0; uint16_t gps_updates = 0; uint16_t attitude_updates = 0; uint16_t flow_updates = 0; uint16_t vision_updates = 0; uint16_t mocap_updates = 0; hrt_abstime updates_counter_start = hrt_absolute_time(); hrt_abstime pub_last = hrt_absolute_time(); hrt_abstime t_prev = 0; /* store error when sensor updates, but correct on each time step to avoid jumps in estimated value */ float acc[] = { 0.0f, 0.0f, 0.0f }; // N E D float acc_bias[] = { 0.0f, 0.0f, 0.0f }; // body frame float corr_baro = 0.0f; // D float corr_gps[3][2] = { { 0.0f, 0.0f }, // N (pos, vel) { 0.0f, 0.0f }, // E (pos, vel) { 0.0f, 0.0f }, // D (pos, vel) }; float w_gps_xy = 1.0f; float w_gps_z = 1.0f; float corr_vision[3][2] = { { 0.0f, 0.0f }, // N (pos, vel) { 0.0f, 0.0f }, // E (pos, vel) { 0.0f, 0.0f }, // D (pos, vel) }; float corr_mocap[3][1] = { { 0.0f }, // N (pos) { 0.0f }, // E (pos) { 0.0f }, // D (pos) }; const int mocap_heading = 2; float dist_ground = 0.0f; //variables for lidar altitude estimation float corr_lidar = 0.0f; float lidar_offset = 0.0f; int lidar_offset_count = 0; bool lidar_first = true; bool use_lidar = false; bool use_lidar_prev = false; float corr_flow[] = { 0.0f, 0.0f }; // N E float w_flow = 0.0f; hrt_abstime lidar_time = 0; // time of last lidar measurement (not filtered) hrt_abstime lidar_valid_time = 0; // time of last lidar measurement used for correction (filtered) int n_flow = 0; float gyro_offset_filtered[] = { 0.0f, 0.0f, 0.0f }; float flow_gyrospeed[] = { 0.0f, 0.0f, 0.0f }; float flow_gyrospeed_filtered[] = { 0.0f, 0.0f, 0.0f }; float att_gyrospeed_filtered[] = { 0.0f, 0.0f, 0.0f }; float yaw_comp[] = { 0.0f, 0.0f }; hrt_abstime flow_time = 0; float flow_min_dist = 0.2f; bool gps_valid = false; // GPS is valid bool lidar_valid = false; // lidar is valid bool flow_valid = false; // flow is valid bool flow_accurate = false; // flow should be accurate (this flag not updated if flow_valid == false) bool vision_valid = false; // vision is valid bool mocap_valid = false; // mocap is valid /* declare and safely initialize all structs */ struct actuator_controls_s actuator; memset(&actuator, 0, sizeof(actuator)); struct actuator_armed_s armed; memset(&armed, 0, sizeof(armed)); struct sensor_combined_s sensor; memset(&sensor, 0, sizeof(sensor)); struct vehicle_gps_position_s gps; memset(&gps, 0, sizeof(gps)); struct vehicle_attitude_s att; memset(&att, 0, sizeof(att)); struct vehicle_local_position_s local_pos; memset(&local_pos, 0, sizeof(local_pos)); struct optical_flow_s flow; memset(&flow, 0, sizeof(flow)); struct vision_position_estimate_s vision; memset(&vision, 0, sizeof(vision)); struct att_pos_mocap_s mocap; memset(&mocap, 0, sizeof(mocap)); struct vehicle_global_position_s global_pos; memset(&global_pos, 0, sizeof(global_pos)); struct distance_sensor_s lidar; memset(&lidar, 0, sizeof(lidar)); struct vehicle_rates_setpoint_s rates_setpoint; memset(&rates_setpoint, 0, sizeof(rates_setpoint)); /**************************订阅各种信息******************************/ /* subscribe */ int parameter_update_sub = orb_subscribe(ORB_ID(parameter_update)); int actuator_sub = orb_subscribe(ORB_ID_VEHICLE_ATTITUDE_CONTROLS); int armed_sub = orb_subscribe(ORB_ID(actuator_armed)); int sensor_combined_sub = orb_subscribe(ORB_ID(sensor_combined)); int vehicle_attitude_sub = orb_subscribe(ORB_ID(vehicle_attitude)); int optical_flow_sub = orb_subscribe(ORB_ID(optical_flow)); int vehicle_gps_position_sub = orb_subscribe(ORB_ID(vehicle_gps_position)); int vision_position_estimate_sub = orb_subscribe(ORB_ID(vision_position_estimate)); int att_pos_mocap_sub = orb_subscribe(ORB_ID(att_pos_mocap)); int distance_sensor_sub = orb_subscribe(ORB_ID(distance_sensor)); int vehicle_rate_sp_sub = orb_subscribe(ORB_ID(vehicle_rates_setpoint)); /**************************发布位置信息******************************/ /* advertise */ orb_advert_t vehicle_local_position_pub = orb_advertise(ORB_ID(vehicle_local_position), &local_pos); orb_advert_t vehicle_global_position_pub = NULL; /**************************类似于初始化params******************************/ struct position_estimator_inav_params params; memset(¶ms, 0, sizeof(params)); struct position_estimator_inav_param_handles pos_inav_param_handles; /* initialize parameter handles */ inav_parameters_init(&pos_inav_param_handles); /* first parameters read at start up */ struct parameter_update_s param_update; orb_copy(ORB_ID(parameter_update), parameter_update_sub, ¶m_update); /* read from param topic to clear updated flag */ /* first parameters update */ inav_parameters_update(&pos_inav_param_handles, ¶ms); /**************************sensor_combined******************************/ px4_pollfd_struct_t fds_init[1] = {}; fds_init[0].fd = sensor_combined_sub; fds_init[0].events = POLLIN; ////////////////////////气压计/////////////////////////////////// /* wait for initial baro value */ bool wait_baro = true; TerrainEstimator *terrain_estimator = new TerrainEstimator(); thread_running = true; hrt_abstime baro_wait_for_sample_time = hrt_absolute_time(); while (wait_baro && !thread_should_exit) { int ret = px4_poll(&fds_init[0], 1, 1000); if (ret < 0) { /* poll error */ mavlink_log_info(&mavlink_log_pub, "[inav] poll error on init"); } else if (hrt_absolute_time() - baro_wait_for_sample_time > MAX_WAIT_FOR_BARO_SAMPLE) { wait_baro = false; mavlink_log_info(&mavlink_log_pub, "[inav] timed out waiting for a baro sample"); } else if (ret > 0) { if (fds_init[0].revents & POLLIN) { orb_copy(ORB_ID(sensor_combined), sensor_combined_sub, &sensor); if (wait_baro && sensor.baro_timestamp[0] != baro_timestamp) { baro_timestamp = sensor.baro_timestamp[0]; baro_wait_for_sample_time = hrt_absolute_time(); /* mean calculation over several measurements */ if (baro_init_cnt < baro_init_num) { if (PX4_ISFINITE(sensor.baro_alt_meter[0])) { baro_offset += sensor.baro_alt_meter[0]; baro_init_cnt++; } } else { wait_baro = false; baro_offset /= (float) baro_init_cnt; local_pos.z_valid = true; local_pos.v_z_valid = true; } } } } else { PX4_WARN("INAV poll timeout"); } } /**************************主循环******************************/ /* main loop */ px4_pollfd_struct_t fds[1]; fds[0].fd = vehicle_attitude_sub; fds[0].events = POLLIN; ////////////////////////判断是否应该退出该线程/////////////////////////////////// while (!thread_should_exit) { //这个判断条件经常使用,到处都用过类似的 int ret = px4_poll(fds, 1, 20); // wait maximal 20 ms = 50 Hz minimum rate hrt_abstime t = hrt_absolute_time(); if (ret < 0) { /* poll error */ mavlink_log_info(&mavlink_log_pub, "[inav] poll error on init"); continue; } else if (ret > 0) { /* act on attitude updates */ /* vehicle attitude */ orb_copy(ORB_ID(vehicle_attitude), vehicle_attitude_sub, &att);//姿态跟新 attitude_updates++; bool updated; /* parameter update */ orb_check(parameter_update_sub, &updated); if (updated) { struct parameter_update_s update; orb_copy(ORB_ID(parameter_update), parameter_update_sub, &update);//parameter跟新 inav_parameters_update(&pos_inav_param_handles, ¶ms); } /* actuator */ orb_check(actuator_sub, &updated); if (updated) { orb_copy(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, actuator_sub, &actuator);//执行器数据跟新(这里并不是pwm) } /* armed */ orb_check(armed_sub, &updated); if (updated) { orb_copy(ORB_ID(actuator_armed), armed_sub, &armed);//解锁数据跟新 } /* sensor combined */ orb_check(sensor_combined_sub, &updated); if (updated) { orb_copy(ORB_ID(sensor_combined), sensor_combined_sub, &sensor);//传感器融合数据跟新 if (sensor.accelerometer_timestamp[0] != accel_timestamp) { if (att.R_valid) { /* correct accel bias */ sensor.accelerometer_m_s2[0] -= acc_bias[0]; sensor.accelerometer_m_s2[1] -= acc_bias[1]; sensor.accelerometer_m_s2[2] -= acc_bias[2]; /* transform acceleration vector from body frame to NED frame */ for (int i = 0; i < 3; i++) { acc[i] = 0.0f; for (int j = 0; j < 3; j++) { acc[i] += PX4_R(att.R, i, j) * sensor.accelerometer_m_s2[j]; } } acc[2] += CONSTANTS_ONE_G; } else { memset(acc, 0, sizeof(acc)); } accel_timestamp = sensor.accelerometer_timestamp[0]; accel_updates++; } if (sensor.baro_timestamp[0] != baro_timestamp) { corr_baro = baro_offset - sensor.baro_alt_meter[0] - z_est[0]; baro_timestamp = sensor.baro_timestamp[0]; baro_updates++; } } /* lidar alt estimation */ orb_check(distance_sensor_sub, &updated); /* update lidar separately, needed by terrain estimator */ if (updated) { orb_copy(ORB_ID(distance_sensor), distance_sensor_sub, &lidar);//高度数据跟新 lidar.current_distance += params.lidar_calibration_offset; } if (updated) { //check if altitude estimation for lidar is enabled and new sensor data if (params.enable_lidar_alt_est && lidar.current_distance > lidar.min_distance && lidar.current_distance < lidar.max_distance && (PX4_R(att.R, 2, 2) > 0.7f)) { if (!use_lidar_prev && use_lidar) { lidar_first = true; } use_lidar_prev = use_lidar; lidar_time = t; dist_ground = lidar.current_distance * PX4_R(att.R, 2, 2); //vertical distance if (lidar_first) { lidar_first = false; lidar_offset = dist_ground + z_est[0]; mavlink_log_info(&mavlink_log_pub, "[inav] LIDAR: new ground offset"); warnx("[inav] LIDAR: new ground offset"); } corr_lidar = lidar_offset - dist_ground - z_est[0]; if (fabsf(corr_lidar) > params.lidar_err) { //check for spike corr_lidar = 0; lidar_valid = false; lidar_offset_count++; if (lidar_offset_count > 3) { //if consecutive bigger/smaller measurements -> new ground offset -> reinit lidar_first = true; lidar_offset_count = 0; } } else { corr_lidar = lidar_offset - dist_ground - z_est[0]; lidar_valid = true; lidar_offset_count = 0; lidar_valid_time = t; } } else { lidar_valid = false; } } /**************************************这里就是光流的处理*********************************/ /* optical flow */ orb_check(optical_flow_sub, &updated); if (updated && lidar_valid) { orb_copy(ORB_ID(optical_flow), optical_flow_sub, &flow);//光流数据跟新 flow_time = t; float flow_q = flow.quality / 255.0f; float dist_bottom = lidar.current_distance;//高度信息 if (dist_bottom > flow_min_dist && flow_q > params.flow_q_min && PX4_R(att.R, 2, 2) > 0.7f) { /* distance to surface */ //float flow_dist = dist_bottom / PX4_R(att.R, 2, 2); //use this if using sonar float flow_dist = dist_bottom; //use this if using lidar /* check if flow if too large for accurate measurements */ /* calculate estimated velocity in body frame */ float body_v_est[2] = { 0.0f, 0.0f }; for (int i = 0; i < 2; i++) { body_v_est[i] = PX4_R(att.R, 0, i) * x_est[1] + PX4_R(att.R, 1, i) * y_est[1] + PX4_R(att.R, 2, i) * z_est[1]; } /* set this flag if flow should be accurate according to current velocity and attitude rate estimate */ flow_accurate = fabsf(body_v_est[1] / flow_dist - att.rollspeed) < max_flow && fabsf(body_v_est[0] / flow_dist + att.pitchspeed) < max_flow; /*calculate offset of flow-gyro using already calibrated gyro from autopilot*/ flow_gyrospeed[0] = flow.gyro_x_rate_integral / (float)flow.integration_timespan * 1000000.0f; flow_gyrospeed[1] = flow.gyro_y_rate_integral / (float)flow.integration_timespan * 1000000.0f; flow_gyrospeed[2] = flow.gyro_z_rate_integral / (float)flow.integration_timespan * 1000000.0f; //moving average if (n_flow >= 100) { gyro_offset_filtered[0] = flow_gyrospeed_filtered[0] - att_gyrospeed_filtered[0]; gyro_offset_filtered[1] = flow_gyrospeed_filtered[1] - att_gyrospeed_filtered[1]; gyro_offset_filtered[2] = flow_gyrospeed_filtered[2] - att_gyrospeed_filtered[2]; n_flow = 0; flow_gyrospeed_filtered[0] = 0.0f; flow_gyrospeed_filtered[1] = 0.0f; flow_gyrospeed_filtered[2] = 0.0f; att_gyrospeed_filtered[0] = 0.0f; att_gyrospeed_filtered[1] = 0.0f; att_gyrospeed_filtered[2] = 0.0f; } else { flow_gyrospeed_filtered[0] = (flow_gyrospeed[0] + n_flow * flow_gyrospeed_filtered[0]) / (n_flow + 1); flow_gyrospeed_filtered[1] = (flow_gyrospeed[1] + n_flow * flow_gyrospeed_filtered[1]) / (n_flow + 1); flow_gyrospeed_filtered[2] = (flow_gyrospeed[2] + n_flow * flow_gyrospeed_filtered[2]) / (n_flow + 1); att_gyrospeed_filtered[0] = (att.pitchspeed + n_flow * att_gyrospeed_filtered[0]) / (n_flow + 1); att_gyrospeed_filtered[1] = (att.rollspeed + n_flow * att_gyrospeed_filtered[1]) / (n_flow + 1); att_gyrospeed_filtered[2] = (att.yawspeed + n_flow * att_gyrospeed_filtered[2]) / (n_flow + 1); n_flow++; } /*yaw compensation (flow sensor is not in center of rotation) -> params in QGC*/ yaw_comp[0] = - params.flow_module_offset_y * (flow_gyrospeed[2] - gyro_offset_filtered[2]); yaw_comp[1] = params.flow_module_offset_x * (flow_gyrospeed[2] - gyro_offset_filtered[2]); /* convert raw flow to angular flow (rad/s) */ float flow_ang[2]; /* check for vehicle rates setpoint - it below threshold -> dont subtract -> better hover */ orb_check(vehicle_rate_sp_sub, &updated); if (updated) orb_copy(ORB_ID(vehicle_rates_setpoint), vehicle_rate_sp_sub, &rates_setpoint); double rate_threshold = 0.15f; if (fabs(rates_setpoint.pitch) < rate_threshold) { //warnx("[inav] test ohne comp"); flow_ang[0] = (flow.pixel_flow_x_integral / (float)flow.integration_timespan * 1000000.0f) * params.flow_k;//for now the flow has to be scaled (to small) } else { //warnx("[inav] test mit comp"); //calculate flow [rad/s] and compensate for rotations (and offset of flow-gyro) flow_ang[0] = ((flow.pixel_flow_x_integral - flow.gyro_x_rate_integral) / (float)flow.integration_timespan * 1000000.0f + gyro_offset_filtered[0]) * params.flow_k;//for now the flow has to be scaled (to small) } if (fabs(rates_setpoint.roll) < rate_threshold) { flow_ang[1] = (flow.pixel_flow_y_integral / (float)flow.integration_timespan * 1000000.0f) * params.flow_k;//for now the flow has to be scaled (to small) } else { //calculate flow [rad/s] and compensate for rotations (and offset of flow-gyro) flow_ang[1] = ((flow.pixel_flow_y_integral - flow.gyro_y_rate_integral) / (float)flow.integration_timespan * 1000000.0f + gyro_offset_filtered[1]) * params.flow_k;//for now the flow has to be scaled (to small) } /* flow measurements vector */ float flow_m[3]; if (fabs(rates_setpoint.yaw) < rate_threshold) { flow_m[0] = -flow_ang[0] * flow_dist; flow_m[1] = -flow_ang[1] * flow_dist; } else { flow_m[0] = -flow_ang[0] * flow_dist - yaw_comp[0] * params.flow_k; flow_m[1] = -flow_ang[1] * flow_dist - yaw_comp[1] * params.flow_k; } flow_m[2] = z_est[1]; /* velocity in NED */ float flow_v[2] = { 0.0f, 0.0f }; /* project measurements vector to NED basis, skip Z component */ for (int i = 0; i < 2; i++) { for (int j = 0; j < 3; j++) { flow_v[i] += PX4_R(att.R, i, j) * flow_m[j]; } } /* velocity correction */ corr_flow[0] = flow_v[0] - x_est[1]; corr_flow[1] = flow_v[1] - y_est[1]; /* adjust correction weight */ float flow_q_weight = (flow_q - params.flow_q_min) / (1.0f - params.flow_q_min); w_flow = PX4_R(att.R, 2, 2) * flow_q_weight / fmaxf(1.0f, flow_dist); /* if flow is not accurate, reduce weight for it */ // TODO make this more fuzzy if (!flow_accurate) { w_flow *= 0.05f; } /* under ideal conditions, on 1m distance assume EPH = 10cm */ eph_flow = 0.1f / w_flow; flow_valid = true; } else { w_flow = 0.0f; flow_valid = false; } flow_updates++; } /**************************************光流结束*********************************/ //////////////////////////////////视觉的处理//////////////////////////////////// /* check no vision circuit breaker is set */ if (params.no_vision != CBRK_NO_VISION_KEY) { /* vehicle vision position */ orb_check(vision_position_estimate_sub, &updated); if (updated) { orb_copy(ORB_ID(vision_position_estimate), vision_position_estimate_sub, &vision); static float last_vision_x = 0.0f; static float last_vision_y = 0.0f; static float last_vision_z = 0.0f; /* reset position estimate on first vision update */ if (!vision_valid) { x_est[0] = vision.x; x_est[1] = vision.vx; y_est[0] = vision.y; y_est[1] = vision.vy; /* only reset the z estimate if the z weight parameter is not zero */ if (params.w_z_vision_p > MIN_VALID_W) { z_est[0] = vision.z; z_est[1] = vision.vz; } vision_valid = true; last_vision_x = vision.x; last_vision_y = vision.y; last_vision_z = vision.z; warnx("VISION estimate valid"); mavlink_log_info(&mavlink_log_pub, "[inav] VISION estimate valid"); } /* calculate correction for position */ corr_vision[0][0] = vision.x - x_est[0]; corr_vision[1][0] = vision.y - y_est[0]; corr_vision[2][0] = vision.z - z_est[0]; static hrt_abstime last_vision_time = 0; float vision_dt = (vision.timestamp_boot - last_vision_time) / 1e6f; last_vision_time = vision.timestamp_boot; if (vision_dt > 0.000001f && vision_dt < 0.2f) { vision.vx = (vision.x - last_vision_x) / vision_dt; vision.vy = (vision.y - last_vision_y) / vision_dt; vision.vz = (vision.z - last_vision_z) / vision_dt; last_vision_x = vision.x; last_vision_y = vision.y; last_vision_z = vision.z; /* calculate correction for velocity */ corr_vision[0][1] = vision.vx - x_est[1]; corr_vision[1][1] = vision.vy - y_est[1]; corr_vision[2][1] = vision.vz - z_est[1]; } else { /* assume zero motion */ corr_vision[0][1] = 0.0f - x_est[1]; corr_vision[1][1] = 0.0f - y_est[1]; corr_vision[2][1] = 0.0f - z_est[1]; } vision_updates++; } } ///////////////////////////////////mocap数据处理/////////////////////////// /* vehicle mocap position */ orb_check(att_pos_mocap_sub, &updated); if (updated) { orb_copy(ORB_ID(att_pos_mocap), att_pos_mocap_sub, &mocap); if (!params.disable_mocap) { /* reset position estimate on first mocap update */ if (!mocap_valid) { x_est[0] = mocap.x; y_est[0] = mocap.y; z_est[0] = mocap.z; mocap_valid = true; warnx("MOCAP data valid"); mavlink_log_info(&mavlink_log_pub, "[inav] MOCAP data valid"); } /* calculate correction for position */ corr_mocap[0][0] = mocap.x - x_est[0]; corr_mocap[1][0] = mocap.y - y_est[0]; corr_mocap[2][0] = mocap.z - z_est[0]; mocap_updates++; } } //////////////////////////////////////GPS数据处理/////////////////////////////////////// /* vehicle GPS position */ orb_check(vehicle_gps_position_sub, &updated); if (updated) { orb_copy(ORB_ID(vehicle_gps_position), vehicle_gps_position_sub, &gps); bool reset_est = false; /* hysteresis for GPS quality */ if (gps_valid) { if (gps.eph > max_eph_epv || gps.epv > max_eph_epv || gps.fix_type < 3) { gps_valid = false; mavlink_log_info(&mavlink_log_pub, "[inav] GPS signal lost"); warnx("[inav] GPS signal lost"); } } else { if (gps.eph < max_eph_epv * 0.7f && gps.epv < max_eph_epv * 0.7f && gps.fix_type >= 3) { gps_valid = true; reset_est = true; mavlink_log_info(&mavlink_log_pub, "[inav] GPS signal found"); warnx("[inav] GPS signal found"); } } if (gps_valid) { double lat = gps.lat * 1e-7; double lon = gps.lon * 1e-7; float alt = gps.alt * 1e-3; /* initialize reference position if needed */ if (!ref_inited) { if (ref_init_start == 0) { ref_init_start = t; } else if (t > ref_init_start + ref_init_delay) { ref_inited = true; /* set position estimate to (0, 0, 0), use GPS velocity for XY */ x_est[0] = 0.0f; x_est[1] = gps.vel_n_m_s; y_est[0] = 0.0f; y_est[1] = gps.vel_e_m_s; local_pos.ref_lat = lat; local_pos.ref_lon = lon; local_pos.ref_alt = alt + z_est[0]; local_pos.ref_timestamp = t; /* initialize projection */ map_projection_init(&ref, lat, lon); // XXX replace this print warnx("init ref: lat=%.7f, lon=%.7f, alt=%8.4f", (double)lat, (double)lon, (double)alt); mavlink_log_info(&mavlink_log_pub, "[inav] init ref: %.7f, %.7f, %8.4f", (double)lat, (double)lon, (double)alt); } } if (ref_inited) { /* project GPS lat lon to plane */ float gps_proj[2]; map_projection_project(&ref, lat, lon, &(gps_proj[0]), &(gps_proj[1])); /* reset position estimate when GPS becomes good */ if (reset_est) { x_est[0] = gps_proj[0]; x_est[1] = gps.vel_n_m_s; y_est[0] = gps_proj[1]; y_est[1] = gps.vel_e_m_s; } /* calculate index of estimated values in buffer */ int est_i = buf_ptr - 1 - min(EST_BUF_SIZE - 1, max(0, (int)(params.delay_gps * 1000000.0f / PUB_INTERVAL))); if (est_i < 0) { est_i += EST_BUF_SIZE; } /* calculate correction for position */ corr_gps[0][0] = gps_proj[0] - est_buf[est_i][0][0]; corr_gps[1][0] = gps_proj[1] - est_buf[est_i][1][0]; corr_gps[2][0] = local_pos.ref_alt - alt - est_buf[est_i][2][0]; /* calculate correction for velocity */ if (gps.vel_ned_valid) { corr_gps[0][1] = gps.vel_n_m_s - est_buf[est_i][0][1]; corr_gps[1][1] = gps.vel_e_m_s - est_buf[est_i][1][1]; corr_gps[2][1] = gps.vel_d_m_s - est_buf[est_i][2][1]; } else { corr_gps[0][1] = 0.0f; corr_gps[1][1] = 0.0f; corr_gps[2][1] = 0.0f; } /* save rotation matrix at this moment */ memcpy(R_gps, R_buf[est_i], sizeof(R_gps)); w_gps_xy = min_eph_epv / fmaxf(min_eph_epv, gps.eph); w_gps_z = min_eph_epv / fmaxf(min_eph_epv, gps.epv); } } else { /* no GPS lock */ memset(corr_gps, 0, sizeof(corr_gps)); ref_init_start = 0; } gps_updates++; } } ///////////////////////////////检查是否timeout////////////////////////////////// /* check for timeout on FLOW topic */ if ((flow_valid || lidar_valid) && t > (flow_time + flow_topic_timeout)) { flow_valid = false; warnx("FLOW timeout"); mavlink_log_info(&mavlink_log_pub, "[inav] FLOW timeout"); } /* check for timeout on GPS topic */ if (gps_valid && (t > (gps.timestamp_position + gps_topic_timeout))) { gps_valid = false; warnx("GPS timeout"); mavlink_log_info(&mavlink_log_pub, "[inav] GPS timeout"); } /* check for timeout on vision topic */ if (vision_valid && (t > (vision.timestamp_boot + vision_topic_timeout))) { vision_valid = false; warnx("VISION timeout"); mavlink_log_info(&mavlink_log_pub, "[inav] VISION timeout"); } /* check for timeout on mocap topic */ if (mocap_valid && (t > (mocap.timestamp_boot + mocap_topic_timeout))) { mocap_valid = false; warnx("MOCAP timeout"); mavlink_log_info(&mavlink_log_pub, "[inav] MOCAP timeout"); } /* check for lidar measurement timeout */ if (lidar_valid && (t > (lidar_time + lidar_timeout))) { lidar_valid = false; warnx("LIDAR timeout"); mavlink_log_info(&mavlink_log_pub, "[inav] LIDAR timeout"); } float dt = t_prev > 0 ? (t - t_prev) / 1000000.0f : 0.0f; dt = fmaxf(fminf(0.02, dt), 0.0002); // constrain dt from 0.2 to 20 ms t_prev = t; /* increase EPH/EPV on each step */ if (eph < 0.000001f) { //get case where eph is 0 -> would stay 0 eph = 0.001; } if (eph < max_eph_epv) { eph *= 1.0f + dt; } if (epv < 0.000001f) { //get case where epv is 0 -> would stay 0 epv = 0.001; } if (epv < max_eph_epv) { epv += 0.005f * dt; // add 1m to EPV each 200s (baro drift) } //////////////////////////////////////根据具体情况,将决策赋给标志位////////////////////////////////////// /* use GPS if it's valid and reference position initialized */使用全球定位系统,如果它是有效的,并参考位置初始化 bool use_gps_xy = ref_inited && gps_valid && params.w_xy_gps_p > MIN_VALID_W; bool use_gps_z = ref_inited && gps_valid && params.w_z_gps_p > MIN_VALID_W; /* use VISION if it's valid and has a valid weight parameter */使用VISION,如果它是有效的,并具有有效的权重参数 bool use_vision_xy = vision_valid && params.w_xy_vision_p > MIN_VALID_W; bool use_vision_z = vision_valid && params.w_z_vision_p > MIN_VALID_W; /* use MOCAP if it's valid and has a valid weight parameter */使用MOCAP如果它是有效的,并具有有效的权重参数 bool use_mocap = mocap_valid && params.w_mocap_p > MIN_VALID_W && params.att_ext_hdg_m == mocap_heading; //check if external heading is mocap if (params.disable_mocap) { //disable mocap if fake gps is used use_mocap = false; } /* use flow if it's valid and (accurate or no GPS available) */ bool use_flow = flow_valid && (flow_accurate || !use_gps_xy); /* use LIDAR if it's valid and lidar altitude estimation is enabled */ use_lidar = lidar_valid && params.enable_lidar_alt_est; bool can_estimate_xy = (eph < max_eph_epv) || use_gps_xy || use_flow || use_vision_xy || use_mocap; bool dist_bottom_valid = (t < lidar_valid_time + lidar_valid_timeout); float w_xy_gps_p = params.w_xy_gps_p * w_gps_xy; float w_xy_gps_v = params.w_xy_gps_v * w_gps_xy; float w_z_gps_p = params.w_z_gps_p * w_gps_z; float w_z_gps_v = params.w_z_gps_v * w_gps_z; float w_xy_vision_p = params.w_xy_vision_p; float w_xy_vision_v = params.w_xy_vision_v; float w_z_vision_p = params.w_z_vision_p; float w_mocap_p = params.w_mocap_p; //////////////////////////////////根据之前的决策,开始校准数据处理/////////////////////////////////// /* reduce GPS weight if optical flow is good */ if (use_flow && flow_accurate) { w_xy_gps_p *= params.w_gps_flow; w_xy_gps_v *= params.w_gps_flow; } /* baro offset correction */ if (use_gps_z) { float offs_corr = corr_gps[2][0] * w_z_gps_p * dt; baro_offset += offs_corr; corr_baro += offs_corr; } /* accelerometer bias correction for GPS (use buffered rotation matrix) */ float accel_bias_corr[3] = { 0.0f, 0.0f, 0.0f }; if (use_gps_xy) { accel_bias_corr[0] -= corr_gps[0][0] * w_xy_gps_p * w_xy_gps_p; accel_bias_corr[0] -= corr_gps[0][1] * w_xy_gps_v; accel_bias_corr[1] -= corr_gps[1][0] * w_xy_gps_p * w_xy_gps_p; accel_bias_corr[1] -= corr_gps[1][1] * w_xy_gps_v; } if (use_gps_z) { accel_bias_corr[2] -= corr_gps[2][0] * w_z_gps_p * w_z_gps_p; accel_bias_corr[2] -= corr_gps[2][1] * w_z_gps_v; } /* transform error vector from NED frame to body frame */ for (int i = 0; i < 3; i++) { float c = 0.0f; for (int j = 0; j < 3; j++) { c += R_gps[j][i] * accel_bias_corr[j]; } if (PX4_ISFINITE(c)) { acc_bias[i] += c * params.w_acc_bias * dt; } } /* accelerometer bias correction for VISION (use buffered rotation matrix) */ accel_bias_corr[0] = 0.0f; accel_bias_corr[1] = 0.0f; accel_bias_corr[2] = 0.0f; if (use_vision_xy) { accel_bias_corr[0] -= corr_vision[0][0] * w_xy_vision_p * w_xy_vision_p; accel_bias_corr[0] -= corr_vision[0][1] * w_xy_vision_v; accel_bias_corr[1] -= corr_vision[1][0] * w_xy_vision_p * w_xy_vision_p; accel_bias_corr[1] -= corr_vision[1][1] * w_xy_vision_v; } if (use_vision_z) { accel_bias_corr[2] -= corr_vision[2][0] * w_z_vision_p * w_z_vision_p; } /* accelerometer bias correction for MOCAP (use buffered rotation matrix) */ accel_bias_corr[0] = 0.0f; accel_bias_corr[1] = 0.0f; accel_bias_corr[2] = 0.0f; if (use_mocap) { accel_bias_corr[0] -= corr_mocap[0][0] * w_mocap_p * w_mocap_p; accel_bias_corr[1] -= corr_mocap[1][0] * w_mocap_p * w_mocap_p; accel_bias_corr[2] -= corr_mocap[2][0] * w_mocap_p * w_mocap_p; } /* transform error vector from NED frame to body frame */ for (int i = 0; i < 3; i++) { float c = 0.0f; for (int j = 0; j < 3; j++) { c += PX4_R(att.R, j, i) * accel_bias_corr[j]; } if (PX4_ISFINITE(c)) { acc_bias[i] += c * params.w_acc_bias * dt; } } /* accelerometer bias correction for flow and baro (assume that there is no delay) */ accel_bias_corr[0] = 0.0f; accel_bias_corr[1] = 0.0f; accel_bias_corr[2] = 0.0f; if (use_flow) { accel_bias_corr[0] -= corr_flow[0] * params.w_xy_flow; accel_bias_corr[1] -= corr_flow[1] * params.w_xy_flow; } if (use_lidar) { accel_bias_corr[2] -= corr_lidar * params.w_z_lidar * params.w_z_lidar; } else { accel_bias_corr[2] -= corr_baro * params.w_z_baro * params.w_z_baro; } /* transform error vector from NED frame to body frame */ for (int i = 0; i < 3; i++) { float c = 0.0f; for (int j = 0; j < 3; j++) { c += PX4_R(att.R, j, i) * accel_bias_corr[j]; } if (PX4_ISFINITE(c)) { acc_bias[i] += c * params.w_acc_bias * dt; } } //////////////////////////////////滤波处理/////////////////////////////////// /* inertial filter prediction for altitude */ inertial_filter_predict(dt, z_est, acc[2]); if (!(PX4_ISFINITE(z_est[0]) && PX4_ISFINITE(z_est[1]))) { write_debug_log("BAD ESTIMATE AFTER Z PREDICTION", dt, x_est, y_est, z_est, x_est_prev, y_est_prev, z_est_prev, acc, corr_gps, w_xy_gps_p, w_xy_gps_v, corr_mocap, w_mocap_p, corr_vision, w_xy_vision_p, w_z_vision_p, w_xy_vision_v); memcpy(z_est, z_est_prev, sizeof(z_est)); } /* inertial filter correction for altitude */ if (use_lidar) { inertial_filter_correct(corr_lidar, dt, z_est, 0, params.w_z_lidar); } else { inertial_filter_correct(corr_baro, dt, z_est, 0, params.w_z_baro); } if (use_gps_z) { epv = fminf(epv, gps.epv); inertial_filter_correct(corr_gps[2][0], dt, z_est, 0, w_z_gps_p); inertial_filter_correct(corr_gps[2][1], dt, z_est, 1, w_z_gps_v); } if (use_vision_z) { epv = fminf(epv, epv_vision); inertial_filter_correct(corr_vision[2][0], dt, z_est, 0, w_z_vision_p); } if (use_mocap) { epv = fminf(epv, epv_mocap); inertial_filter_correct(corr_mocap[2][0], dt, z_est, 0, w_mocap_p); } if (!(PX4_ISFINITE(z_est[0]) && PX4_ISFINITE(z_est[1]))) { write_debug_log("BAD ESTIMATE AFTER Z CORRECTION", dt, x_est, y_est, z_est, x_est_prev, y_est_prev, z_est_prev, acc, corr_gps, w_xy_gps_p, w_xy_gps_v, corr_mocap, w_mocap_p, corr_vision, w_xy_vision_p, w_z_vision_p, w_xy_vision_v); memcpy(z_est, z_est_prev, sizeof(z_est)); memset(corr_gps, 0, sizeof(corr_gps)); memset(corr_vision, 0, sizeof(corr_vision)); memset(corr_mocap, 0, sizeof(corr_mocap)); corr_baro = 0; } else { memcpy(z_est_prev, z_est, sizeof(z_est)); } if (can_estimate_xy) { /* inertial filter prediction for position */ inertial_filter_predict(dt, x_est, acc[0]); inertial_filter_predict(dt, y_est, acc[1]); if (!(PX4_ISFINITE(x_est[0]) && PX4_ISFINITE(x_est[1]) && PX4_ISFINITE(y_est[0]) && PX4_ISFINITE(y_est[1]))) { write_debug_log("BAD ESTIMATE AFTER PREDICTION", dt, x_est, y_est, z_est, x_est_prev, y_est_prev, z_est_prev, acc, corr_gps, w_xy_gps_p, w_xy_gps_v, corr_mocap, w_mocap_p, corr_vision, w_xy_vision_p, w_z_vision_p, w_xy_vision_v); memcpy(x_est, x_est_prev, sizeof(x_est)); memcpy(y_est, y_est_prev, sizeof(y_est)); } /* inertial filter correction for position */ if (use_flow) { eph = fminf(eph, eph_flow); inertial_filter_correct(corr_flow[0], dt, x_est, 1, params.w_xy_flow * w_flow); inertial_filter_correct(corr_flow[1], dt, y_est, 1, params.w_xy_flow * w_flow); } if (use_gps_xy) { eph = fminf(eph, gps.eph); inertial_filter_correct(corr_gps[0][0], dt, x_est, 0, w_xy_gps_p); inertial_filter_correct(corr_gps[1][0], dt, y_est, 0, w_xy_gps_p); if (gps.vel_ned_valid && t < gps.timestamp_velocity + gps_topic_timeout) { inertial_filter_correct(corr_gps[0][1], dt, x_est, 1, w_xy_gps_v); inertial_filter_correct(corr_gps[1][1], dt, y_est, 1, w_xy_gps_v); } } if (use_vision_xy) { eph = fminf(eph, eph_vision); inertial_filter_correct(corr_vision[0][0], dt, x_est, 0, w_xy_vision_p); inertial_filter_correct(corr_vision[1][0], dt, y_est, 0, w_xy_vision_p); if (w_xy_vision_v > MIN_VALID_W) { inertial_filter_correct(corr_vision[0][1], dt, x_est, 1, w_xy_vision_v); inertial_filter_correct(corr_vision[1][1], dt, y_est, 1, w_xy_vision_v); } } if (use_mocap) { eph = fminf(eph, eph_mocap); inertial_filter_correct(corr_mocap[0][0], dt, x_est, 0, w_mocap_p); inertial_filter_correct(corr_mocap[1][0], dt, y_est, 0, w_mocap_p); } if (!(PX4_ISFINITE(x_est[0]) && PX4_ISFINITE(x_est[1]) && PX4_ISFINITE(y_est[0]) && PX4_ISFINITE(y_est[1]))) { write_debug_log("BAD ESTIMATE AFTER CORRECTION", dt, x_est, y_est, z_est, x_est_prev, y_est_prev, z_est_prev, acc, corr_gps, w_xy_gps_p, w_xy_gps_v, corr_mocap, w_mocap_p, corr_vision, w_xy_vision_p, w_z_vision_p, w_xy_vision_v); memcpy(x_est, x_est_prev, sizeof(x_est)); memcpy(y_est, y_est_prev, sizeof(y_est)); memset(corr_gps, 0, sizeof(corr_gps)); memset(corr_vision, 0, sizeof(corr_vision)); memset(corr_mocap, 0, sizeof(corr_mocap)); memset(corr_flow, 0, sizeof(corr_flow)); } else { memcpy(x_est_prev, x_est, sizeof(x_est)); memcpy(y_est_prev, y_est, sizeof(y_est)); } } else { /* gradually reset xy velocity estimates */ inertial_filter_correct(-x_est[1], dt, x_est, 1, params.w_xy_res_v); inertial_filter_correct(-y_est[1], dt, y_est, 1, params.w_xy_res_v); } ///////////////////////////////运行地形估计////////////////////////////////////// /* run terrain estimator */ terrain_estimator->predict(dt, &att, &sensor, &lidar); terrain_estimator->measurement_update(hrt_absolute_time(), &gps, &lidar, &att); if (inav_verbose_mode) { /* print updates rate */ if (t > updates_counter_start + updates_counter_len) { float updates_dt = (t - updates_counter_start) * 0.000001f; warnx( "updates rate: accelerometer = %.1f/s, baro = %.1f/s, gps = %.1f/s, attitude = %.1f/s, flow = %.1f/s, vision = %.1f/s, mocap = %.1f/s", (double)(accel_updates / updates_dt), (double)(baro_updates / updates_dt), (double)(gps_updates / updates_dt), (double)(attitude_updates / updates_dt), (double)(flow_updates / updates_dt), (double)(vision_updates / updates_dt), (double)(mocap_updates / updates_dt)); updates_counter_start = t; accel_updates = 0; baro_updates = 0; gps_updates = 0; attitude_updates = 0; flow_updates = 0; vision_updates = 0; mocap_updates = 0; } } if (t > pub_last + PUB_INTERVAL) { pub_last = t; /* push current estimate to buffer */ est_buf[buf_ptr][0][0] = x_est[0]; est_buf[buf_ptr][0][1] = x_est[1]; est_buf[buf_ptr][1][0] = y_est[0]; est_buf[buf_ptr][1][1] = y_est[1]; est_buf[buf_ptr][2][0] = z_est[0]; est_buf[buf_ptr][2][1] = z_est[1]; /* push current rotation matrix to buffer */ memcpy(R_buf[buf_ptr], att.R, sizeof(att.R)); buf_ptr++; if (buf_ptr >= EST_BUF_SIZE) { buf_ptr = 0; } ///////////////////////////////////////发布位置信息/////////////////////////////////// /* publish local position */ local_pos.xy_valid = can_estimate_xy; local_pos.v_xy_valid = can_estimate_xy; local_pos.xy_global = local_pos.xy_valid && use_gps_xy; local_pos.z_global = local_pos.z_valid && use_gps_z; local_pos.x = x_est[0]; local_pos.vx = x_est[1]; local_pos.y = y_est[0]; local_pos.vy = y_est[1]; local_pos.z = z_est[0]; local_pos.vz = z_est[1]; local_pos.yaw = att.yaw; local_pos.dist_bottom_valid = dist_bottom_valid; local_pos.eph = eph; local_pos.epv = epv; if (local_pos.dist_bottom_valid) { local_pos.dist_bottom = dist_ground; local_pos.dist_bottom_rate = - z_est[1]; } local_pos.timestamp = t; orb_publish(ORB_ID(vehicle_local_position), vehicle_local_position_pub, &local_pos); if (local_pos.xy_global && local_pos.z_global) { /* publish global position */ global_pos.timestamp = t; global_pos.time_utc_usec = gps.time_utc_usec; double est_lat, est_lon; map_projection_reproject(&ref, local_pos.x, local_pos.y, &est_lat, &est_lon); global_pos.lat = est_lat; global_pos.lon = est_lon; global_pos.alt = local_pos.ref_alt - local_pos.z; global_pos.vel_n = local_pos.vx; global_pos.vel_e = local_pos.vy; global_pos.vel_d = local_pos.vz; global_pos.yaw = local_pos.yaw; global_pos.eph = eph; global_pos.epv = epv; if (terrain_estimator->is_valid()) { global_pos.terrain_alt = global_pos.alt - terrain_estimator->get_distance_to_ground(); global_pos.terrain_alt_valid = true; } else { global_pos.terrain_alt_valid = false; } global_pos.pressure_alt = sensor.baro_alt_meter[0]; if (vehicle_global_position_pub == NULL) { vehicle_global_position_pub = orb_advertise(ORB_ID(vehicle_global_position), &global_pos); } else { orb_publish(ORB_ID(vehicle_global_position), vehicle_global_position_pub, &global_pos); } } } } /***************************************大循环结束********************************/ warnx("stopped"); mavlink_log_info(&mavlink_log_pub, "[inav] stopped"); thread_running = false; return 0; }
这里发布的信息有2个vehicle_local_position和vehicle_global_position
#ifdef __cplusplus struct __EXPORT vehicle_local_position_s { #else struct vehicle_local_position_s { #endif uint64_t timestamp; bool xy_valid; bool z_valid; bool v_xy_valid; bool v_z_valid; float x; float y; float z; float vx; float vy; float vz; float yaw; bool xy_global; bool z_global; uint64_t ref_timestamp; double ref_lat; double ref_lon; float ref_alt; float dist_bottom; float dist_bottom_rate; uint64_t surface_bottom_timestamp; bool dist_bottom_valid; float eph; float epv; #ifdef __cplusplus #endif };
#ifdef __cplusplus struct __EXPORT vehicle_global_position_s { #else struct vehicle_global_position_s { #endif uint64_t timestamp; uint64_t time_utc_usec; double lat; double lon; float alt; float vel_n; float vel_e; float vel_d; float yaw; float eph; float epv; float terrain_alt; bool terrain_alt_valid; bool dead_reckoning; float pressure_alt; #ifdef __cplusplus #endif };
其他会用到的数据全部在ardupilot/modules/PX4Firmware/src/modules/uORB/topics文件夹中
#ifdef __cplusplus struct __EXPORT sensor_combined_s { #else struct sensor_combined_s { #endif uint64_t timestamp; uint64_t gyro_timestamp[3]; int16_t gyro_raw[9]; float gyro_rad_s[9]; uint32_t gyro_priority[3]; float gyro_integral_rad[9]; uint64_t gyro_integral_dt[3]; uint32_t gyro_errcount[3]; float gyro_temp[3]; int16_t accelerometer_raw[9]; float accelerometer_m_s2[9]; float accelerometer_integral_m_s[9]; uint64_t accelerometer_integral_dt[3]; int16_t accelerometer_mode[3]; float accelerometer_range_m_s2[3]; uint64_t accelerometer_timestamp[3]; uint32_t accelerometer_priority[3]; uint32_t accelerometer_errcount[3]; float accelerometer_temp[3]; int16_t magnetometer_raw[9]; float magnetometer_ga[9]; int16_t magnetometer_mode[3]; float magnetometer_range_ga[3]; float magnetometer_cuttoff_freq_hz[3]; uint64_t magnetometer_timestamp[3]; uint32_t magnetometer_priority[3]; uint32_t magnetometer_errcount[3]; float magnetometer_temp[3]; float baro_pres_mbar[3]; float baro_alt_meter[3]; float baro_temp_celcius[3]; uint64_t baro_timestamp[3]; uint32_t baro_priority[3]; uint32_t baro_errcount[3]; float adc_voltage_v[10]; uint16_t adc_mapping[10]; float mcu_temp_celcius; float differential_pressure_pa[3]; uint64_t differential_pressure_timestamp[3]; float differential_pressure_filtered_pa[3]; uint32_t differential_pressure_priority[3]; uint32_t differential_pressure_errcount[3]; #ifdef __cplusplus static const int32_t MAGNETOMETER_MODE_NORMAL = 0; static const int32_t MAGNETOMETER_MODE_POSITIVE_BIAS = 1; static const int32_t MAGNETOMETER_MODE_NEGATIVE_BIAS = 2; static const uint32_t SENSOR_PRIO_MIN = 0; static const uint32_t SENSOR_PRIO_VERY_LOW = 25; static const uint32_t SENSOR_PRIO_LOW = 50; static const uint32_t SENSOR_PRIO_DEFAULT = 75; static const uint32_t SENSOR_PRIO_HIGH = 100; static const uint32_t SENSOR_PRIO_VERY_HIGH = 125; static const uint32_t SENSOR_PRIO_MAX = 255; #endif }; #ifdef __cplusplus struct __EXPORT parameter_update_s { #else struct parameter_update_s { #endif uint64_t timestamp; bool saved; #ifdef __cplusplus #endif }; #ifdef __cplusplus struct __EXPORT actuator_controls_s { #else struct actuator_controls_s { #endif uint64_t timestamp; uint64_t timestamp_sample; float control[8]; #ifdef __cplusplus static const uint8_t NUM_ACTUATOR_CONTROLS = 8; static const uint8_t NUM_ACTUATOR_CONTROL_GROUPS = 4; static const uint8_t INDEX_ROLL = 0; static const uint8_t INDEX_PITCH = 1; static const uint8_t INDEX_YAW = 2; static const uint8_t INDEX_THROTTLE = 3; static const uint8_t INDEX_FLAPS = 4; static const uint8_t INDEX_SPOILERS = 5; static const uint8_t INDEX_AIRBRAKES = 6; static const uint8_t INDEX_LANDING_GEAR = 7; static const uint8_t GROUP_INDEX_ATTITUDE = 0; static const uint8_t GROUP_INDEX_ATTITUDE_ALTERNATE = 1; #endif }; #ifdef __cplusplus struct __EXPORT actuator_armed_s { #else struct actuator_armed_s { #endif uint64_t timestamp; bool armed; bool prearmed; bool ready_to_arm; bool lockdown; bool force_failsafe; bool in_esc_calibration_mode; #ifdef __cplusplus #endif #ifdef __cplusplus struct __EXPORT vision_position_estimate_s { #else struct vision_position_estimate_s { #endif uint32_t id; uint64_t timestamp_boot; uint64_t timestamp_computer; float x; float y; float z; float vx; float vy; float vz; float q[4]; #ifdef __cplusplus #endif }; #ifdef __cplusplus struct __EXPORT att_pos_mocap_s { #else struct att_pos_mocap_s { #endif uint32_t id; uint64_t timestamp_boot; uint64_t timestamp_computer; float q[4]; float x; float y; float z; #ifdef __cplusplus #endif }; #ifdef __cplusplus struct __EXPORT vehicle_gps_position_s { #else struct vehicle_gps_position_s { #endif uint64_t timestamp_position; int32_t lat; int32_t lon; int32_t alt; int32_t alt_ellipsoid; uint64_t timestamp_variance; float s_variance_m_s; float c_variance_rad; uint8_t fix_type; float eph; float epv; float hdop; float vdop; int32_t noise_per_ms; int32_t jamming_indicator; uint64_t timestamp_velocity; float vel_m_s; float vel_n_m_s; float vel_e_m_s; float vel_d_m_s; float cog_rad; bool vel_ned_valid; uint64_t timestamp_time; uint64_t time_utc_usec; uint8_t satellites_used; #ifdef __cplusplus #endif };
把光流部分单独拿出来
/**************************************这里就是光流的处理*********************************/ /* optical flow */ orb_check(optical_flow_sub, &updated); if (updated && lidar_valid) { orb_copy(ORB_ID(optical_flow), optical_flow_sub, &flow);//光流数据跟新 flow_time = t; float flow_q = flow.quality / 255.0f; float dist_bottom = lidar.current_distance;//高度信息 if (dist_bottom > flow_min_dist && flow_q > params.flow_q_min && PX4_R(att.R, 2, 2) > 0.7f) { /* distance to surface */ //float flow_dist = dist_bottom / PX4_R(att.R, 2, 2); //use this if using sonar float flow_dist = dist_bottom; //use this if using lidar /* check if flow if too large for accurate measurements */检查速度是否太大,影响光流精确测量 /*****************************计算机架估计的速度*****************************/ /* calculate estimated velocity in body frame */ float body_v_est[2] = { 0.0f, 0.0f }; for (int i = 0; i < 2; i++) { body_v_est[i] = PX4_R(att.R, 0, i) * x_est[1] + PX4_R(att.R, 1, i) * y_est[1] + PX4_R(att.R, 2, i) * z_est[1]; } /*************设置这个标志,如果光流按照当前速度和姿态率的估计是准确的*************/ /* set this flag if flow should be accurate according to current velocity and attitude rate estimate */ flow_accurate = fabsf(body_v_est[1] / flow_dist - att.rollspeed) < max_flow && fabsf(body_v_est[0] / flow_dist + att.pitchspeed) < max_flow; /*************利用自动驾驶仪(飞控)已经校准的陀螺仪计算光流用的陀螺仪的偏移*************/ /*calculate offset of flow-gyro using already calibrated gyro from autopilot*/ flow_gyrospeed[0] = flow.gyro_x_rate_integral / (float)flow.integration_timespan * 1000000.0f; flow_gyrospeed[1] = flow.gyro_y_rate_integral / (float)flow.integration_timespan * 1000000.0f; flow_gyrospeed[2] = flow.gyro_z_rate_integral / (float)flow.integration_timespan * 1000000.0f; //moving average if (n_flow >= 100) { gyro_offset_filtered[0] = flow_gyrospeed_filtered[0] - att_gyrospeed_filtered[0]; gyro_offset_filtered[1] = flow_gyrospeed_filtered[1] - att_gyrospeed_filtered[1]; gyro_offset_filtered[2] = flow_gyrospeed_filtered[2] - att_gyrospeed_filtered[2]; n_flow = 0; flow_gyrospeed_filtered[0] = 0.0f; flow_gyrospeed_filtered[1] = 0.0f; flow_gyrospeed_filtered[2] = 0.0f; att_gyrospeed_filtered[0] = 0.0f; att_gyrospeed_filtered[1] = 0.0f; att_gyrospeed_filtered[2] = 0.0f; } else { flow_gyrospeed_filtered[0] = (flow_gyrospeed[0] + n_flow * flow_gyrospeed_filtered[0]) / (n_flow + 1); flow_gyrospeed_filtered[1] = (flow_gyrospeed[1] + n_flow * flow_gyrospeed_filtered[1]) / (n_flow + 1); flow_gyrospeed_filtered[2] = (flow_gyrospeed[2] + n_flow * flow_gyrospeed_filtered[2]) / (n_flow + 1); att_gyrospeed_filtered[0] = (att.pitchspeed + n_flow * att_gyrospeed_filtered[0]) / (n_flow + 1); att_gyrospeed_filtered[1] = (att.rollspeed + n_flow * att_gyrospeed_filtered[1]) / (n_flow + 1); att_gyrospeed_filtered[2] = (att.yawspeed + n_flow * att_gyrospeed_filtered[2]) / (n_flow + 1); n_flow++; } /*******************************偏航补偿(光流传感器不处于旋转中心)->参数在QGC中***********************/ /*yaw compensation (flow sensor is not in center of rotation) -> params in QGC*/ yaw_comp[0] = - params.flow_module_offset_y * (flow_gyrospeed[2] - gyro_offset_filtered[2]); yaw_comp[1] = params.flow_module_offset_x * (flow_gyrospeed[2] - gyro_offset_filtered[2]); /***************************将原始流转换成角度流**********************************/ /* convert raw flow to angular flow (rad/s) */ float flow_ang[2]; /***************************检查机体速度设定值->它低于阈值->不减去->更好悬停*********************/ /* check for vehicle rates setpoint - it below threshold -> dont subtract -> better hover */ orb_check(vehicle_rate_sp_sub, &updated); if (updated) orb_copy(ORB_ID(vehicle_rates_setpoint), vehicle_rate_sp_sub, &rates_setpoint); double rate_threshold = 0.15f; if (fabs(rates_setpoint.pitch) < rate_threshold) { //warnx("[inav] test ohne comp"); flow_ang[0] = (flow.pixel_flow_x_integral / (float)flow.integration_timespan * 1000000.0f) * params.flow_k;//for now the flow has to be scaled (to small) } else { //warnx("[inav] test mit comp"); //calculate flow [rad/s] and compensate for rotations (and offset of flow-gyro) //计算光流的速度[rad/s]并补偿旋转(和光流的陀螺仪的偏置) flow_ang[0] = ((flow.pixel_flow_x_integral - flow.gyro_x_rate_integral) / (float)flow.integration_timespan * 1000000.0f + gyro_offset_filtered[0]) * params.flow_k;//for now the flow has to be scaled (to small) } if (fabs(rates_setpoint.roll) < rate_threshold) { flow_ang[1] = (flow.pixel_flow_y_integral / (float)flow.integration_timespan * 1000000.0f) * params.flow_k;//for now the flow has to be scaled (to small) } else { //calculate flow [rad/s] and compensate for rotations (and offset of flow-gyro) //计算光流的速度[rad/s]并补偿旋转(和光流的陀螺仪的偏置) flow_ang[1] = ((flow.pixel_flow_y_integral - flow.gyro_y_rate_integral) / (float)flow.integration_timespan * 1000000.0f + gyro_offset_filtered[1]) * params.flow_k;//for now the flow has to be scaled (to small) } /************************光流测量向量********************************/ /* flow measurements vector */ float flow_m[3]; if (fabs(rates_setpoint.yaw) < rate_threshold) { flow_m[0] = -flow_ang[0] * flow_dist; flow_m[1] = -flow_ang[1] * flow_dist; } else { flow_m[0] = -flow_ang[0] * flow_dist - yaw_comp[0] * params.flow_k; flow_m[1] = -flow_ang[1] * flow_dist - yaw_comp[1] * params.flow_k; } flow_m[2] = z_est[1]; /* velocity in NED */NED是北东地,还有一种旋转方式是东北天,都是属于右手系 float flow_v[2] = { 0.0f, 0.0f }; /************************基于NED工程测量向量,跳过Z分量*********************/ /* project measurements vector to NED basis, skip Z component */ for (int i = 0; i < 2; i++) { for (int j = 0; j < 3; j++) { flow_v[i] += PX4_R(att.R, i, j) * flow_m[j]; } } /********************向量矫正***********************/ /* velocity correction */ corr_flow[0] = flow_v[0] - x_est[1]; corr_flow[1] = flow_v[1] - y_est[1]; /********************调整矫正权重***********************/ /* adjust correction weight */ float flow_q_weight = (flow_q - params.flow_q_min) / (1.0f - params.flow_q_min); w_flow = PX4_R(att.R, 2, 2) * flow_q_weight / fmaxf(1.0f, flow_dist); /********************如果光流是不准确的,那么减少他的权重***********************/ /* if flow is not accurate, reduce weight for it */ // TODO make this more fuzzy if (!flow_accurate) { w_flow *= 0.05f; } /**********************在理想条件下,在1m的距离EPH =10厘米****************/ /* under ideal conditions, on 1m distance assume EPH = 10cm */ eph_flow = 0.1f / w_flow; flow_valid = true; } else { w_flow = 0.0f; flow_valid = false; } flow_updates++; } /**************************************光流结束*********************************/接下来就到了 ardupilot/modules/PX4Firmware/src/modules/mc_pos_control/mc_pos_control_main.cpp
和ardupilot/modules/PX4Firmware/src/modules/vtol_att_control_main.cpp
进行控制
先看mc_pos_control_main.cpp
extern "C" __EXPORT int mc_pos_control_main(int argc, char *argv[]);
int mc_pos_control_main(int argc, char *argv[]) { if (argc < 2) { warnx("usage: mc_pos_control {start|stop|status}"); return 1; } if (!strcmp(argv[1], "start")) { if (pos_control::g_control != nullptr) { warnx("already running"); return 1; } pos_control::g_control = new MulticopterPositionControl; if (pos_control::g_control == nullptr) { warnx("alloc failed"); return 1; } if (OK != pos_control::g_control->start()) { delete pos_control::g_control; pos_control::g_control = nullptr; warnx("start failed"); return 1; } return 0; } if (!strcmp(argv[1], "stop")) { if (pos_control::g_control == nullptr) { warnx("not running"); return 1; } delete pos_control::g_control; pos_control::g_control = nullptr; return 0; } if (!strcmp(argv[1], "status")) { if (pos_control::g_control) { warnx("running"); return 0; } else { warnx("not running"); return 1; } } warnx("unrecognized command"); return 1; }这个里面就start和status需要看
int MulticopterPositionControl::start() { ASSERT(_control_task == -1); /* start the task */ _control_task = px4_task_spawn_cmd("mc_pos_control", SCHED_DEFAULT, SCHED_PRIORITY_MAX - 5, 1900, (px4_main_t)&MulticopterPositionControl::task_main_trampoline, nullptr); if (_control_task < 0) { warn("task start failed"); return -errno; } return OK; }
void MulticopterPositionControl::task_main_trampoline(int argc, char *argv[]) { pos_control::g_control->task_main(); }
void MulticopterPositionControl::task_main() { /***************************订阅各种信息***********************/ /* * do subscriptions */ _vehicle_status_sub = orb_subscribe(ORB_ID(vehicle_status)); _ctrl_state_sub = orb_subscribe(ORB_ID(control_state)); _att_sp_sub = orb_subscribe(ORB_ID(vehicle_attitude_setpoint)); _control_mode_sub = orb_subscribe(ORB_ID(vehicle_control_mode)); _params_sub = orb_subscribe(ORB_ID(parameter_update)); _manual_sub = orb_subscribe(ORB_ID(manual_control_setpoint)); _arming_sub = orb_subscribe(ORB_ID(actuator_armed)); _local_pos_sub = orb_subscribe(ORB_ID(vehicle_local_position)); //这个就是从position_estimator_inav_main.cpp中来的,里面包含光流数据 _pos_sp_triplet_sub = orb_subscribe(ORB_ID(position_setpoint_triplet)); _local_pos_sp_sub = orb_subscribe(ORB_ID(vehicle_local_position_setpoint)); _global_vel_sp_sub = orb_subscribe(ORB_ID(vehicle_global_velocity_setpoint)); parameters_update(true);//参数跟新 /*****************初始化关键结构的值,直到第一次定期更新*****************/ /* initialize values of critical structs until first regular update */ _arming.armed = false; /*****************初始跟新,获取所有传感器的数据和状态数据*****************/ /* get an initial update for all sensor and status data */ poll_subscriptions();//具体在下面展开了 bool reset_int_z = true; bool reset_int_z_manual = false; bool reset_int_xy = true; bool reset_yaw_sp = true; bool was_armed = false; hrt_abstime t_prev = 0; math::Vector<3> thrust_int; thrust_int.zero(); math::Matrix<3, 3> R; R.identity(); /* wakeup source */ px4_pollfd_struct_t fds[1]; fds[0].fd = _local_pos_sub; fds[0].events = POLLIN; /***********************大循环***************************/ while (!_task_should_exit) { /* wait for up to 500ms for data */等待500ms来获取数据 int pret = px4_poll(&fds[0], (sizeof(fds) / sizeof(fds[0])), 500); /* timed out - periodic check for _task_should_exit */ if (pret == 0) { continue; } /* this is undesirable but not much we can do */ if (pret < 0) { warn("poll error %d, %d", pret, errno); continue; } //////////////获取所有传感器的数据和状态数据////////////////////// poll_subscriptions(); ///////////////////////////参数跟新//////////////////////// parameters_update(false); hrt_abstime t = hrt_absolute_time(); float dt = t_prev != 0 ? (t - t_prev) * 0.000001f : 0.0f;//dt是控制周期吗? t_prev = t; // set dt for control blocks setDt(dt); /////////////////////////判断是否锁定//////////////////////////// if (_control_mode.flag_armed && !was_armed) { /* reset setpoints and integrals on arming */ _reset_pos_sp = true; _reset_alt_sp = true; _vel_sp_prev.zero(); reset_int_z = true; reset_int_xy = true; reset_yaw_sp = true; } //////////////////复位yaw和姿态的设定 为垂直起降 fw模式///////////////////// /* reset yaw and altitude setpoint for VTOL which are in fw mode */ if (_vehicle_status.is_vtol) { if (!_vehicle_status.is_rotary_wing) { reset_yaw_sp = true; _reset_alt_sp = true; } } ////////////////////跟新前一时刻锁定的状态/////////////////////// //Update previous arming state was_armed = _control_mode.flag_armed; update_ref();//跟新参考点,是不是期望目标?该函数在下面会展开 ////////////////////跟新速度导数(加速度) 独立于当前飞行模式/////////// /* Update velocity derivative, * independent of the current flight mode */ if (_local_pos.timestamp > 0) { if (PX4_ISFINITE(_local_pos.x) && //ISFINITE是测试数据是否是个有限数的函数 PX4_ISFINITE(_local_pos.y) && PX4_ISFINITE(_local_pos.z)) { _pos(0) = _local_pos.x; _pos(1) = _local_pos.y; _pos(2) = _local_pos.z; } if (PX4_ISFINITE(_local_pos.vx) && PX4_ISFINITE(_local_pos.vy) && PX4_ISFINITE(_local_pos.vz)) { _vel(0) = _local_pos.vx; _vel(1) = _local_pos.vy; _vel(2) = _local_pos.vz; } _vel_err_d(0) = _vel_x_deriv.update(-_vel(0)); _vel_err_d(1) = _vel_y_deriv.update(-_vel(1)); _vel_err_d(2) = _vel_z_deriv.update(-_vel(2)); } ///////////////复位在非手动模式下的水平垂直位置保持标志位,让位置和姿态不被控制////////////////////// // reset the horizontal and vertical position hold flags for non-manual modes // or if position / altitude is not controlled if (!_control_mode.flag_control_position_enabled || !_control_mode.flag_control_manual_enabled) { _pos_hold_engaged = false; } if (!_control_mode.flag_control_altitude_enabled || !_control_mode.flag_control_manual_enabled) { _alt_hold_engaged = false; } //////////////////根据控制模式选择对应的控制函数////////////////////// if (_control_mode.flag_control_altitude_enabled || _control_mode.flag_control_position_enabled || _control_mode.flag_control_climb_rate_enabled || _control_mode.flag_control_velocity_enabled) { _vel_ff.zero(); /////////默认运行位置和姿态控制,在control_*函数中可以失能这种模式而直接在循环中运行速度控制模式///////// /* by default, run position/altitude controller. the control_* functions * can disable this and run velocity controllers directly in this cycle */ _run_pos_control = true; _run_alt_control = true; /////////选择控制源///////////// /* select control source */ if (_control_mode.flag_control_manual_enabled) { /* manual control */ control_manual(dt);//手动控制,在下面会展开 _mode_auto = false; } else if (_control_mode.flag_control_offboard_enabled) { /* offboard control */ control_offboard(dt);//和control_manual(dt);貌似都是根据具体情况设定期望值 _mode_auto = false; } else { /* AUTO */ control_auto(dt);//自动控制 } /********感觉这3个函数功能是实现根据实际复杂情况调整期望值,之后用控制函数一个一个的达到期望值*************/ /* weather-vane mode for vtol: disable yaw control */风向标模式垂直起降:禁用偏航控制 if (!_control_mode.flag_control_manual_enabled && _pos_sp_triplet.current.disable_mc_yaw_control == true) { _att_sp.disable_mc_yaw_control = true; } else { /* reset in case of setpoint updates */ _att_sp.disable_mc_yaw_control = false; } /////////////////////////判断/////////////////////// if (!_control_mode.flag_control_manual_enabled && _pos_sp_triplet.current.valid && _pos_sp_triplet.current.type == position_setpoint_s::SETPOINT_TYPE_IDLE) { /* idle state, don't run controller and set zero thrust */ //////////空闲状态,不运行控制,并设置为零推力////////// R.identity(); memcpy(&_att_sp.R_body[0], R.data, sizeof(_att_sp.R_body)); _att_sp.R_valid = true; _att_sp.roll_body = 0.0f; _att_sp.pitch_body = 0.0f; _att_sp.yaw_body = _yaw; _att_sp.thrust = 0.0f; _att_sp.timestamp = hrt_absolute_time(); /////////////////////发布姿态设定值/////////////////////// /* publish attitude setpoint */ if (_att_sp_pub != nullptr) { orb_publish(_attitude_setpoint_id, _att_sp_pub, &_att_sp); } else if (_attitude_setpoint_id) { _att_sp_pub = orb_advertise(_attitude_setpoint_id, &_att_sp); } /////////////////////////另一个判断/////////////////////// } else if (_control_mode.flag_control_manual_enabled && _vehicle_status.condition_landed) { /* don't run controller when landed */着陆不要运行控制器 _reset_pos_sp = true; _reset_alt_sp = true; _mode_auto = false; reset_int_z = true; reset_int_xy = true; R.identity(); memcpy(&_att_sp.R_body[0], R.data, sizeof(_att_sp.R_body)); _att_sp.R_valid = true; _att_sp.roll_body = 0.0f; _att_sp.pitch_body = 0.0f; _att_sp.yaw_body = _yaw; _att_sp.thrust = 0.0f; _att_sp.timestamp = hrt_absolute_time(); /* publish attitude setpoint */ if (_att_sp_pub != nullptr) { orb_publish(_attitude_setpoint_id, _att_sp_pub, &_att_sp); } else if (_attitude_setpoint_id) { _att_sp_pub = orb_advertise(_attitude_setpoint_id, &_att_sp); } /////////////////////////另一个判断/////////////////////// } else { /* run position & altitude controllers, if enabled (otherwise use already computed velocity setpoints) */ if (_run_pos_control) { _vel_sp(0) = (_pos_sp(0) - _pos(0)) * _params.pos_p(0); _vel_sp(1) = (_pos_sp(1) - _pos(1)) * _params.pos_p(1); } // do not go slower than the follow target velocity when position tracking is active (set to valid) if(_pos_sp_triplet.current.type == position_setpoint_s::SETPOINT_TYPE_FOLLOW_TARGET && _pos_sp_triplet.current.velocity_valid && _pos_sp_triplet.current.position_valid) { math::Vector<3> ft_vel(_pos_sp_triplet.current.vx, _pos_sp_triplet.current.vy, 0); float cos_ratio = (ft_vel*_vel_sp)/(ft_vel.length()*_vel_sp.length()); // only override velocity set points when uav is traveling in same direction as target and vector component // is greater than calculated position set point velocity component if(cos_ratio > 0) { ft_vel *= (cos_ratio); // min speed a little faster than target vel ft_vel += ft_vel.normalized()*1.5f; } else { ft_vel.zero(); } _vel_sp(0) = fabs(ft_vel(0)) > fabs(_vel_sp(0)) ? ft_vel(0) : _vel_sp(0); _vel_sp(1) = fabs(ft_vel(1)) > fabs(_vel_sp(1)) ? ft_vel(1) : _vel_sp(1); // track target using velocity only } else if (_pos_sp_triplet.current.type == position_setpoint_s::SETPOINT_TYPE_FOLLOW_TARGET && _pos_sp_triplet.current.velocity_valid) { _vel_sp(0) = _pos_sp_triplet.current.vx; _vel_sp(1) = _pos_sp_triplet.current.vy; } if (_run_alt_control) { _vel_sp(2) = (_pos_sp(2) - _pos(2)) * _params.pos_p(2); } /* make sure velocity setpoint is saturated in xy*/ float vel_norm_xy = sqrtf(_vel_sp(0) * _vel_sp(0) + _vel_sp(1) * _vel_sp(1)); if (vel_norm_xy > _params.vel_max(0)) { /* note assumes vel_max(0) == vel_max(1) */ _vel_sp(0) = _vel_sp(0) * _params.vel_max(0) / vel_norm_xy; _vel_sp(1) = _vel_sp(1) * _params.vel_max(1) / vel_norm_xy; } /* make sure velocity setpoint is saturated in z*/ float vel_norm_z = sqrtf(_vel_sp(2) * _vel_sp(2)); if (vel_norm_z > _params.vel_max(2)) { _vel_sp(2) = _vel_sp(2) * _params.vel_max(2) / vel_norm_z; } if (!_control_mode.flag_control_position_enabled) { _reset_pos_sp = true; } if (!_control_mode.flag_control_altitude_enabled) { _reset_alt_sp = true; } if (!_control_mode.flag_control_velocity_enabled) { _vel_sp_prev(0) = _vel(0); _vel_sp_prev(1) = _vel(1); _vel_sp(0) = 0.0f; _vel_sp(1) = 0.0f; control_vel_enabled_prev = false; } if (!_control_mode.flag_control_climb_rate_enabled) { _vel_sp(2) = 0.0f; } /* use constant descend rate when landing, ignore altitude setpoint */ if (!_control_mode.flag_control_manual_enabled && _pos_sp_triplet.current.valid && _pos_sp_triplet.current.type == position_setpoint_s::SETPOINT_TYPE_LAND) { _vel_sp(2) = _params.land_speed; } /* special thrust setpoint generation for takeoff from ground */ if (!_control_mode.flag_control_manual_enabled && _pos_sp_triplet.current.valid && _pos_sp_triplet.current.type == position_setpoint_s::SETPOINT_TYPE_TAKEOFF && _control_mode.flag_armed) { // check if we are not already in air. // if yes then we don't need a jumped takeoff anymore if (!_takeoff_jumped && !_vehicle_status.condition_landed && fabsf(_takeoff_thrust_sp) < FLT_EPSILON) { _takeoff_jumped = true; } if (!_takeoff_jumped) { // ramp thrust setpoint up if (_vel(2) > -(_params.tko_speed / 2.0f)) { _takeoff_thrust_sp += 0.5f * dt; _vel_sp.zero(); _vel_prev.zero(); } else { // copter has reached our takeoff speed. split the thrust setpoint up // into an integral part and into a P part thrust_int(2) = _takeoff_thrust_sp - _params.vel_p(2) * fabsf(_vel(2)); thrust_int(2) = -math::constrain(thrust_int(2), _params.thr_min, _params.thr_max); _vel_sp_prev(2) = -_params.tko_speed; _takeoff_jumped = true; reset_int_z = false; } } if (_takeoff_jumped) { _vel_sp(2) = -_params.tko_speed; } } else { _takeoff_jumped = false; _takeoff_thrust_sp = 0.0f; } // limit total horizontal acceleration math::Vector<2> acc_hor; acc_hor(0) = (_vel_sp(0) - _vel_sp_prev(0)) / dt; acc_hor(1) = (_vel_sp(1) - _vel_sp_prev(1)) / dt; if (acc_hor.length() > _params.acc_hor_max) { acc_hor.normalize(); acc_hor *= _params.acc_hor_max; math::Vector<2> vel_sp_hor_prev(_vel_sp_prev(0), _vel_sp_prev(1)); math::Vector<2> vel_sp_hor = acc_hor * dt + vel_sp_hor_prev; _vel_sp(0) = vel_sp_hor(0); _vel_sp(1) = vel_sp_hor(1); } // limit vertical acceleration float acc_v = (_vel_sp(2) - _vel_sp_prev(2)) / dt; if (fabsf(acc_v) > 2 * _params.acc_hor_max) { acc_v /= fabsf(acc_v); _vel_sp(2) = acc_v * 2 * _params.acc_hor_max * dt + _vel_sp_prev(2); } _vel_sp_prev = _vel_sp; _global_vel_sp.vx = _vel_sp(0); _global_vel_sp.vy = _vel_sp(1); _global_vel_sp.vz = _vel_sp(2); /* publish velocity setpoint */ if (_global_vel_sp_pub != nullptr) { orb_publish(ORB_ID(vehicle_global_velocity_setpoint), _global_vel_sp_pub, &_global_vel_sp); } else { _global_vel_sp_pub = orb_advertise(ORB_ID(vehicle_global_velocity_setpoint), &_global_vel_sp); } if (_control_mode.flag_control_climb_rate_enabled || _control_mode.flag_control_velocity_enabled) { /* reset integrals if needed */ if (_control_mode.flag_control_climb_rate_enabled) { if (reset_int_z) { reset_int_z = false; float i = _params.thr_min; if (reset_int_z_manual) { i = _params.thr_hover; if (i < _params.thr_min) { i = _params.thr_min; } else if (i > _params.thr_max) { i = _params.thr_max; } } thrust_int(2) = -i; } } else { reset_int_z = true; } if (_control_mode.flag_control_velocity_enabled) { if (reset_int_xy) { reset_int_xy = false; thrust_int(0) = 0.0f; thrust_int(1) = 0.0f; } } else { reset_int_xy = true; } /* velocity error */ math::Vector<3> vel_err = _vel_sp - _vel; ////////////检查是否从非速度模式转到速度控制模式,如果是的话,校正xy的速度设定值以便姿态设定值保持不变///// // check if we have switched from a non-velocity controlled mode into a velocity controlled mode // if yes, then correct xy velocity setpoint such that the attitude setpoint is continuous if (!control_vel_enabled_prev && _control_mode.flag_control_velocity_enabled) { // choose velocity xyz setpoint such that the resulting thrust setpoint has the direction // given by the last attitude setpoint _vel_sp(0) = _vel(0) + (-PX4_R(_att_sp.R_body, 0, 2) * _att_sp.thrust - thrust_int(0) - _vel_err_d(0) * _params.vel_d(0)) / _params.vel_p(0); _vel_sp(1) = _vel(1) + (-PX4_R(_att_sp.R_body, 1, 2) * _att_sp.thrust - thrust_int(1) - _vel_err_d(1) * _params.vel_d(1)) / _params.vel_p(1); _vel_sp(2) = _vel(2) + (-PX4_R(_att_sp.R_body, 2, 2) * _att_sp.thrust - thrust_int(2) - _vel_err_d(2) * _params.vel_d(2)) / _params.vel_p(2); _vel_sp_prev(0) = _vel_sp(0); _vel_sp_prev(1) = _vel_sp(1); _vel_sp_prev(2) = _vel_sp(2); control_vel_enabled_prev = true; // compute updated velocity error vel_err = _vel_sp - _vel; } /* thrust vector in NED frame */ math::Vector<3> thrust_sp = vel_err.emult(_params.vel_p) + _vel_err_d.emult(_params.vel_d) + thrust_int; if (_pos_sp_triplet.current.type == position_setpoint_s::SETPOINT_TYPE_TAKEOFF && !_takeoff_jumped && !_control_mode.flag_control_manual_enabled) { // for jumped takeoffs use special thrust setpoint calculated above thrust_sp.zero(); thrust_sp(2) = -_takeoff_thrust_sp; } if (!_control_mode.flag_control_velocity_enabled) { thrust_sp(0) = 0.0f; thrust_sp(1) = 0.0f; } if (!_control_mode.flag_control_climb_rate_enabled) { thrust_sp(2) = 0.0f; } /* limit thrust vector and check for saturation */ bool saturation_xy = false; bool saturation_z = false; /* limit min lift */ float thr_min = _params.thr_min; if (!_control_mode.flag_control_velocity_enabled && thr_min < 0.0f) { /* don't allow downside thrust direction in manual attitude mode */ thr_min = 0.0f; } float thrust_abs = thrust_sp.length(); float tilt_max = _params.tilt_max_air; float thr_max = _params.thr_max; /* filter vel_z over 1/8sec */ _vel_z_lp = _vel_z_lp * (1.0f - dt * 8.0f) + dt * 8.0f * _vel(2); /* filter vel_z change over 1/8sec */ float vel_z_change = (_vel(2) - _vel_prev(2)) / dt; _acc_z_lp = _acc_z_lp * (1.0f - dt * 8.0f) + dt * 8.0f * vel_z_change; /* adjust limits for landing mode */调整限制 着陆模式 if (!_control_mode.flag_control_manual_enabled && _pos_sp_triplet.current.valid && _pos_sp_triplet.current.type == position_setpoint_s::SETPOINT_TYPE_LAND) { /* limit max tilt and min lift when landing */ tilt_max = _params.tilt_max_land; if (thr_min < 0.0f) { thr_min = 0.0f; } /* descend stabilized, we're landing */ if (!_in_landing && !_lnd_reached_ground && (float)fabs(_acc_z_lp) < 0.1f && _vel_z_lp > 0.5f * _params.land_speed) { _in_landing = true; } /* assume ground, cut thrust */ if (_in_landing && _vel_z_lp < 0.1f) { thr_max = 0.0f; _in_landing = false; _lnd_reached_ground = true; } /* once we assumed to have reached the ground always cut the thrust. Only free fall detection below can revoke this */ if (!_in_landing && _lnd_reached_ground) { thr_max = 0.0f; } /* if we suddenly fall, reset landing logic and remove thrust limit */ if (_lnd_reached_ground /* XXX: magic value, assuming free fall above 4m/s2 acceleration */ && (_acc_z_lp > 4.0f || _vel_z_lp > 2.0f * _params.land_speed)) { thr_max = _params.thr_max; _in_landing = false; _lnd_reached_ground = false; } } else { _in_landing = false; _lnd_reached_ground = false; } /* limit min lift */ if (-thrust_sp(2) < thr_min) { thrust_sp(2) = -thr_min; saturation_z = true; } if (_control_mode.flag_control_velocity_enabled) { /* limit max tilt */ if (thr_min >= 0.0f && tilt_max < M_PI_F / 2 - 0.05f) { /* absolute horizontal thrust */ float thrust_sp_xy_len = math::Vector<2>(thrust_sp(0), thrust_sp(1)).length(); if (thrust_sp_xy_len > 0.01f) { /* max horizontal thrust for given vertical thrust*/ float thrust_xy_max = -thrust_sp(2) * tanf(tilt_max); if (thrust_sp_xy_len > thrust_xy_max) { float k = thrust_xy_max / thrust_sp_xy_len; thrust_sp(0) *= k; thrust_sp(1) *= k; saturation_xy = true; } } } } if (_control_mode.flag_control_altitude_enabled) { /* thrust compensation for altitude only control modes */推力的补偿 高度控制模式 float att_comp; if (_R(2, 2) > TILT_COS_MAX) { att_comp = 1.0f / _R(2, 2); } else if (_R(2, 2) > 0.0f) { att_comp = ((1.0f / TILT_COS_MAX - 1.0f) / TILT_COS_MAX) * _R(2, 2) + 1.0f; saturation_z = true; } else { att_comp = 1.0f; saturation_z = true; } thrust_sp(2) *= att_comp; } ///////////////////限制最大推力/////////////////// /* limit max thrust */ thrust_abs = thrust_sp.length(); /* recalculate because it might have changed */ if (thrust_abs > thr_max) { if (thrust_sp(2) < 0.0f) { if (-thrust_sp(2) > thr_max) { /* thrust Z component is too large, limit it */ thrust_sp(0) = 0.0f; thrust_sp(1) = 0.0f; thrust_sp(2) = -thr_max; saturation_xy = true; saturation_z = true; } else { /* preserve thrust Z component and lower XY, keeping altitude is more important than position */ float thrust_xy_max = sqrtf(thr_max * thr_max - thrust_sp(2) * thrust_sp(2)); float thrust_xy_abs = math::Vector<2>(thrust_sp(0), thrust_sp(1)).length(); float k = thrust_xy_max / thrust_xy_abs; thrust_sp(0) *= k; thrust_sp(1) *= k; saturation_xy = true; } } else { /* Z component is negative, going down, simply limit thrust vector */ float k = thr_max / thrust_abs; thrust_sp *= k; saturation_xy = true; saturation_z = true; } thrust_abs = thr_max; } ///////////跟新积分////////// /* update integrals */ if (_control_mode.flag_control_velocity_enabled && !saturation_xy) { thrust_int(0) += vel_err(0) * _params.vel_i(0) * dt; thrust_int(1) += vel_err(1) * _params.vel_i(1) * dt; } if (_control_mode.flag_control_climb_rate_enabled && !saturation_z) { thrust_int(2) += vel_err(2) * _params.vel_i(2) * dt; /* protection against flipping on ground when landing */ if (thrust_int(2) > 0.0f) { thrust_int(2) = 0.0f; } } /////////////根据推力矢量计算的姿态设定值////////////// /* calculate attitude setpoint from thrust vector */ if (_control_mode.flag_control_velocity_enabled) { /* desired body_z axis = -normalize(thrust_vector) */ math::Vector<3> body_x; math::Vector<3> body_y; math::Vector<3> body_z; if (thrust_abs > SIGMA) { body_z = -thrust_sp / thrust_abs; } else { /* no thrust, set Z axis to safe value */ body_z.zero(); body_z(2) = 1.0f; } /* vector of desired yaw direction in XY plane, rotated by PI/2 */ math::Vector<3> y_C(-sinf(_att_sp.yaw_body), cosf(_att_sp.yaw_body), 0.0f); if (fabsf(body_z(2)) > SIGMA) { /* desired body_x axis, orthogonal to body_z */ body_x = y_C % body_z; /* keep nose to front while inverted upside down */ if (body_z(2) < 0.0f) { body_x = -body_x; } body_x.normalize(); } else { /* desired thrust is in XY plane, set X downside to construct correct matrix, * but yaw component will not be used actually */ body_x.zero(); body_x(2) = 1.0f; } /* desired body_y axis */ body_y = body_z % body_x; /* fill rotation matrix */ for (int i = 0; i < 3; i++) { R(i, 0) = body_x(i); R(i, 1) = body_y(i); R(i, 2) = body_z(i); } ////////////////////复制旋转矩阵到姿态设定值主题中//////////////////// /* copy rotation matrix to attitude setpoint topic */ memcpy(&_att_sp.R_body[0], R.data, sizeof(_att_sp.R_body)); _att_sp.R_valid = true; /////////////////////复制四元数设定值到姿态设定值主题中/////////////////////////// /* copy quaternion setpoint to attitude setpoint topic */ math::Quaternion q_sp; q_sp.from_dcm(R); memcpy(&_att_sp.q_d[0], &q_sp.data[0], sizeof(_att_sp.q_d)); //////////////计算欧拉角,只记录,不得用于控制////////////////// /* calculate euler angles, for logging only, must not be used for control */ math::Vector<3> euler = R.to_euler(); _att_sp.roll_body = euler(0); _att_sp.pitch_body = euler(1); /* yaw already used to construct rot matrix, but actual rotation matrix can have different yaw near singularity */ /////////偏航已经用于构建rot矩阵,但实际旋转矩阵可以有奇点附近的不同偏航/////////// } else if (!_control_mode.flag_control_manual_enabled) { /////////////没有位置控制的自主高度控制(故障安全降落),强制水平姿态,不改变yaw/////////////////// /* autonomous altitude control without position control (failsafe landing), * force level attitude, don't change yaw */ R.from_euler(0.0f, 0.0f, _att_sp.yaw_body); ////////////////////复制旋转矩阵到姿态设定值主题中//////////////////// /* copy rotation matrix to attitude setpoint topic */ memcpy(&_att_sp.R_body[0], R.data, sizeof(_att_sp.R_body)); _att_sp.R_valid = true; /////////////////////复制四元数设定值到姿态设定值主题中/////////////////////////// /* copy quaternion setpoint to attitude setpoint topic */ math::Quaternion q_sp; q_sp.from_dcm(R); memcpy(&_att_sp.q_d[0], &q_sp.data[0], sizeof(_att_sp.q_d)); _att_sp.roll_body = 0.0f; _att_sp.pitch_body = 0.0f; } _att_sp.thrust = thrust_abs; /* save thrust setpoint for logging */保存推理设定值在logging里 _local_pos_sp.acc_x = thrust_sp(0) * ONE_G; _local_pos_sp.acc_y = thrust_sp(1) * ONE_G; _local_pos_sp.acc_z = thrust_sp(2) * ONE_G; _att_sp.timestamp = hrt_absolute_time(); } else { reset_int_z = true; } } /* fill local position, velocity and thrust setpoint */ _local_pos_sp.timestamp = hrt_absolute_time(); _local_pos_sp.x = _pos_sp(0); _local_pos_sp.y = _pos_sp(1); _local_pos_sp.z = _pos_sp(2); _local_pos_sp.yaw = _att_sp.yaw_body; _local_pos_sp.vx = _vel_sp(0); _local_pos_sp.vy = _vel_sp(1); _local_pos_sp.vz = _vel_sp(2); ////////////////////////发布当地位置设定值//////////////////// /***************这个应该是这段程序处理的最后结果********************/ /* publish local position setpoint */ if (_local_pos_sp_pub != nullptr) { orb_publish(ORB_ID(vehicle_local_position_setpoint), _local_pos_sp_pub, &_local_pos_sp); } else { _local_pos_sp_pub = orb_advertise(ORB_ID(vehicle_local_position_setpoint), &_local_pos_sp); } } else {位置控制模式失能,那么复位设定值 /* position controller disabled, reset setpoints */ _reset_alt_sp = true; _reset_pos_sp = true; _mode_auto = false; reset_int_z = true; reset_int_xy = true; control_vel_enabled_prev = false; /* store last velocity in case a mode switch to position control occurs */ _vel_sp_prev = _vel; } //////////////////////从手动控制中产生姿态设定值////////////////////////// /* generate attitude setpoint from manual controls */ if (_control_mode.flag_control_manual_enabled && _control_mode.flag_control_attitude_enabled) { /* reset yaw setpoint to current position if needed */ if (reset_yaw_sp) { reset_yaw_sp = false; _att_sp.yaw_body = _yaw; } //////////////////在地上时,不动偏航方向///////////////////////// /* do not move yaw while sitting on the ground */ else if (!_vehicle_status.condition_landed && !(!_control_mode.flag_control_altitude_enabled && _manual.z < 0.1f)) { /* we want to know the real constraint, and global overrides manual */ const float yaw_rate_max = (_params.man_yaw_max < _params.global_yaw_max) ? _params.man_yaw_max : _params.global_yaw_max; const float yaw_offset_max = yaw_rate_max / _params.mc_att_yaw_p; _att_sp.yaw_sp_move_rate = _manual.r * yaw_rate_max; float yaw_target = _wrap_pi(_att_sp.yaw_body + _att_sp.yaw_sp_move_rate * dt); float yaw_offs = _wrap_pi(yaw_target - _yaw); ////////////////////如果对于系统跟踪,yaw偏移变得过大,那么停止其转移//////////////////// // If the yaw offset became too big for the system to track stop // shifting it // XXX this needs inspection - probably requires a clamp, not an if if (fabsf(yaw_offs) < yaw_offset_max) { _att_sp.yaw_body = yaw_target; } } //////////////////////直接控制侧倾和俯仰,如果协助速度控制器没有被激活//////////////////// /* control roll and pitch directly if we no aiding velocity controller is active */ if (!_control_mode.flag_control_velocity_enabled) { _att_sp.roll_body = _manual.y * _params.man_roll_max; _att_sp.pitch_body = -_manual.x * _params.man_pitch_max; } //////////////////////控制油门直接,如果没有爬升率控制器被激活///////////////////// /* control throttle directly if no climb rate controller is active */ if (!_control_mode.flag_control_climb_rate_enabled) { float thr_val = throttle_curve(_manual.z, _params.thr_hover); _att_sp.thrust = math::min(thr_val, _manual_thr_max.get()); /* enforce minimum throttle if not landed */ if (!_vehicle_status.condition_landed) { _att_sp.thrust = math::max(_att_sp.thrust, _manual_thr_min.get()); } } math::Matrix<3, 3> R_sp; //////////////////////构建姿态设定值的旋转矩阵/////////////////////// /* construct attitude setpoint rotation matrix */ R_sp.from_euler(_att_sp.roll_body, _att_sp.pitch_body, _att_sp.yaw_body); memcpy(&_att_sp.R_body[0], R_sp.data, sizeof(_att_sp.R_body)); ////////////////////为所有非姿态飞行模式 复位加速度设定值///////////////// /* reset the acceleration set point for all non-attitude flight modes */ if (!(_control_mode.flag_control_offboard_enabled && !(_control_mode.flag_control_position_enabled || _control_mode.flag_control_velocity_enabled))) { _thrust_sp_prev = R_sp * math::Vector<3>(0, 0, -_att_sp.thrust); } /////////////////////将四元数设定值复制到姿态设定值的主题中///////////////////////// /* copy quaternion setpoint to attitude setpoint topic */ math::Quaternion q_sp; q_sp.from_dcm(R_sp); memcpy(&_att_sp.q_d[0], &q_sp.data[0], sizeof(_att_sp.q_d)); _att_sp.timestamp = hrt_absolute_time(); } else { reset_yaw_sp = true; } /* update previous velocity for velocity controller D part */ _vel_prev = _vel; /////////////////////////发布设定值////////////////////////////// /* publish attitude setpoint * Do not publish if offboard is enabled but position/velocity control is disabled, * in this case the attitude setpoint is published by the mavlink app. Also do not publish * if the vehicle is a VTOL and it's just doing a transition (the VTOL attitude control module will generate * attitude setpoints for the transition). */ if (!(_control_mode.flag_control_offboard_enabled && !(_control_mode.flag_control_position_enabled || _control_mode.flag_control_velocity_enabled))) { if (_att_sp_pub != nullptr) { orb_publish(_attitude_setpoint_id, _att_sp_pub, &_att_sp); } else if (_attitude_setpoint_id) { _att_sp_pub = orb_advertise(_attitude_setpoint_id, &_att_sp); } } /////////////手动油门控制后,将高度控制器积分(悬停油门)复位到手动油门/////////////// /* reset altitude controller integral (hovering throttle) to manual throttle after manual throttle control */ reset_int_z_manual = _control_mode.flag_armed && _control_mode.flag_control_manual_enabled && !_control_mode.flag_control_climb_rate_enabled; } mavlink_log_info(&_mavlink_log_pub, "[mpc] stopped"); _control_task = -1; }这些设定值的确定是不是就是控制策略的体现?
void MulticopterPositionControl::poll_subscriptions() { bool updated; /**********************机体状态跟新**************************/ orb_check(_vehicle_status_sub, &updated); if (updated) { orb_copy(ORB_ID(vehicle_status), _vehicle_status_sub, &_vehicle_status); /* set correct uORB ID, depending on if vehicle is VTOL or not */ if (!_attitude_setpoint_id) { if (_vehicle_status.is_vtol) { _attitude_setpoint_id = ORB_ID(mc_virtual_attitude_setpoint); } else { _attitude_setpoint_id = ORB_ID(vehicle_attitude_setpoint); } } } /**********************控制状态跟新**************************/ orb_check(_ctrl_state_sub, &updated); if (updated) { orb_copy(ORB_ID(control_state), _ctrl_state_sub, &_ctrl_state); /* get current rotation matrix and euler angles from control state quaternions */ math::Quaternion q_att(_ctrl_state.q[0], _ctrl_state.q[1], _ctrl_state.q[2], _ctrl_state.q[3]); _R = q_att.to_dcm(); math::Vector<3> euler_angles; euler_angles = _R.to_euler(); _yaw = euler_angles(2); } /**********************setpoint状态跟新**************************/ orb_check(_att_sp_sub, &updated); if (updated) { orb_copy(ORB_ID(vehicle_attitude_setpoint), _att_sp_sub, &_att_sp); } orb_check(_control_mode_sub, &updated); if (updated) { orb_copy(ORB_ID(vehicle_control_mode), _control_mode_sub, &_control_mode); } /**********************手动控制状态跟新**************************/ orb_check(_manual_sub, &updated); if (updated) { orb_copy(ORB_ID(manual_control_setpoint), _manual_sub, &_manual); } /**********************解锁状态跟新**************************/ orb_check(_arming_sub, &updated); if (updated) { orb_copy(ORB_ID(actuator_armed), _arming_sub, &_arming); } /**********************位置跟新**************************/ orb_check(_local_pos_sub, &updated); if (updated) { orb_copy(ORB_ID(vehicle_local_position), _local_pos_sub, &_local_pos); } }
void MulticopterPositionControl::update_ref() { if (_local_pos.ref_timestamp != _ref_timestamp) { double lat_sp, lon_sp;//维度设定、经度设定 float alt_sp = 0.0f;//姿态设定 if (_ref_timestamp != 0) { /*************在整体框架下计算当前位置设定值***************/ /* calculate current position setpoint in global frame */ map_projection_reproject(&_ref_pos, _pos_sp(0), _pos_sp(1), &lat_sp, &lon_sp); alt_sp = _ref_alt - _pos_sp(2); } /*************跟新投影参考*********************/ /* update local projection reference */ map_projection_init(&_ref_pos, _local_pos.ref_lat, _local_pos.ref_lon); _ref_alt = _local_pos.ref_alt; if (_ref_timestamp != 0) { /*********************设置新的位置设定值*******************/ /* reproject position setpoint to new reference */ map_projection_project(&_ref_pos, lat_sp, lon_sp, &_pos_sp.data[0], &_pos_sp.data[1]); _pos_sp(2) = -(alt_sp - _ref_alt); } _ref_timestamp = _local_pos.ref_timestamp; } }
void MulticopterPositionControl::control_manual(float dt) { math::Vector<3> req_vel_sp; // X,Y in local frame and Z in global (D), in [-1,1] normalized range req_vel_sp.zero(); if (_control_mode.flag_control_altitude_enabled) { /* set vertical velocity setpoint with throttle stick */用油门棒设置垂直速度 req_vel_sp(2) = -scale_control(_manual.z - 0.5f, 0.5f, _params.alt_ctl_dz, _params.alt_ctl_dy); // D } if (_control_mode.flag_control_position_enabled) { /* set horizontal velocity setpoint with roll/pitch stick */用roll/pitch棒设置水平速度 req_vel_sp(0) = _manual.x; req_vel_sp(1) = _manual.y; } if (_control_mode.flag_control_altitude_enabled) { /* reset alt setpoint to current altitude if needed */复位姿态设定值 reset_alt_sp(); } if (_control_mode.flag_control_position_enabled) { /* reset position setpoint to current position if needed */复位位置设定值 reset_pos_sp(); } /* limit velocity setpoint */限制速度设定值 float req_vel_sp_norm = req_vel_sp.length(); if (req_vel_sp_norm > 1.0f) { req_vel_sp /= req_vel_sp_norm;//相当于是向量除以它的模值 } ///////////_req_vel_sp的范围是0到1,围绕yaw旋转 /* _req_vel_sp scaled to 0..1, scale it to max speed and rotate around yaw */ math::Matrix<3, 3> R_yaw_sp; R_yaw_sp.from_euler(0.0f, 0.0f, _att_sp.yaw_body); math::Vector<3> req_vel_sp_scaled = R_yaw_sp * req_vel_sp.emult( _params.vel_cruise); // in NED and scaled to actual velocity在NED旋转方式下缩放到实际速度 /////////用户辅助模式:用户控制速度,但是,如果速度足够小,相应的轴的位置就会保持//////////////// /* * assisted velocity mode: user controls velocity, but if velocity is small enough, position * hold is activated for the corresponding axis */ /* horizontal axes */水平轴 if (_control_mode.flag_control_position_enabled) { /* check for pos. hold */检测位置的保持状态 if (fabsf(req_vel_sp(0)) < _params.hold_xy_dz && fabsf(req_vel_sp(1)) < _params.hold_xy_dz) { if (!_pos_hold_engaged) { if (_params.hold_max_xy < FLT_EPSILON || (fabsf(_vel(0)) < _params.hold_max_xy && fabsf(_vel(1)) < _params.hold_max_xy)) { _pos_hold_engaged = true; } else { _pos_hold_engaged = false; } } } else { _pos_hold_engaged = false; } /* set requested velocity setpoint */设置所需的速度设定值 if (!_pos_hold_engaged) { _pos_sp(0) = _pos(0); _pos_sp(1) = _pos(1); //////////速度设定值会被利用,位置设定值不会被用///////////// _run_pos_control = false; /* request velocity setpoint to be used, instead of position setpoint */ _vel_sp(0) = req_vel_sp_scaled(0); _vel_sp(1) = req_vel_sp_scaled(1); } } /* vertical axis */垂直轴 if (_control_mode.flag_control_altitude_enabled) { /* check for pos. hold */ if (fabsf(req_vel_sp(2)) < FLT_EPSILON) { if (!_alt_hold_engaged) { if (_params.hold_max_z < FLT_EPSILON || fabsf(_vel(2)) < _params.hold_max_z) { _alt_hold_engaged = true; } else { _alt_hold_engaged = false; } } } else { _alt_hold_engaged = false; } /* set requested velocity setpoint */ if (!_alt_hold_engaged) { _run_alt_control = false; /* request velocity setpoint to be used, instead of altitude setpoint */ _vel_sp(2) = req_vel_sp_scaled(2); _pos_sp(2) = _pos(2); } } }
#ifdef __cplusplus struct __EXPORT position_setpoint_triplet_s { #else struct position_setpoint_triplet_s { #endif struct position_setpoint_s previous; struct position_setpoint_s current; struct position_setpoint_s next; uint8_t nav_state; #ifdef __cplusplus #endif };
整个MulticopterPositionControl::task_main()的输出应该是
orb_publish(_attitude_setpoint_id, _att_sp_pub, &_att_sp); orb_publish(ORB_ID(vehicle_global_velocity_setpoint), _global_vel_sp_pub, &_global_vel_sp); orb_publish(ORB_ID(vehicle_local_position_setpoint), _local_pos_sp_pub, &_local_pos_sp);
其中orb_publish(_attitude_setpoint_id, _att_sp_pub, &_att_sp);是在main的最下面,应该这个是最后处理的结果
但是全局搜索ORB_ID(vehicle_local_position_setpoint)if (!_attitude_setpoint_id) { if (_vehicle_status.is_vtol) { _attitude_setpoint_id = ORB_ID(mc_virtual_attitude_setpoint); } else { _attitude_setpoint_id = ORB_ID(vehicle_attitude_setpoint); } }再看看
void MulticopterPositionControl::task_main() { /* * do subscriptions */ _vehicle_status_sub = orb_subscribe(ORB_ID(vehicle_status)); _ctrl_state_sub = orb_subscribe(ORB_ID(control_state)); _att_sp_sub = orb_subscribe(ORB_ID(vehicle_attitude_setpoint)); _control_mode_sub = orb_subscribe(ORB_ID(vehicle_control_mode)); _params_sub = orb_subscribe(ORB_ID(parameter_update)); _manual_sub = orb_subscribe(ORB_ID(manual_control_setpoint)); _arming_sub = orb_subscribe(ORB_ID(actuator_armed)); _local_pos_sub = orb_subscribe(ORB_ID(vehicle_local_position)); _pos_sp_triplet_sub = orb_subscribe(ORB_ID(position_setpoint_triplet)); _local_pos_sp_sub = orb_subscribe(ORB_ID(vehicle_local_position_setpoint)); _global_vel_sp_sub = orb_subscribe(ORB_ID(vehicle_global_velocity_setpoint));全局搜索vehicle_attitude_setpoint
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
void MulticopterAttitudeControl::vehicle_attitude_setpoint_poll() { /* check if there is a new setpoint */ bool updated; orb_check(_v_att_sp_sub, &updated); if (updated) { orb_copy(ORB_ID(vehicle_attitude_setpoint), _v_att_sp_sub, &_v_att_sp); } }突然发现//ardupilot/modules/PX4Firmware/src/modules/vtol_att_control//vtol_att_control_main.cpp中也有orb_copy(ORB_ID(vehicle_attitude_setpoint)
void VtolAttitudeControl::vehicle_attitude_setpoint_poll() { /* check if there is a new setpoint */ bool updated; orb_check(_v_att_sp_sub, &updated); if (updated) { orb_copy(ORB_ID(vehicle_attitude_setpoint), _v_att_sp_sub, &_v_att_sp); } }
void VtolAttitudeControl::mc_virtual_att_sp_poll() { bool updated; orb_check(_mc_virtual_att_sp_sub, &updated); if (updated) { orb_copy(ORB_ID(mc_virtual_attitude_setpoint), _mc_virtual_att_sp_sub , &_mc_virtual_att_sp); } }
void VtolAttitudeControl::publish_att_sp() { if (_v_att_sp_pub != nullptr) { /* publish the attitude setpoint */ orb_publish(ORB_ID(vehicle_attitude_setpoint), _v_att_sp_pub, &_v_att_sp); } else { /* advertise and publish */ _v_att_sp_pub = orb_advertise(ORB_ID(vehicle_attitude_setpoint), &_v_att_sp); } }好奇葩啊,//ardupilot/modules/PX4Firmware/src/modules/vtol_att_control//vtol_att_control_main.cpp
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是最大的一个类的实例
/* fetch initial parameter values */ parameters_update();里面好多参数,不知道是不是上位机配置的参数,就是说是不是把上位机的各种关于飞行的参数存到了sd卡中,在通过这个函数跟新到程序中用于飞行器的控制??
int MulticopterAttitudeControl::start() { ASSERT(_control_task == -1); /* start the task */ _control_task = px4_task_spawn_cmd("mc_att_control", SCHED_DEFAULT, SCHED_PRIORITY_MAX - 5, 1500, (px4_main_t)&MulticopterAttitudeControl::task_main_trampoline, nullptr); if (_control_task < 0) { warn("task start failed"); return -errno; } return OK; }
void MulticopterAttitudeControl::task_main_trampoline(int argc, char *argv[]) { mc_att_control::g_control->task_main(); }
void MulticopterAttitudeControl::task_main() { /**************************订阅各种信息**************************/ /* * do subscriptions */ _v_att_sp_sub = orb_subscribe(ORB_ID(vehicle_attitude_setpoint)); _v_rates_sp_sub = orb_subscribe(ORB_ID(vehicle_rates_setpoint)); _ctrl_state_sub = orb_subscribe(ORB_ID(control_state)); _v_control_mode_sub = orb_subscribe(ORB_ID(vehicle_control_mode)); _params_sub = orb_subscribe(ORB_ID(parameter_update)); _manual_control_sp_sub = orb_subscribe(ORB_ID(manual_control_setpoint)); _armed_sub = orb_subscribe(ORB_ID(actuator_armed)); _vehicle_status_sub = orb_subscribe(ORB_ID(vehicle_status)); _motor_limits_sub = orb_subscribe(ORB_ID(multirotor_motor_limits)); /**************************初始化各种参数**************************/ /* initialize parameters cache */ parameters_update(); /**************************启动机体姿态原函数**************************/ /* wakeup source: vehicle attitude */ px4_pollfd_struct_t fds[1]; fds[0].fd = _ctrl_state_sub; fds[0].events = POLLIN; /**************************大循环**************************/ while (!_task_should_exit) { /////////////////////////////等待100ms 为了获取数据///////////////////////// /* wait for up to 100ms for data */ int pret = px4_poll(&fds[0], (sizeof(fds) / sizeof(fds[0])), 100); /* timed out - periodic check for _task_should_exit */ if (pret == 0) { continue; } /* this is undesirable but not much we can do - might want to flag unhappy status */ if (pret < 0) { warn("mc att ctrl: poll error %d, %d", pret, errno); /* sleep a bit before next try */ usleep(100000); continue; } perf_begin(_loop_perf); ///////////////////////运行姿态变化控制器////////////////////////// /* run controller on attitude changes */ if (fds[0].revents & POLLIN) { static uint64_t last_run = 0; float dt = (hrt_absolute_time() - last_run) / 1000000.0f; last_run = hrt_absolute_time(); /////////////////////防止dt太小或太大 2ms<dt<20ms//////////////////// /* guard against too small (< 2ms) and too large (> 20ms) dt's */ if (dt < 0.002f) { dt = 0.002f; } else if (dt > 0.02f) { dt = 0.02f; } /* copy attitude and control state topics */ orb_copy(ORB_ID(control_state), _ctrl_state_sub, &_ctrl_state); /* check for updates in other topics */ parameter_update_poll(); vehicle_control_mode_poll(); arming_status_poll(); vehicle_manual_poll(); vehicle_status_poll(); vehicle_motor_limits_poll(); ////////////////若xy轴的设定值大于阈值,那么不运行姿态控制///////////////// /* Check if we are in rattitude mode and the pilot is above the threshold on pitch * or roll (yaw can rotate 360 in normal att control). If both are true don't * even bother running the attitude controllers */ if (_vehicle_status.main_state == vehicle_status_s::MAIN_STATE_RATTITUDE) { if (fabsf(_manual_control_sp.y) > _params.rattitude_thres || fabsf(_manual_control_sp.x) > _params.rattitude_thres) { _v_control_mode.flag_control_attitude_enabled = false; } } /////////////////////////姿态控制使能//////////////////////////// if (_v_control_mode.flag_control_attitude_enabled) { if (_ts_opt_recovery == nullptr) { // the tailsitter recovery instance has not been created, thus, the vehicle // is not a tailsitter, do normal attitude control control_attitude(dt); } else { vehicle_attitude_setpoint_poll(); _thrust_sp = _v_att_sp.thrust; math::Quaternion q(_ctrl_state.q[0], _ctrl_state.q[1], _ctrl_state.q[2], _ctrl_state.q[3]); math::Quaternion q_sp(&_v_att_sp.q_d[0]); _ts_opt_recovery->setAttGains(_params.att_p, _params.yaw_ff); _ts_opt_recovery->calcOptimalRates(q, q_sp, _v_att_sp.yaw_sp_move_rate, _rates_sp); /* limit rates */ for (int i = 0; i < 3; i++) { _rates_sp(i) = math::constrain(_rates_sp(i), -_params.mc_rate_max(i), _params.mc_rate_max(i)); } } /* publish attitude rates setpoint */ _v_rates_sp.roll = _rates_sp(0); _v_rates_sp.pitch = _rates_sp(1); _v_rates_sp.yaw = _rates_sp(2); _v_rates_sp.thrust = _thrust_sp; _v_rates_sp.timestamp = hrt_absolute_time(); if (_v_rates_sp_pub != nullptr) { orb_publish(_rates_sp_id, _v_rates_sp_pub, &_v_rates_sp); } else if (_rates_sp_id) { _v_rates_sp_pub = orb_advertise(_rates_sp_id, &_v_rates_sp); } //} } else { /////////////////////////////////////手动控制使能////////////////////////////// /* attitude controller disabled, poll rates setpoint topic */ if (_v_control_mode.flag_control_manual_enabled) { ///////////////////////特技模式///////////////////////// /* manual rates control - ACRO mode */ _rates_sp = math::Vector<3>(_manual_control_sp.y, -_manual_control_sp.x, _manual_control_sp.r).emult(_params.acro_rate_max); _thrust_sp = math::min(_manual_control_sp.z, MANUAL_THROTTLE_MAX_MULTICOPTER); /////////////////////////发布角速度设定值//////////////////// /* publish attitude rates setpoint */ _v_rates_sp.roll = _rates_sp(0); _v_rates_sp.pitch = _rates_sp(1); _v_rates_sp.yaw = _rates_sp(2); _v_rates_sp.thrust = _thrust_sp; _v_rates_sp.timestamp = hrt_absolute_time(); if (_v_rates_sp_pub != nullptr) { orb_publish(_rates_sp_id, _v_rates_sp_pub, &_v_rates_sp); } else if (_rates_sp_id) { _v_rates_sp_pub = orb_advertise(_rates_sp_id, &_v_rates_sp); } } //////////////非特技模式////////////////// else { //////////////速度设定值来自于vehicle_rates_setpoint////////////////// //////////////vehicle_rates_setpoint来自于哪里??/////////////////// /* attitude controller disabled, poll rates setpoint topic */ vehicle_rates_setpoint_poll(); _rates_sp(0) = _v_rates_sp.roll; _rates_sp(1) = _v_rates_sp.pitch; _rates_sp(2) = _v_rates_sp.yaw; _thrust_sp = _v_rates_sp.thrust; } } ///////////////////////////////速度控制///////////////////////////////////// if (_v_control_mode.flag_control_rates_enabled) { //////////////////角速度控制//////////////////// control_attitude_rates(dt); //////////////////发布执行器控制/////////////// //////////////////这个就是pwm吗??//////////// /* publish actuator controls */ _actuators.control[0] = (PX4_ISFINITE(_att_control(0))) ? _att_control(0) : 0.0f; _actuators.control[1] = (PX4_ISFINITE(_att_control(1))) ? _att_control(1) : 0.0f; _actuators.control[2] = (PX4_ISFINITE(_att_control(2))) ? _att_control(2) : 0.0f; _actuators.control[3] = (PX4_ISFINITE(_thrust_sp)) ? _thrust_sp : 0.0f; _actuators.timestamp = hrt_absolute_time(); _actuators.timestamp_sample = _ctrl_state.timestamp; _controller_status.roll_rate_integ = _rates_int(0); _controller_status.pitch_rate_integ = _rates_int(1); _controller_status.yaw_rate_integ = _rates_int(2); _controller_status.timestamp = hrt_absolute_time(); if (!_actuators_0_circuit_breaker_enabled) { if (_actuators_0_pub != nullptr) { orb_publish(_actuators_id, _actuators_0_pub, &_actuators); perf_end(_controller_latency_perf); } else if (_actuators_id) { _actuators_0_pub = orb_advertise(_actuators_id, &_actuators); } } /* publish controller status */ if (_controller_status_pub != nullptr) { orb_publish(ORB_ID(mc_att_ctrl_status), _controller_status_pub, &_controller_status); } else { _controller_status_pub = orb_advertise(ORB_ID(mc_att_ctrl_status), &_controller_status); } } } perf_end(_loop_perf); } _control_task = -1; return; }看看这3段控制程序的输入输出,大概能猜想他们的运行逻辑并不是并列的,很有可能_v_control_mode.flag_control_rates_enabled跟在_v_control_mode.flag_control_attitude_enabled之后,这样就形成了常用的外环角度(也就是这里的姿态)内环角速度(姿态速度)的控制结构,这个还需要细看里面的输入输出和标志位如何赋值控制运行逻辑。
/** * Attitude controller. * Input: 'vehicle_attitude_setpoint' topics (depending on mode) * Output: '_rates_sp' vector, '_thrust_sp' */ /*************************注意看注释!!********************/ /***********输入是'vehicle_attitude_setpoint'主题***********/ /************输出是角速度设定值向量和油门设定值*************/ void MulticopterAttitudeControl::control_attitude(float dt) { vehicle_attitude_setpoint_poll();//获取'vehicle_attitude_setpoint'主题 _thrust_sp = _v_att_sp.thrust; //////////构建姿态设定值的旋转矩阵就是vehicle_attitude_setpoint_poll()获取的////////// /* construct attitude setpoint rotation matrix */ math::Matrix<3, 3> R_sp; R_sp.set(_v_att_sp.R_body);//这里做的事只是把订阅_v_att_sp复制到一个新的地方 /**** void set(const float *d) { **** memcpy(data, d, sizeof(data)); **** } ****/ //////////////////////定义一个控制状态的四元数和相应的方向余弦矩阵(dcm)///////////////// /* get current rotation matrix from control state quaternions */ math::Quaternion q_att(_ctrl_state.q[0], _ctrl_state.q[1], _ctrl_state.q[2], _ctrl_state.q[3]); math::Matrix<3, 3> R = q_att.to_dcm(); /////////////////输入就准备好了,就是姿态设定值,并转变成能用的形式四元数和dcm/////////////// /* all input data is ready, run controller itself */ /* try to move thrust vector shortest way, because yaw response is slower than roll/pitch */ math::Vector<3> R_z(R(0, 2), R(1, 2), R(2, 2));//控制状态的z轴 math::Vector<3> R_sp_z(R_sp(0, 2), R_sp(1, 2), R_sp(2, 2));//设定姿态的z轴 /* axis and sin(angle) of desired rotation */轴与角的理想旋转 math::Vector<3> e_R = R.transposed() * (R_z % R_sp_z);//transposed()换位函数,%是求叉积的意思 /*** ardupilot/modules/PX4Firmware/src/lib/matrix/matrix/Matrix.hpp *** Matrix<Type, N, M> transpose() const *** { *** Matrix<Type, N, M> res; *** const Matrix<Type, M, N> &self = *this; *** *** for (size_t i = 0; i < M; i++) { *** for (size_t j = 0; j < N; j++) { *** res(j, i) = self(i, j); *** } *** } *** *** return res; *** } ***/ /* calculate angle error */计算角的误差 float e_R_z_sin = e_R.length(); float e_R_z_cos = R_z * R_sp_z; //计算姿态角度误差(姿态误差),一个数学知识背景:由公式a×b=︱a︱︱b︱sinθ,a•b=︱a︱︱b︱cosθ //这里的R_z和R_sp_z都是单位向量,模值为1,因此误差向量e_R(a×b叉积就是误差)的模就是sinθ,点积就是cosθ。 /*** ardupilot/modules/PX4Firmware/src/lib/mathlib/math/Vector.hpp *** float length() const { *** float res = 0.0f; *** *** for (unsigned int i = 0; i < N; i++) *** res += data[i] * data[i]; *** *** return sqrtf(res); *** } ***/ /* calculate weight for yaw control */计算yaw控制的权重 float yaw_w = R_sp(2, 2) * R_sp(2, 2);//姿态设定值矩阵的第三行第三列元素的平方 /* calculate rotation matrix after roll/pitch only rotation */ math::Matrix<3, 3> R_rp; if (e_R_z_sin > 0.0f) { /* get axis-angle representation */ float e_R_z_angle = atan2f(e_R_z_sin, e_R_z_cos);//得到z轴的误差角度 math::Vector<3> e_R_z_axis = e_R / e_R_z_sin; /*** R.transposed() * (R_z % R_sp_z) *** ----------------------------------- *** ||R.transposed() * (R_z % R_sp_z)|| ***/ /***********这些式子的数据具体怎么算的可以看得出来,具体含义应该与理论知识有关吧**********/ e_R = e_R_z_axis * e_R_z_angle; /* cross product matrix for e_R_axis */ math::Matrix<3, 3> e_R_cp; e_R_cp.zero(); e_R_cp(0, 1) = -e_R_z_axis(2); e_R_cp(0, 2) = e_R_z_axis(1); e_R_cp(1, 0) = e_R_z_axis(2); e_R_cp(1, 2) = -e_R_z_axis(0); e_R_cp(2, 0) = -e_R_z_axis(1); e_R_cp(2, 1) = e_R_z_axis(0); /* rotation matrix for roll/pitch only rotation */ R_rp = R * (_I + e_R_cp * e_R_z_sin + e_R_cp * e_R_cp * (1.0f - e_R_z_cos)); } else { /* zero roll/pitch rotation */ R_rp = R; } /* R_rp and R_sp has the same Z axis, calculate yaw error */ math::Vector<3> R_sp_x(R_sp(0, 0), R_sp(1, 0), R_sp(2, 0)); math::Vector<3> R_rp_x(R_rp(0, 0), R_rp(1, 0), R_rp(2, 0)); e_R(2) = atan2f((R_rp_x % R_sp_x) * R_sp_z, R_rp_x * R_sp_x) * yaw_w; if (e_R_z_cos < 0.0f) { /* for large thrust vector rotations use another rotation method: * calculate angle and axis for R -> R_sp rotation directly */ math::Quaternion q; q.from_dcm(R.transposed() * R_sp); math::Vector<3> e_R_d = q.imag(); e_R_d.normalize(); e_R_d *= 2.0f * atan2f(e_R_d.length(), q(0)); /* use fusion of Z axis based rotation and direct rotation */ float direct_w = e_R_z_cos * e_R_z_cos * yaw_w;//计算权重 e_R = e_R * (1.0f - direct_w) + e_R_d * direct_w; } ///////////////////计算角速度设定值 也是矩阵表达的1*3矩阵/////////////////////// /* calculate angular rates setpoint */ _rates_sp = _params.att_p.emult(e_R); /* limit rates */限制角速度大小 for (int i = 0; i < 3; i++) { if (_v_control_mode.flag_control_velocity_enabled && !_v_control_mode.flag_control_manual_enabled) { _rates_sp(i) = math::constrain(_rates_sp(i), -_params.auto_rate_max(i), _params.auto_rate_max(i)); } else { _rates_sp(i) = math::constrain(_rates_sp(i), -_params.mc_rate_max(i), _params.mc_rate_max(i)); } } //////////////////风向标模式,抑制yaw角速度(风向标模式是定航向的意思吗??)/////////////// //////////////////yaw控制失能、速度模式使能、非手动模式///////////////////////// /* weather-vane mode, dampen yaw rate */ if (_v_att_sp.disable_mc_yaw_control == true && _v_control_mode.flag_control_velocity_enabled && !_v_control_mode.flag_control_manual_enabled) { float wv_yaw_rate_max = _params.auto_rate_max(2) * _params.vtol_wv_yaw_rate_scale; _rates_sp(2) = math::constrain(_rates_sp(2), -wv_yaw_rate_max, wv_yaw_rate_max); // prevent integrator winding up in weathervane mode _rates_int(2) = 0.0f; } ///////////////////反馈角速度设定值(2) 矩阵中第三个元素///////////////// /* feed forward yaw setpoint rate */ _rates_sp(2) += _v_att_sp.yaw_sp_move_rate * yaw_w * _params.yaw_ff; ///////////////////风向标模式,减小yaw角速度/////////////////// /* weather-vane mode, scale down yaw rate */ if (_v_att_sp.disable_mc_yaw_control == true && _v_control_mode.flag_control_velocity_enabled && !_v_control_mode.flag_control_manual_enabled) { float wv_yaw_rate_max = _params.auto_rate_max(2) * _params.vtol_wv_yaw_rate_scale; _rates_sp(2) = math::constrain(_rates_sp(2), -wv_yaw_rate_max, wv_yaw_rate_max); // prevent integrator winding up in weathervane mode _rates_int(2) = 0.0f; } }
/* * Attitude rates controller. * Input: '_rates_sp' vector, '_thrust_sp' * Output: '_att_control' vector */ /*************************注意看注释!!********************/ /************输入是角速度设定值向量、油门设定值*************/ /**************************姿态控制值***********************/ void MulticopterAttitudeControl::control_attitude_rates(float dt) { /* reset integral if disarmed */如果锁定,则复位角速度积分 if (!_armed.armed || !_vehicle_status.is_rotary_wing) { _rates_int.zero(); } //////////////////当前机体角速度设定值////////////////////// /////////由此可知_ctrl_state表示的传感器测得的机体状态//////////// //_ctrl_state来源应该是ardupilot/modules/PX4Firmware/src/modules/attitude_estimator_q/attitude_estimator_q.main.cpp// /* current body angular rates */ math::Vector<3> rates; rates(0) = _ctrl_state.roll_rate; rates(1) = _ctrl_state.pitch_rate; rates(2) = _ctrl_state.yaw_rate; ////////////角速度误差=角速度设定值-机体角速度/////////////// /* angular rates error */ math::Vector<3> rates_err = _rates_sp - rates; /////////////////_att_control的三个量分别为pitch、roll、yaw方向的pwm值//////////////// /*** _att_control的三个量就是下面的Roll、pitch、yaw *** Motor1=(int)(COMD.THR + Roll - pitch + yaw); *** Motor2=(int)(COMD.THR + Roll + pitch - yaw); *** Motor3=(int)(COMD.THR - Roll + pitch + yaw); *** Motor4=(int)(COMD.THR - Roll - pitch - yaw); ***/ _att_control = _params.rate_p.emult(rates_err) + _params.rate_d.emult(_rates_prev - rates) / dt + _rates_int + _params.rate_ff.emult(_rates_sp - _rates_sp_prev) / dt; _rates_sp_prev = _rates_sp; _rates_prev = rates; //注意这里用的算法pwm=P*error+D*(角速度_last-角速度_now)/dt+角速度积分+(角速度设定值_now-角速度设定值_last)/dt /*** Matrix<Type, M, N> emult(const Matrix<Type, M, N> &other) const *** { *** Matrix<Type, M, N> res; *** const Matrix<Type, M, N> &self = *this; *** *** for (size_t i = 0; i < M; i++) { *** for (size_t j = 0; j < N; j++) { *** res(i , j) = self(i, j)*other(i, j); *** } *** } *** *** return res; *** } ***/ /* update integral only if not saturated on low limit and if motor commands are not saturated */ /////////////////////下面是积分的计算,注意积分饱和问题//////////////// if (_thrust_sp > MIN_TAKEOFF_THRUST && !_motor_limits.lower_limit && !_motor_limits.upper_limit) { for (int i = 0; i < 3; i++) { if (fabsf(_att_control(i)) < _thrust_sp) { float rate_i = _rates_int(i) + _params.rate_i(i) * rates_err(i) * dt; if (PX4_ISFINITE(rate_i) && rate_i > -RATES_I_LIMIT && rate_i < RATES_I_LIMIT && _att_control(i) > -RATES_I_LIMIT && _att_control(i) < RATES_I_LIMIT) { _rates_int(i) = rate_i; } } } } }
<pre name="code" class="cpp"> ardupilot/modules/PX4Firmware/src/modules/position_estimator_inav/position_estimator_inav_main.cpp int parameter_update_sub = orb_subscribe(ORB_ID(parameter_update)); int actuator_sub = orb_subscribe(ORB_ID_VEHICLE_ATTITUDE_CONTROLS); int armed_sub = orb_subscribe(ORB_ID(actuator_armed)); int sensor_combined_sub = orb_subscribe(ORB_ID(sensor_combined)); int vehicle_attitude_sub = orb_subscribe(ORB_ID(vehicle_attitude)); int optical_flow_sub = orb_subscribe(ORB_ID(optical_flow)); int vehicle_gps_position_sub = orb_subscribe(ORB_ID(vehicle_gps_position)); int vision_position_estimate_sub = orb_subscribe(ORB_ID(vision_position_estimate)); int att_pos_mocap_sub = orb_subscribe(ORB_ID(att_pos_mocap)); int distance_sensor_sub = orb_subscribe(ORB_ID(distance_sensor)); int vehicle_rate_sp_sub = orb_subscribe(ORB_ID(vehicle_rates_setpoint)); orb_copy(ORB_ID(sensor_combined), sensor_combined_sub, &sensor); orb_copy(ORB_ID(vehicle_attitude), vehicle_attitude_sub, &att); orb_copy(ORB_ID(parameter_update), parameter_update_sub, &update); orb_copy(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, actuator_sub, &actuator);//actuator_controls_0 orb_copy(ORB_ID(actuator_armed), armed_sub, &armed); orb_copy(ORB_ID(distance_sensor), distance_sensor_sub, &lidar); orb_copy(ORB_ID(optical_flow), optical_flow_sub, &flow); orb_copy(ORB_ID(vehicle_rates_setpoint), vehicle_rate_sp_sub, &rates_setpoint); orb_copy(ORB_ID(vision_position_estimate), vision_position_estimate_sub, &vision); orb_copy(ORB_ID(att_pos_mocap), att_pos_mocap_sub, &mocap); orb_copy(ORB_ID(vehicle_gps_position), vehicle_gps_position_sub, &gps); orb_advert_t vehicle_local_position_pub = orb_advertise(ORB_ID(vehicle_local_position), &local_pos); orb_publish(ORB_ID(vehicle_local_position), vehicle_local_position_pub, &local_pos); vehicle_global_position_pub = orb_advertise(ORB_ID(vehicle_global_position), &global_pos); orb_publish(ORB_ID(vehicle_global_position), vehicle_global_position_pub, &global_pos); ardupilot/modules/PX4Firmware/src/modules/attitude_estimator_q/attitude_estimator_q_main.cpp _sensors_sub = orb_subscribe(ORB_ID(sensor_combined)); _vision_sub = orb_subscribe(ORB_ID(vision_position_estimate)); _mocap_sub = orb_subscribe(ORB_ID(att_pos_mocap)); _airspeed_sub = orb_subscribe(ORB_ID(airspeed)); _params_sub = orb_subscribe(ORB_ID(parameter_update)); _global_pos_sub = orb_subscribe(ORB_ID(vehicle_global_position)); orb_copy(ORB_ID(sensor_combined), _sensors_sub, &sensors) orb_copy(ORB_ID(vision_position_estimate), _vision_sub, &_vision); orb_copy(ORB_ID(att_pos_mocap), _mocap_sub, &_mocap); orb_copy(ORB_ID(airspeed), _airspeed_sub, &_airspeed); orb_copy(ORB_ID(vehicle_global_position), _global_pos_sub, &_gpos); orb_copy(ORB_ID(att_pos_mocap), _mocap_sub, &_mocap); orb_publish_auto(ORB_ID(vehicle_attitude), &_att_pub, &att, &att_inst, ORB_PRIO_HIGH); orb_publish_auto(ORB_ID(control_state), &_ctrl_state_pub, &ctrl_state, &ctrl_inst, ORB_PRIO_HIGH); orb_publish_auto(ORB_ID(estimator_status), &_est_state_pub, &est, &est_inst, ORB_PRIO_HIGH); ardupilot/modules/PX4Firmware/src/modules/mc_pos_control/mc_pos_control_main.cpp _vehicle_status_sub = orb_subscribe(ORB_ID(vehicle_status)); _ctrl_state_sub = orb_subscribe(ORB_ID(control_state)); _att_sp_sub = orb_subscribe(ORB_ID(vehicle_attitude_setpoint)); _control_mode_sub = orb_subscribe(ORB_ID(vehicle_control_mode)); _params_sub = orb_subscribe(ORB_ID(parameter_update)); _manual_sub = orb_subscribe(ORB_ID(manual_control_setpoint)); _arming_sub = orb_subscribe(ORB_ID(actuator_armed)); _local_pos_sub = orb_subscribe(ORB_ID(vehicle_local_position)); _pos_sp_triplet_sub = orb_subscribe(ORB_ID(position_setpoint_triplet)); _local_pos_sp_sub = orb_subscribe(ORB_ID(vehicle_local_position_setpoint)); _global_vel_sp_sub = orb_subscribe(ORB_ID(vehicle_global_velocity_setpoint)); orb_copy(ORB_ID(vehicle_status), _vehicle_status_sub, &_vehicle_status); orb_copy(ORB_ID(control_state), _ctrl_state_sub, &_ctrl_state); orb_copy(ORB_ID(vehicle_attitude_setpoint), _att_sp_sub, &_att_sp); orb_copy(ORB_ID(vehicle_control_mode), _control_mode_sub, &_control_mode); orb_copy(ORB_ID(manual_control_setpoint), _manual_sub, &_manual); orb_copy(ORB_ID(actuator_armed), _arming_sub, &_arming); orb_copy(ORB_ID(vehicle_local_position), _local_pos_sub, &_local_pos); orb_copy(ORB_ID(position_setpoint_triplet), _pos_sp_triplet_sub, &_pos_sp_triplet); orb_publish(_attitude_setpoint_id, _att_sp_pub, &_att_sp); _att_sp_pub = orb_advertise(_attitude_setpoint_id, &_att_sp); if (!_attitude_setpoint_id) { if (_vehicle_status.is_vtol) { _attitude_setpoint_id = ORB_ID(mc_virtual_attitude_setpoint); } else { _attitude_setpoint_id = ORB_ID(vehicle_attitude_setpoint); } } orb_publish(ORB_ID(vehicle_global_velocity_setpoint), _global_vel_sp_pub, &_global_vel_sp); _global_vel_sp_pub = orb_advertise(ORB_ID(vehicle_global_velocity_setpoint), &_global_vel_sp); ardupilot/modules/PX4Firmware/src/modules/mc_att_control/mc_att_control_main.cpp _v_att_sp_sub = orb_subscribe(ORB_ID(vehicle_attitude_setpoint)); _v_rates_sp_sub = orb_subscribe(ORB_ID(vehicle_rates_setpoint)); _ctrl_state_sub = orb_subscribe(ORB_ID(control_state)); _v_control_mode_sub = orb_subscribe(ORB_ID(vehicle_control_mode)); _params_sub = orb_subscribe(ORB_ID(parameter_update)); _manual_control_sp_sub = orb_subscribe(ORB_ID(manual_control_setpoint)); _armed_sub = orb_subscribe(ORB_ID(actuator_armed)); _vehicle_status_sub = orb_subscribe(ORB_ID(vehicle_status)); _motor_limits_sub = orb_subscribe(ORB_ID(multirotor_motor_limits)); orb_copy(ORB_ID(vehicle_attitude_setpoint), _v_att_sp_sub, &_v_att_sp); orb_copy(ORB_ID(control_state), _ctrl_state_sub, &_ctrl_state); orb_copy(ORB_ID(parameter_update), _params_sub, ¶m_update); orb_copy(ORB_ID(vehicle_control_mode), _v_control_mode_sub, &_v_control_mode); orb_copy(ORB_ID(actuator_armed), _armed_sub, &_armed); orb_copy(ORB_ID(manual_control_setpoint), _manual_control_sp_sub, &_manual_control_sp); orb_copy(ORB_ID(vehicle_status), _vehicle_status_sub, &_vehicle_status); if (!_rates_sp_id) { if (_vehicle_status.is_vtol) { _rates_sp_id = ORB_ID(mc_virtual_rates_setpoint); _actuators_id = ORB_ID(actuator_controls_virtual_mc); } else { _rates_sp_id = ORB_ID(vehicle_rates_setpoint); _actuators_id = ORB_ID(actuator_controls_0); } } } orb_copy(ORB_ID(multirotor_motor_limits), _motor_limits_sub, &_motor_limits); orb_copy(ORB_ID(vehicle_rates_setpoint), _v_rates_sp_sub, &_v_rates_sp); orb_publish(_rates_sp_id, _v_rates_sp_pub, &_v_rates_sp); _v_rates_sp_pub = orb_advertise(_rates_sp_id, &_v_rates_sp); orb_publish(_actuators_id, _actuators_0_pub, &_actuators); _actuators_0_pub = orb_advertise(_actuators_id, &_actuators); orb_publish(ORB_ID(mc_att_ctrl_status), _controller_status_pub, &_controller_status); _controller_status_pub = orb_advertise(ORB_ID(mc_att_ctrl_status), &_controller_status);
#ifdef __cplusplus struct __EXPORT vehicle_local_position_s { #else struct vehicle_local_position_s { #endif uint64_t timestamp; bool xy_valid; bool z_valid; bool v_xy_valid; bool v_z_valid; float x; float y; float z; float vx; float vy; float vz; float yaw; bool xy_global; bool z_global; uint64_t ref_timestamp; double ref_lat; double ref_lon; float ref_alt; float dist_bottom; float dist_bottom_rate; uint64_t surface_bottom_timestamp; bool dist_bottom_valid; float eph; float epv; #ifdef __cplusplus #endif }; #ifdef __cplusplus struct __EXPORT vehicle_global_position_s { #else struct vehicle_global_position_s { #endif uint64_t timestamp; uint64_t time_utc_usec; double lat; double lon; float alt; float vel_n; float vel_e; float vel_d; float yaw; float eph; float epv; float terrain_alt; bool terrain_alt_valid; bool dead_reckoning; float pressure_alt; #ifdef __cplusplus #endif }; #ifdef __cplusplus struct __EXPORT vehicle_attitude_s { #else struct vehicle_attitude_s { #endif uint64_t timestamp; float roll; float pitch; float yaw; float rollspeed; float pitchspeed; float yawspeed; float rollacc; float pitchacc; float yawacc; float rate_vibration; float accel_vibration; float mag_vibration; float rate_offsets[3]; float R[9]; float q[4]; float g_comp[3]; bool R_valid; bool q_valid; #ifdef __cplusplus #endif }; #ifdef __cplusplus struct __EXPORT control_state_s { #else struct control_state_s { #endif uint64_t timestamp; float x_acc; float y_acc; float z_acc; float x_vel; float y_vel; float z_vel; float x_pos; float y_pos; float z_pos; float airspeed; bool airspeed_valid; float vel_variance[3]; float pos_variance[3]; float q[4]; float roll_rate; float pitch_rate; float yaw_rate; float horz_acc_mag; #ifdef __cplusplus #endif }; #ifdef __cplusplus struct __EXPORT vehicle_attitude_setpoint_s { #else struct vehicle_attitude_setpoint_s { #endif uint64_t timestamp; float roll_body; float pitch_body; float yaw_body; float yaw_sp_move_rate; float R_body[9]; bool R_valid; float q_d[4]; bool q_d_valid; float q_e[4]; bool q_e_valid; float thrust; bool roll_reset_integral; bool pitch_reset_integral; bool yaw_reset_integral; bool fw_control_yaw; bool disable_mc_yaw_control; bool apply_flaps; #ifdef __cplusplus #endif }; #ifdef __cplusplus struct __EXPORT vehicle_rates_setpoint_s { #else struct vehicle_rates_setpoint_s { #endif uint64_t timestamp; float roll; float pitch; float yaw; float thrust; #ifdef __cplusplus #endif }; #ifdef __cplusplus struct __EXPORT actuator_controls_0_s { #else struct actuator_controls_0_s { #endif uint64_t timestamp; uint64_t timestamp_sample; float control[8]; #ifdef __cplusplus static const uint8_t NUM_ACTUATOR_CONTROLS = 8; static const uint8_t NUM_ACTUATOR_CONTROL_GROUPS = 4; #endif };这是用到的结构体