pixhawk 光流--位置估计--姿态估计--位置控制--姿态控制
本文是边分析边写的，顺便就记录下了分析的思路，并不是按照教材那种思路介绍性的，而是按照程序员分析程序的思路来的。所以读者跟着看有些地方看了意义不大，但是这种程序分析的思路还是可以借鉴的，如果有大神看到了本文，有更好的见解，欢迎指教。
前面的是从APM代码角度看的，后面是从原生码角度看的。这blog写的我自己看了都想吐，翻下去都是代码，就最后一张图人性化一点。
温馨提示：可以从后面点看，程序中有注释。
先从ardupilot/libraries/AP_OpticalFlow/examples/AP_OpticalFlow_test/ AP_OpticalFlow_test.cpp看起
voidsetup()
{
hal.console->println("OpticalFlowlibrary test ver 1.6");
hal.scheduler->delay(1000);
// flowSensor initialization
optflow.init();
if (!optflow.healthy()) {
hal.console->print("Failed toinitialise PX4Flow ");
}
hal.scheduler->delay(1000);只需要看
}
voidloop()
{
hal.console->println("this onlytests compilation succeeds");
hal.scheduler->delay(5000);
}因此optflow.init();再进行判断if (!optflow.healthy())，任务就完成了
跳转到optflow.init();
voidOpticalFlow::init(void)
{
if (backend != NULL) {
backend->init();
} else {
_enabled = 0;
}
}跳转到init();
classOpticalFlow_backend
{
friend class OpticalFlow;
public:
// constructor
OpticalFlow_backend(OpticalFlow&_frontend) : frontend(_frontend) {}
// init - initialise sensor
virtual void init() = 0;
// read latest values from sensor and fillin x,y and totals.
virtual void update() = 0;
protected:
// access to frontend
OpticalFlow &frontend;
// update the frontend
void _update_frontend(const structOpticalFlow::OpticalFlow_state &state);
// get the flow scaling parameters
Vector2f _flowScaler(void) const { returnVector2f(frontend._flowScalerX, frontend._flowScalerY); }
// get the yaw angle in radians
float _yawAngleRad(void) const { returnradians(float(frontend._yawAngle_cd) * 0.01f); }
};看到了吧，virtualvoid init() = 0;就是这个虚函数
怎么找到它的实例化呢？
Ctrl+H全局搜索public OpticalFlow_backend
路径是：ardupilot/libraries/AP_OpticalFlow/ AP_OpticalFlow_PX4.h
路径是：ardupilot/libraries/AP_OpticalFlow/ AP_OpticalFlow_PX4.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:: initvoid 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 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");
}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
}/****************************************************************************
* 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于是进入了“文件操作”
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 */
};看注释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(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;
} 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;
}
}继续看 measure()和 collect()(read函数中)
int
PX4FLOW::measure()
{
int ret;
/*
* Send the command to begin a measurement.
*/
uint8_t cmd = PX4FLOW_REG;
ret = transfer(&cmd, 1, nullptr, 0);
if (OK != ret) {
perf_count(_comms_errors);
DEVICE_DEBUG("i2c::transfer returned %d", ret);
return ret;
}
ret = OK;
return ret;
}/* PX4FLOW 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(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;
};#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(f_integral.pixel_flow_x_integral) / 10000.0f;//convert to radians
report.pixel_flow_y_integral = static_cast(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(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(f_integral.gyro_x_rate_integral) / 10000.0f; //convert to radians
report.gyro_y_rate_integral = static_cast(f_integral.gyro_y_rate_integral) / 10000.0f; //convert to radians
report.gyro_z_rate_integral = static_cast(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;再换个地方，看read数据之后如何用于控制的
全局搜索ORB_ID(optical_flow)，看哪个地方订阅了光流消息
有2个地方orb_copy(ORB_ID(optical_flow), optical_flow_sub, &flow);，再根据rc.mc_apps
默认采用的是
姿态估计 Attitude_estimator_q
位置估计 position_estimator_inav姿态控制 mc_att_control
位置控制 mc_pos_control
EKF2暂时先不考虑
接下来要看position_estimator_inav _main.cpp(从常识也知道光流是用来获取空间xy方向的位置信息，所以之后肯定是用于位置估计上)
那么先宏观上分析下这个 position_estimator_inav _main.cpp#include
#include
#include
#include
#include
#include
#include
#include
#include
#include
#include
#include
#include
#include
#include
#include
#include
#include
#include
#include
#include
#include
#include
#include
#include
#include
#include
#include
#include
#include
#include
#include
#include
#include
#include
#include "position_estimator_inav_params.h"
#include "inertial_filter.h" extern "C" __EXPORT int position_estimator_inav_main(int argc, char *argv[]);这里顺便猜想下，每个这样的功能函数或者文件，都有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/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;
}
int
MulticopterPositionControl::start()
{
ASSERT(_control_task == -1);
/* start the task */
_control_task = px4_task_spawn_cmd("mc_pos_control",
SCHED_DEFAULT,
SCHED_PRIORITY_MAX - 5,
1900,
(px4_main_t)&MulticopterPositionControl::task_main_trampoline,
nullptr);
if (_control_task < 0) {
warn("task start failed");
return -errno;
}
return OK;
}void
MulticopterPositionControl::task_main_trampoline(int argc, char *argv[])
{
pos_control::g_control->task_main();
}void
MulticopterPositionControl::task_main()
{
/***************************订阅各种信息***********************/
/*
* do subscriptions
*/
_vehicle_status_sub = orb_subscribe(ORB_ID(vehicle_status));
_ctrl_state_sub = orb_subscribe(ORB_ID(control_state));
_att_sp_sub = orb_subscribe(ORB_ID(vehicle_attitude_setpoint));
_control_mode_sub = orb_subscribe(ORB_ID(vehicle_control_mode));
_params_sub = orb_subscribe(ORB_ID(parameter_update));
_manual_sub = orb_subscribe(ORB_ID(manual_control_setpoint));
_arming_sub = orb_subscribe(ORB_ID(actuator_armed));
_local_pos_sub = orb_subscribe(ORB_ID(vehicle_local_position));
//这个就是从position_estimator_inav_main.cpp中来的，里面包含光流数据
_pos_sp_triplet_sub = orb_subscribe(ORB_ID(position_setpoint_triplet));
_local_pos_sp_sub = orb_subscribe(ORB_ID(vehicle_local_position_setpoint));
_global_vel_sp_sub = orb_subscribe(ORB_ID(vehicle_global_velocity_setpoint));
parameters_update(true);//参数跟新
/*****************初始化关键结构的值，直到第一次定期更新*****************/
/* initialize values of critical structs until first regular update */
_arming.armed = false;
/*****************初始跟新，获取所有传感器的数据和状态数据*****************/
/* get an initial update for all sensor and status data */
poll_subscriptions();//具体在下面展开了
bool reset_int_z = true;
bool reset_int_z_manual = false;
bool reset_int_xy = true;
bool reset_yaw_sp = true;
bool was_armed = false;
hrt_abstime t_prev = 0;
math::Vector<3> thrust_int;
thrust_int.zero();
math::Matrix<3, 3> R;
R.identity();
/* wakeup source */
px4_pollfd_struct_t fds[1];
fds[0].fd = _local_pos_sub;
fds[0].events = POLLIN;
/***********************大循环***************************/
while (!_task_should_exit) {
/* wait for up to 500ms for data */等待500ms来获取数据
int pret = px4_poll(&fds[0], (sizeof(fds) / sizeof(fds[0])), 500);
/* timed out - periodic check for _task_should_exit */
if (pret == 0) {
continue;
}
/* this is undesirable but not much we can do */
if (pret < 0) {
warn("poll error %d, %d", pret, errno);
continue;
}
//////////////获取所有传感器的数据和状态数据//////////////////////
poll_subscriptions();
///////////////////////////参数跟新////////////////////////
parameters_update(false);
hrt_abstime t = hrt_absolute_time();
float dt = t_prev != 0 ? (t - t_prev) * 0.000001f : 0.0f;//dt是控制周期吗？
t_prev = t;
// set dt for control blocks
setDt(dt);
/////////////////////////判断是否锁定////////////////////////////
if (_control_mode.flag_armed && !was_armed) {
/* reset setpoints and integrals on arming */
_reset_pos_sp = true;
_reset_alt_sp = true;
_vel_sp_prev.zero();
reset_int_z = true;
reset_int_xy = true;
reset_yaw_sp = true;
}
//////////////////复位yaw和姿态的设定 为垂直起降 fw模式/////////////////////
/* reset yaw and altitude setpoint for VTOL which are in fw mode */
if (_vehicle_status.is_vtol) {
if (!_vehicle_status.is_rotary_wing) {
reset_yaw_sp = true;
_reset_alt_sp = true;
}
}
////////////////////跟新前一时刻锁定的状态///////////////////////
//Update previous arming state
was_armed = _control_mode.flag_armed;
update_ref();//跟新参考点，是不是期望目标？该函数在下面会展开
////////////////////跟新速度导数(加速度) 独立于当前飞行模式///////////
/* Update velocity derivative,
* independent of the current flight mode
*/
if (_local_pos.timestamp > 0) {
if (PX4_ISFINITE(_local_pos.x) && //ISFINITE是测试数据是否是个有限数的函数
PX4_ISFINITE(_local_pos.y) &&
PX4_ISFINITE(_local_pos.z)) {
_pos(0) = _local_pos.x;
_pos(1) = _local_pos.y;
_pos(2) = _local_pos.z;
}
if (PX4_ISFINITE(_local_pos.vx) &&
PX4_ISFINITE(_local_pos.vy) &&
PX4_ISFINITE(_local_pos.vz)) {
_vel(0) = _local_pos.vx;
_vel(1) = _local_pos.vy;
_vel(2) = _local_pos.vz;
}
_vel_err_d(0) = _vel_x_deriv.update(-_vel(0));
_vel_err_d(1) = _vel_y_deriv.update(-_vel(1));
_vel_err_d(2) = _vel_z_deriv.update(-_vel(2));
}
///////////////复位在非手动模式下的水平垂直位置保持标志位，让位置和姿态不被控制//////////////////////
// reset the horizontal and vertical position hold flags for non-manual modes
// or if position / altitude is not controlled
if (!_control_mode.flag_control_position_enabled || !_control_mode.flag_control_manual_enabled) {
_pos_hold_engaged = false;
}
if (!_control_mode.flag_control_altitude_enabled || !_control_mode.flag_control_manual_enabled) {
_alt_hold_engaged = false;
}
//////////////////根据控制模式选择对应的控制函数//////////////////////
if (_control_mode.flag_control_altitude_enabled ||
_control_mode.flag_control_position_enabled ||
_control_mode.flag_control_climb_rate_enabled ||
_control_mode.flag_control_velocity_enabled) {
_vel_ff.zero();
/////////默认运行位置和姿态控制，在control_*函数中可以失能这种模式而直接在循环中运行速度控制模式/////////
/* by default, run position/altitude controller. the control_* functions
* can disable this and run velocity controllers directly in this cycle */
_run_pos_control = true;
_run_alt_control = true;
/////////选择控制源/////////////
/* select control source */
if (_control_mode.flag_control_manual_enabled) {
/* manual control */
control_manual(dt);//手动控制，在下面会展开
_mode_auto = false;
} else if (_control_mode.flag_control_offboard_enabled) {
/* offboard control */
control_offboard(dt);//和control_manual(dt);貌似都是根据具体情况设定期望值
_mode_auto = false;
} else {
/* AUTO */
control_auto(dt);//自动控制
}
/********感觉这3个函数功能是实现根据实际复杂情况调整期望值，之后用控制函数一个一个的达到期望值*************/
/* weather-vane mode for vtol: disable yaw control */风向标模式垂直起降：禁用偏航控制
if (!_control_mode.flag_control_manual_enabled && _pos_sp_triplet.current.disable_mc_yaw_control == true) {
_att_sp.disable_mc_yaw_control = true;
} else {
/* reset in case of setpoint updates */
_att_sp.disable_mc_yaw_control = false;
}
/////////////////////////判断///////////////////////
if (!_control_mode.flag_control_manual_enabled && _pos_sp_triplet.current.valid
&& _pos_sp_triplet.current.type == position_setpoint_s::SETPOINT_TYPE_IDLE) {
/* idle state, don't run controller and set zero thrust */
//////////空闲状态，不运行控制，并设置为零推力//////////
R.identity();
memcpy(&_att_sp.R_body[0], R.data, sizeof(_att_sp.R_body));
_att_sp.R_valid = true;
_att_sp.roll_body = 0.0f;
_att_sp.pitch_body = 0.0f;
_att_sp.yaw_body = _yaw;
_att_sp.thrust = 0.0f;
_att_sp.timestamp = hrt_absolute_time();
/////////////////////发布姿态设定值///////////////////////
/* publish attitude setpoint */
if (_att_sp_pub != nullptr) {
orb_publish(_attitude_setpoint_id, _att_sp_pub, &_att_sp);
} else if (_attitude_setpoint_id) {
_att_sp_pub = orb_advertise(_attitude_setpoint_id, &_att_sp);
}
/////////////////////////另一个判断///////////////////////
} else if (_control_mode.flag_control_manual_enabled
&& _vehicle_status.condition_landed) {
/* don't run controller when landed */着陆不要运行控制器
_reset_pos_sp = true;
_reset_alt_sp = true;
_mode_auto = false;
reset_int_z = true;
reset_int_xy = true;
R.identity();
memcpy(&_att_sp.R_body[0], R.data, sizeof(_att_sp.R_body));
_att_sp.R_valid = true;
_att_sp.roll_body = 0.0f;
_att_sp.pitch_body = 0.0f;
_att_sp.yaw_body = _yaw;
_att_sp.thrust = 0.0f;
_att_sp.timestamp = hrt_absolute_time();
/* publish attitude setpoint */
if (_att_sp_pub != nullptr) {
orb_publish(_attitude_setpoint_id, _att_sp_pub, &_att_sp);
} else if (_attitude_setpoint_id) {
_att_sp_pub = orb_advertise(_attitude_setpoint_id, &_att_sp);
}
/////////////////////////另一个判断///////////////////////
} else {
/* run position & altitude controllers, if enabled (otherwise use already computed velocity setpoints) */
if (_run_pos_control) {
_vel_sp(0) = (_pos_sp(0) - _pos(0)) * _params.pos_p(0);
_vel_sp(1) = (_pos_sp(1) - _pos(1)) * _params.pos_p(1);
}
// do not go slower than the follow target velocity when position tracking is active (set to valid)
if(_pos_sp_triplet.current.type == position_setpoint_s::SETPOINT_TYPE_FOLLOW_TARGET &&
_pos_sp_triplet.current.velocity_valid &&
_pos_sp_triplet.current.position_valid) {
math::Vector<3> ft_vel(_pos_sp_triplet.current.vx, _pos_sp_triplet.current.vy, 0);
float cos_ratio = (ft_vel*_vel_sp)/(ft_vel.length()*_vel_sp.length());
// only override velocity set points when uav is traveling in same direction as target and vector component
// is greater than calculated position set point velocity component
if(cos_ratio > 0) {
ft_vel *= (cos_ratio);
// min speed a little faster than target vel
ft_vel += ft_vel.normalized()*1.5f;
} else {
ft_vel.zero();
}
_vel_sp(0) = fabs(ft_vel(0)) > fabs(_vel_sp(0)) ? ft_vel(0) : _vel_sp(0);
_vel_sp(1) = fabs(ft_vel(1)) > fabs(_vel_sp(1)) ? ft_vel(1) : _vel_sp(1);
// track target using velocity only
} else if (_pos_sp_triplet.current.type == position_setpoint_s::SETPOINT_TYPE_FOLLOW_TARGET &&
_pos_sp_triplet.current.velocity_valid) {
_vel_sp(0) = _pos_sp_triplet.current.vx;
_vel_sp(1) = _pos_sp_triplet.current.vy;
}
if (_run_alt_control) {
_vel_sp(2) = (_pos_sp(2) - _pos(2)) * _params.pos_p(2);
}
/* make sure velocity setpoint is saturated in xy*/
float vel_norm_xy = sqrtf(_vel_sp(0) * _vel_sp(0) +
_vel_sp(1) * _vel_sp(1));
if (vel_norm_xy > _params.vel_max(0)) {
/* note assumes vel_max(0) == vel_max(1) */
_vel_sp(0) = _vel_sp(0) * _params.vel_max(0) / vel_norm_xy;
_vel_sp(1) = _vel_sp(1) * _params.vel_max(1) / vel_norm_xy;
}
/* make sure velocity setpoint is saturated in z*/
float vel_norm_z = sqrtf(_vel_sp(2) * _vel_sp(2));
if (vel_norm_z > _params.vel_max(2)) {
_vel_sp(2) = _vel_sp(2) * _params.vel_max(2) / vel_norm_z;
}
if (!_control_mode.flag_control_position_enabled) {
_reset_pos_sp = true;
}
if (!_control_mode.flag_control_altitude_enabled) {
_reset_alt_sp = true;
}
if (!_control_mode.flag_control_velocity_enabled) {
_vel_sp_prev(0) = _vel(0);
_vel_sp_prev(1) = _vel(1);
_vel_sp(0) = 0.0f;
_vel_sp(1) = 0.0f;
control_vel_enabled_prev = false;
}
if (!_control_mode.flag_control_climb_rate_enabled) {
_vel_sp(2) = 0.0f;
}
/* use constant descend rate when landing, ignore altitude setpoint */
if (!_control_mode.flag_control_manual_enabled && _pos_sp_triplet.current.valid
&& _pos_sp_triplet.current.type == position_setpoint_s::SETPOINT_TYPE_LAND) {
_vel_sp(2) = _params.land_speed;
}
/* special thrust setpoint generation for takeoff from ground */
if (!_control_mode.flag_control_manual_enabled && _pos_sp_triplet.current.valid
&& _pos_sp_triplet.current.type == position_setpoint_s::SETPOINT_TYPE_TAKEOFF
&& _control_mode.flag_armed) {
// check if we are not already in air.
// if yes then we don't need a jumped takeoff anymore
if (!_takeoff_jumped && !_vehicle_status.condition_landed && fabsf(_takeoff_thrust_sp) < FLT_EPSILON) {
_takeoff_jumped = true;
}
if (!_takeoff_jumped) {
// ramp thrust setpoint up
if (_vel(2) > -(_params.tko_speed / 2.0f)) {
_takeoff_thrust_sp += 0.5f * dt;
_vel_sp.zero();
_vel_prev.zero();
} else {
// copter has reached our takeoff speed. split the thrust setpoint up
// into an integral part and into a P part
thrust_int(2) = _takeoff_thrust_sp - _params.vel_p(2) * fabsf(_vel(2));
thrust_int(2) = -math::constrain(thrust_int(2), _params.thr_min, _params.thr_max);
_vel_sp_prev(2) = -_params.tko_speed;
_takeoff_jumped = true;
reset_int_z = false;
}
}
if (_takeoff_jumped) {
_vel_sp(2) = -_params.tko_speed;
}
} else {
_takeoff_jumped = false;
_takeoff_thrust_sp = 0.0f;
}
// limit total horizontal acceleration
math::Vector<2> acc_hor;
acc_hor(0) = (_vel_sp(0) - _vel_sp_prev(0)) / dt;
acc_hor(1) = (_vel_sp(1) - _vel_sp_prev(1)) / dt;
if (acc_hor.length() > _params.acc_hor_max) {
acc_hor.normalize();
acc_hor *= _params.acc_hor_max;
math::Vector<2> vel_sp_hor_prev(_vel_sp_prev(0), _vel_sp_prev(1));
math::Vector<2> vel_sp_hor = acc_hor * dt + vel_sp_hor_prev;
_vel_sp(0) = vel_sp_hor(0);
_vel_sp(1) = vel_sp_hor(1);
}
// limit vertical acceleration
float acc_v = (_vel_sp(2) - _vel_sp_prev(2)) / dt;
if (fabsf(acc_v) > 2 * _params.acc_hor_max) {
acc_v /= fabsf(acc_v);
_vel_sp(2) = acc_v * 2 * _params.acc_hor_max * dt + _vel_sp_prev(2);
}
_vel_sp_prev = _vel_sp;
_global_vel_sp.vx = _vel_sp(0);
_global_vel_sp.vy = _vel_sp(1);
_global_vel_sp.vz = _vel_sp(2);
/* publish velocity setpoint */
if (_global_vel_sp_pub != nullptr) {
orb_publish(ORB_ID(vehicle_global_velocity_setpoint), _global_vel_sp_pub, &_global_vel_sp);
} else {
_global_vel_sp_pub = orb_advertise(ORB_ID(vehicle_global_velocity_setpoint), &_global_vel_sp);
}
if (_control_mode.flag_control_climb_rate_enabled || _control_mode.flag_control_velocity_enabled) {
/* reset integrals if needed */
if (_control_mode.flag_control_climb_rate_enabled) {
if (reset_int_z) {
reset_int_z = false;
float i = _params.thr_min;
if (reset_int_z_manual) {
i = _params.thr_hover;
if (i < _params.thr_min) {
i = _params.thr_min;
} else if (i > _params.thr_max) {
i = _params.thr_max;
}
}
thrust_int(2) = -i;
}
} else {
reset_int_z = true;
}
if (_control_mode.flag_control_velocity_enabled) {
if (reset_int_xy) {
reset_int_xy = false;
thrust_int(0) = 0.0f;
thrust_int(1) = 0.0f;
}
} else {
reset_int_xy = true;
}
/* velocity error */
math::Vector<3> vel_err = _vel_sp - _vel;
////////////检查是否从非速度模式转到速度控制模式，如果是的话，校正xy的速度设定值以便姿态设定值保持不变/////
// check if we have switched from a non-velocity controlled mode into a velocity controlled mode
// if yes, then correct xy velocity setpoint such that the attitude setpoint is continuous
if (!control_vel_enabled_prev && _control_mode.flag_control_velocity_enabled) {
// choose velocity xyz setpoint such that the resulting thrust setpoint has the direction
// given by the last attitude setpoint
_vel_sp(0) = _vel(0) + (-PX4_R(_att_sp.R_body, 0, 2) * _att_sp.thrust - thrust_int(0) - _vel_err_d(0) * _params.vel_d(0)) / _params.vel_p(0);
_vel_sp(1) = _vel(1) + (-PX4_R(_att_sp.R_body, 1, 2) * _att_sp.thrust - thrust_int(1) - _vel_err_d(1) * _params.vel_d(1)) / _params.vel_p(1);
_vel_sp(2) = _vel(2) + (-PX4_R(_att_sp.R_body, 2, 2) * _att_sp.thrust - thrust_int(2) - _vel_err_d(2) * _params.vel_d(2)) / _params.vel_p(2);
_vel_sp_prev(0) = _vel_sp(0);
_vel_sp_prev(1) = _vel_sp(1);
_vel_sp_prev(2) = _vel_sp(2);
control_vel_enabled_prev = true;
// compute updated velocity error
vel_err = _vel_sp - _vel;
}
/* thrust vector in NED frame */
math::Vector<3> thrust_sp = vel_err.emult(_params.vel_p) + _vel_err_d.emult(_params.vel_d) + thrust_int;
if (_pos_sp_triplet.current.type == position_setpoint_s::SETPOINT_TYPE_TAKEOFF
&& !_takeoff_jumped && !_control_mode.flag_control_manual_enabled) {
// for jumped takeoffs use special thrust setpoint calculated above
thrust_sp.zero();
thrust_sp(2) = -_takeoff_thrust_sp;
}
if (!_control_mode.flag_control_velocity_enabled) {
thrust_sp(0) = 0.0f;
thrust_sp(1) = 0.0f;
}
if (!_control_mode.flag_control_climb_rate_enabled) {
thrust_sp(2) = 0.0f;
}
/* limit thrust vector and check for saturation */
bool saturation_xy = false;
bool saturation_z = false;
/* limit min lift */
float thr_min = _params.thr_min;
if (!_control_mode.flag_control_velocity_enabled && thr_min < 0.0f) {
/* don't allow downside thrust direction in manual attitude mode */
thr_min = 0.0f;
}
float thrust_abs = thrust_sp.length();
float tilt_max = _params.tilt_max_air;
float thr_max = _params.thr_max;
/* filter vel_z over 1/8sec */
_vel_z_lp = _vel_z_lp * (1.0f - dt * 8.0f) + dt * 8.0f * _vel(2);
/* filter vel_z change over 1/8sec */
float vel_z_change = (_vel(2) - _vel_prev(2)) / dt;
_acc_z_lp = _acc_z_lp * (1.0f - dt * 8.0f) + dt * 8.0f * vel_z_change;
/* adjust limits for landing mode */调整限制 着陆模式
if (!_control_mode.flag_control_manual_enabled && _pos_sp_triplet.current.valid &&
_pos_sp_triplet.current.type == position_setpoint_s::SETPOINT_TYPE_LAND) {
/* limit max tilt and min lift when landing */
tilt_max = _params.tilt_max_land;
if (thr_min < 0.0f) {
thr_min = 0.0f;
}
/* descend stabilized, we're landing */
if (!_in_landing && !_lnd_reached_ground
&& (float)fabs(_acc_z_lp) < 0.1f
&& _vel_z_lp > 0.5f * _params.land_speed) {
_in_landing = true;
}
/* assume ground, cut thrust */
if (_in_landing
&& _vel_z_lp < 0.1f) {
thr_max = 0.0f;
_in_landing = false;
_lnd_reached_ground = true;
}
/* once we assumed to have reached the ground always cut the thrust.
Only free fall detection below can revoke this
*/
if (!_in_landing && _lnd_reached_ground) {
thr_max = 0.0f;
}
/* if we suddenly fall, reset landing logic and remove thrust limit */
if (_lnd_reached_ground
/* XXX: magic value, assuming free fall above 4m/s2 acceleration */
&& (_acc_z_lp > 4.0f
|| _vel_z_lp > 2.0f * _params.land_speed)) {
thr_max = _params.thr_max;
_in_landing = false;
_lnd_reached_ground = false;
}
} else {
_in_landing = false;
_lnd_reached_ground = false;
}
/* limit min lift */
if (-thrust_sp(2) < thr_min) {
thrust_sp(2) = -thr_min;
saturation_z = true;
}
if (_control_mode.flag_control_velocity_enabled) {
/* limit max tilt */
if (thr_min >= 0.0f && tilt_max < M_PI_F / 2 - 0.05f) {
/* absolute horizontal thrust */
float thrust_sp_xy_len = math::Vector<2>(thrust_sp(0), thrust_sp(1)).length();
if (thrust_sp_xy_len > 0.01f) {
/* max horizontal thrust for given vertical thrust*/
float thrust_xy_max = -thrust_sp(2) * tanf(tilt_max);
if (thrust_sp_xy_len > thrust_xy_max) {
float k = thrust_xy_max / thrust_sp_xy_len;
thrust_sp(0) *= k;
thrust_sp(1) *= k;
saturation_xy = true;
}
}
}
}
if (_control_mode.flag_control_altitude_enabled) {
/* thrust compensation for altitude only control modes */推力的补偿 高度控制模式
float att_comp;
if (_R(2, 2) > TILT_COS_MAX) {
att_comp = 1.0f / _R(2, 2);
} else if (_R(2, 2) > 0.0f) {
att_comp = ((1.0f / TILT_COS_MAX - 1.0f) / TILT_COS_MAX) * _R(2, 2) + 1.0f;
saturation_z = true;
} else {
att_comp = 1.0f;
saturation_z = true;
}
thrust_sp(2) *= att_comp;
}
///////////////////限制最大推力///////////////////
/* limit max thrust */
thrust_abs = thrust_sp.length(); /* recalculate because it might have changed */
if (thrust_abs > thr_max) {
if (thrust_sp(2) < 0.0f) {
if (-thrust_sp(2) > thr_max) {
/* thrust Z component is too large, limit it */
thrust_sp(0) = 0.0f;
thrust_sp(1) = 0.0f;
thrust_sp(2) = -thr_max;
saturation_xy = true;
saturation_z = true;
} else {
/* preserve thrust Z component and lower XY, keeping altitude is more important than position */
float thrust_xy_max = sqrtf(thr_max * thr_max - thrust_sp(2) * thrust_sp(2));
float thrust_xy_abs = math::Vector<2>(thrust_sp(0), thrust_sp(1)).length();
float k = thrust_xy_max / thrust_xy_abs;
thrust_sp(0) *= k;
thrust_sp(1) *= k;
saturation_xy = true;
}
} else {
/* Z component is negative, going down, simply limit thrust vector */
float k = thr_max / thrust_abs;
thrust_sp *= k;
saturation_xy = true;
saturation_z = true;
}
thrust_abs = thr_max;
}
///////////跟新积分//////////
/* update integrals */
if (_control_mode.flag_control_velocity_enabled && !saturation_xy) {
thrust_int(0) += vel_err(0) * _params.vel_i(0) * dt;
thrust_int(1) += vel_err(1) * _params.vel_i(1) * dt;
}
if (_control_mode.flag_control_climb_rate_enabled && !saturation_z) {
thrust_int(2) += vel_err(2) * _params.vel_i(2) * dt;
/* protection against flipping on ground when landing */
if (thrust_int(2) > 0.0f) {
thrust_int(2) = 0.0f;
}
}
/////////////根据推力矢量计算的姿态设定值//////////////
/* calculate attitude setpoint from thrust vector */
if (_control_mode.flag_control_velocity_enabled) {
/* desired body_z axis = -normalize(thrust_vector) */
math::Vector<3> body_x;
math::Vector<3> body_y;
math::Vector<3> body_z;
if (thrust_abs > SIGMA) {
body_z = -thrust_sp / thrust_abs;
} else {
/* no thrust, set Z axis to safe value */
body_z.zero();
body_z(2) = 1.0f;
}
/* vector of desired yaw direction in XY plane, rotated by PI/2 */
math::Vector<3> y_C(-sinf(_att_sp.yaw_body), cosf(_att_sp.yaw_body), 0.0f);
if (fabsf(body_z(2)) > SIGMA) {
/* desired body_x axis, orthogonal to body_z */
body_x = y_C % body_z;
/* keep nose to front while inverted upside down */
if (body_z(2) < 0.0f) {
body_x = -body_x;
}
body_x.normalize();
} else {
/* desired thrust is in XY plane, set X downside to construct correct matrix,
* but yaw component will not be used actually */
body_x.zero();
body_x(2) = 1.0f;
}
/* desired body_y axis */
body_y = body_z % body_x;
/* fill rotation matrix */
for (int i = 0; i < 3; i++) {
R(i, 0) = body_x(i);
R(i, 1) = body_y(i);
R(i, 2) = body_z(i);
}
////////////////////复制旋转矩阵到姿态设定值主题中////////////////////
/* copy rotation matrix to attitude setpoint topic */
memcpy(&_att_sp.R_body[0], R.data, sizeof(_att_sp.R_body));
_att_sp.R_valid = true;
/////////////////////复制四元数设定值到姿态设定值主题中///////////////////////////
/* copy quaternion setpoint to attitude setpoint topic */
math::Quaternion q_sp;
q_sp.from_dcm(R);
memcpy(&_att_sp.q_d[0], &q_sp.data[0], sizeof(_att_sp.q_d));
//////////////计算欧拉角，只记录，不得用于控制//////////////////
/* calculate euler angles, for logging only, must not be used for control */
math::Vector<3> euler = R.to_euler();
_att_sp.roll_body = euler(0);
_att_sp.pitch_body = euler(1);
/* yaw already used to construct rot matrix, but actual rotation matrix can have different yaw near singularity */
/////////偏航已经用于构建rot矩阵，但实际旋转矩阵可以有奇点附近的不同偏航///////////
} else if (!_control_mode.flag_control_manual_enabled) {
/////////////没有位置控制的自主高度控制（故障安全降落），强制水平姿态，不改变yaw///////////////////
/* autonomous altitude control without position control (failsafe landing),
* force level attitude, don't change yaw */
R.from_euler(0.0f, 0.0f, _att_sp.yaw_body);
////////////////////复制旋转矩阵到姿态设定值主题中////////////////////
/* copy rotation matrix to attitude setpoint topic */
memcpy(&_att_sp.R_body[0], R.data, sizeof(_att_sp.R_body));
_att_sp.R_valid = true;
/////////////////////复制四元数设定值到姿态设定值主题中///////////////////////////
/* copy quaternion setpoint to attitude setpoint topic */
math::Quaternion q_sp;
q_sp.from_dcm(R);
memcpy(&_att_sp.q_d[0], &q_sp.data[0], sizeof(_att_sp.q_d));
_att_sp.roll_body = 0.0f;
_att_sp.pitch_body = 0.0f;
}
_att_sp.thrust = thrust_abs;
/* save thrust setpoint for logging */保存推理设定值在logging里
_local_pos_sp.acc_x = thrust_sp(0) * ONE_G;
_local_pos_sp.acc_y = thrust_sp(1) * ONE_G;
_local_pos_sp.acc_z = thrust_sp(2) * ONE_G;
_att_sp.timestamp = hrt_absolute_time();
} else {
reset_int_z = true;
}
}
/* fill local position, velocity and thrust setpoint */
_local_pos_sp.timestamp = hrt_absolute_time();
_local_pos_sp.x = _pos_sp(0);
_local_pos_sp.y = _pos_sp(1);
_local_pos_sp.z = _pos_sp(2);
_local_pos_sp.yaw = _att_sp.yaw_body;
_local_pos_sp.vx = _vel_sp(0);
_local_pos_sp.vy = _vel_sp(1);
_local_pos_sp.vz = _vel_sp(2);
////////////////////////发布当地位置设定值////////////////////
/***************这个应该是这段程序处理的最后结果********************/
/* publish local position setpoint */
if (_local_pos_sp_pub != nullptr) {
orb_publish(ORB_ID(vehicle_local_position_setpoint), _local_pos_sp_pub, &_local_pos_sp);
} else {
_local_pos_sp_pub = orb_advertise(ORB_ID(vehicle_local_position_setpoint), &_local_pos_sp);
}
} else {位置控制模式失能，那么复位设定值
/* position controller disabled, reset setpoints */
_reset_alt_sp = true;
_reset_pos_sp = true;
_mode_auto = false;
reset_int_z = true;
reset_int_xy = true;
control_vel_enabled_prev = false;
/* store last velocity in case a mode switch to position control occurs */
_vel_sp_prev = _vel;
}
//////////////////////从手动控制中产生姿态设定值//////////////////////////
/* generate attitude setpoint from manual controls */
if (_control_mode.flag_control_manual_enabled && _control_mode.flag_control_attitude_enabled) {
/* reset yaw setpoint to current position if needed */
if (reset_yaw_sp) {
reset_yaw_sp = false;
_att_sp.yaw_body = _yaw;
}
//////////////////在地上时，不动偏航方向/////////////////////////
/* do not move yaw while sitting on the ground */
else if (!_vehicle_status.condition_landed &&
!(!_control_mode.flag_control_altitude_enabled && _manual.z < 0.1f)) {
/* we want to know the real constraint, and global overrides manual */
const float yaw_rate_max = (_params.man_yaw_max < _params.global_yaw_max) ? _params.man_yaw_max :
_params.global_yaw_max;
const float yaw_offset_max = yaw_rate_max / _params.mc_att_yaw_p;
_att_sp.yaw_sp_move_rate = _manual.r * yaw_rate_max;
float yaw_target = _wrap_pi(_att_sp.yaw_body + _att_sp.yaw_sp_move_rate * dt);
float yaw_offs = _wrap_pi(yaw_target - _yaw);
////////////////////如果对于系统跟踪，yaw偏移变得过大，那么停止其转移////////////////////
// If the yaw offset became too big for the system to track stop
// shifting it
// XXX this needs inspection - probably requires a clamp, not an if
if (fabsf(yaw_offs) < yaw_offset_max) {
_att_sp.yaw_body = yaw_target;
}
}
//////////////////////直接控制侧倾和俯仰，如果协助速度控制器没有被激活////////////////////
/* control roll and pitch directly if we no aiding velocity controller is active */
if (!_control_mode.flag_control_velocity_enabled) {
_att_sp.roll_body = _manual.y * _params.man_roll_max;
_att_sp.pitch_body = -_manual.x * _params.man_pitch_max;
}
//////////////////////控制油门直接，如果没有爬升率控制器被激活/////////////////////
/* control throttle directly if no climb rate controller is active */
if (!_control_mode.flag_control_climb_rate_enabled) {
float thr_val = throttle_curve(_manual.z, _params.thr_hover);
_att_sp.thrust = math::min(thr_val, _manual_thr_max.get());
/* enforce minimum throttle if not landed */
if (!_vehicle_status.condition_landed) {
_att_sp.thrust = math::max(_att_sp.thrust, _manual_thr_min.get());
}
}
math::Matrix<3, 3> R_sp;
//////////////////////构建姿态设定值的旋转矩阵///////////////////////
/* construct attitude setpoint rotation matrix */
R_sp.from_euler(_att_sp.roll_body, _att_sp.pitch_body, _att_sp.yaw_body);
memcpy(&_att_sp.R_body[0], R_sp.data, sizeof(_att_sp.R_body));
////////////////////为所有非姿态飞行模式 复位加速度设定值/////////////////
/* reset the acceleration set point for all non-attitude flight modes */
if (!(_control_mode.flag_control_offboard_enabled &&
!(_control_mode.flag_control_position_enabled ||
_control_mode.flag_control_velocity_enabled))) {
_thrust_sp_prev = R_sp * math::Vector<3>(0, 0, -_att_sp.thrust);
}
/////////////////////将四元数设定值复制到姿态设定值的主题中/////////////////////////
/* copy quaternion setpoint to attitude setpoint topic */
math::Quaternion q_sp;
q_sp.from_dcm(R_sp);
memcpy(&_att_sp.q_d[0], &q_sp.data[0], sizeof(_att_sp.q_d));
_att_sp.timestamp = hrt_absolute_time();
} else {
reset_yaw_sp = true;
}
/* update previous velocity for velocity controller D part */
_vel_prev = _vel;
/////////////////////////发布设定值//////////////////////////////
/* publish attitude setpoint
* Do not publish if offboard is enabled but position/velocity control is disabled,
* in this case the attitude setpoint is published by the mavlink app. Also do not publish
* if the vehicle is a VTOL and it's just doing a transition (the VTOL attitude control module will generate
* attitude setpoints for the transition).
*/
if (!(_control_mode.flag_control_offboard_enabled &&
!(_control_mode.flag_control_position_enabled ||
_control_mode.flag_control_velocity_enabled))) {
if (_att_sp_pub != nullptr) {
orb_publish(_attitude_setpoint_id, _att_sp_pub, &_att_sp);
} else if (_attitude_setpoint_id) {
_att_sp_pub = orb_advertise(_attitude_setpoint_id, &_att_sp);
}
}
/////////////手动油门控制后,将高度控制器积分（悬停油门）复位到手动油门///////////////
/* reset altitude controller integral (hovering throttle) to manual throttle after manual throttle control */
reset_int_z_manual = _control_mode.flag_armed && _control_mode.flag_control_manual_enabled
&& !_control_mode.flag_control_climb_rate_enabled;
}
mavlink_log_info(&_mavlink_log_pub, "[mpc] stopped");
_control_task = -1;
}void
MulticopterPositionControl::poll_subscriptions()
{
bool updated;
/**********************机体状态跟新**************************/
orb_check(_vehicle_status_sub, &updated);
if (updated) {
orb_copy(ORB_ID(vehicle_status), _vehicle_status_sub, &_vehicle_status);
/* set correct uORB ID, depending on if vehicle is VTOL or not */
if (!_attitude_setpoint_id) {
if (_vehicle_status.is_vtol) {
_attitude_setpoint_id = ORB_ID(mc_virtual_attitude_setpoint);
} else {
_attitude_setpoint_id = ORB_ID(vehicle_attitude_setpoint);
}
}
}
/**********************控制状态跟新**************************/
orb_check(_ctrl_state_sub, &updated);
if (updated) {
orb_copy(ORB_ID(control_state), _ctrl_state_sub, &_ctrl_state);
/* get current rotation matrix and euler angles from control state quaternions */
math::Quaternion q_att(_ctrl_state.q[0], _ctrl_state.q[1], _ctrl_state.q[2], _ctrl_state.q[3]);
_R = q_att.to_dcm();
math::Vector<3> euler_angles;
euler_angles = _R.to_euler();
_yaw = euler_angles(2);
}
/**********************setpoint状态跟新**************************/
orb_check(_att_sp_sub, &updated);
if (updated) {
orb_copy(ORB_ID(vehicle_attitude_setpoint), _att_sp_sub, &_att_sp);
}
orb_check(_control_mode_sub, &updated);
if (updated) {
orb_copy(ORB_ID(vehicle_control_mode), _control_mode_sub, &_control_mode);
}
/**********************手动控制状态跟新**************************/
orb_check(_manual_sub, &updated);
if (updated) {
orb_copy(ORB_ID(manual_control_setpoint), _manual_sub, &_manual);
}
/**********************解锁状态跟新**************************/
orb_check(_arming_sub, &updated);
if (updated) {
orb_copy(ORB_ID(actuator_armed), _arming_sub, &_arming);
}
/**********************位置跟新**************************/
orb_check(_local_pos_sub, &updated);
if (updated) {
orb_copy(ORB_ID(vehicle_local_position), _local_pos_sub, &_local_pos);
}
}void
MulticopterPositionControl::update_ref()
{
if (_local_pos.ref_timestamp != _ref_timestamp) {
double lat_sp, lon_sp;//维度设定、经度设定
float alt_sp = 0.0f;//姿态设定
if (_ref_timestamp != 0) {
/*************在整体框架下计算当前位置设定值***************/
/* calculate current position setpoint in global frame */
map_projection_reproject(&_ref_pos, _pos_sp(0), _pos_sp(1), &lat_sp, &lon_sp);
alt_sp = _ref_alt - _pos_sp(2);
}
/*************跟新投影参考*********************/
/* update local projection reference */
map_projection_init(&_ref_pos, _local_pos.ref_lat, _local_pos.ref_lon);
_ref_alt = _local_pos.ref_alt;
if (_ref_timestamp != 0) {
/*********************设置新的位置设定值*******************/
/* reproject position setpoint to new reference */
map_projection_project(&_ref_pos, lat_sp, lon_sp, &_pos_sp.data[0], &_pos_sp.data[1]);
_pos_sp(2) = -(alt_sp - _ref_alt);
}
_ref_timestamp = _local_pos.ref_timestamp;
}
}void
MulticopterPositionControl::control_manual(float dt)
{
math::Vector<3> req_vel_sp; // X,Y in local frame and Z in global (D), in [-1,1] normalized range
req_vel_sp.zero();
if (_control_mode.flag_control_altitude_enabled) {
/* set vertical velocity setpoint with throttle stick */用油门棒设置垂直速度
req_vel_sp(2) = -scale_control(_manual.z - 0.5f, 0.5f, _params.alt_ctl_dz, _params.alt_ctl_dy); // D
}
if (_control_mode.flag_control_position_enabled) {
/* set horizontal velocity setpoint with roll/pitch stick */用roll/pitch棒设置水平速度
req_vel_sp(0) = _manual.x;
req_vel_sp(1) = _manual.y;
}
if (_control_mode.flag_control_altitude_enabled) {
/* reset alt setpoint to current altitude if needed */复位姿态设定值
reset_alt_sp();
}
if (_control_mode.flag_control_position_enabled) {
/* reset position setpoint to current position if needed */复位位置设定值
reset_pos_sp();
}
/* limit velocity setpoint */限制速度设定值
float req_vel_sp_norm = req_vel_sp.length();
if (req_vel_sp_norm > 1.0f) {
req_vel_sp /= req_vel_sp_norm;//相当于是向量除以它的模值
}
///////////_req_vel_sp的范围是0到1，围绕yaw旋转
/* _req_vel_sp scaled to 0..1, scale it to max speed and rotate around yaw */
math::Matrix<3, 3> R_yaw_sp;
R_yaw_sp.from_euler(0.0f, 0.0f, _att_sp.yaw_body);
math::Vector<3> req_vel_sp_scaled = R_yaw_sp * req_vel_sp.emult(
_params.vel_cruise); // in NED and scaled to actual velocity在NED旋转方式下缩放到实际速度
/////////用户辅助模式：用户控制速度，但是，如果速度足够小，相应的轴的位置就会保持////////////////
/*
* assisted velocity mode: user controls velocity, but if velocity is small enough, position
* hold is activated for the corresponding axis
*/
/* horizontal axes */水平轴
if (_control_mode.flag_control_position_enabled) {
/* check for pos. hold */检测位置的保持状态
if (fabsf(req_vel_sp(0)) < _params.hold_xy_dz && fabsf(req_vel_sp(1)) < _params.hold_xy_dz) {
if (!_pos_hold_engaged) {
if (_params.hold_max_xy < FLT_EPSILON || (fabsf(_vel(0)) < _params.hold_max_xy
&& fabsf(_vel(1)) < _params.hold_max_xy)) {
_pos_hold_engaged = true;
} else {
_pos_hold_engaged = false;
}
}
} else {
_pos_hold_engaged = false;
}
/* set requested velocity setpoint */设置所需的速度设定值
if (!_pos_hold_engaged) {
_pos_sp(0) = _pos(0);
_pos_sp(1) = _pos(1);
//////////速度设定值会被利用，位置设定值不会被用/////////////
_run_pos_control = false; /* request velocity setpoint to be used, instead of position setpoint */
_vel_sp(0) = req_vel_sp_scaled(0);
_vel_sp(1) = req_vel_sp_scaled(1);
}
}
/* vertical axis */垂直轴
if (_control_mode.flag_control_altitude_enabled) {
/* check for pos. hold */
if (fabsf(req_vel_sp(2)) < FLT_EPSILON) {
if (!_alt_hold_engaged) {
if (_params.hold_max_z < FLT_EPSILON || fabsf(_vel(2)) < _params.hold_max_z) {
_alt_hold_engaged = true;
} else {
_alt_hold_engaged = false;
}
}
} else {
_alt_hold_engaged = false;
}
/* set requested velocity setpoint */
if (!_alt_hold_engaged) {
_run_alt_control = false; /* request velocity setpoint to be used, instead of altitude setpoint */
_vel_sp(2) = req_vel_sp_scaled(2);
_pos_sp(2) = _pos(2);
}
}
}#ifdef __cplusplus
struct __EXPORT position_setpoint_triplet_s {
#else
struct position_setpoint_triplet_s {
#endif
struct position_setpoint_s previous;
struct position_setpoint_s current;
struct position_setpoint_s next;
uint8_t nav_state;
#ifdef __cplusplus
#endif
};整个MulticopterPositionControl::task_main()的输出应该是
orb_publish(_attitude_setpoint_id, _att_sp_pub, &_att_sp);
orb_publish(ORB_ID(vehicle_global_velocity_setpoint), _global_vel_sp_pub, &_global_vel_sp);
orb_publish(ORB_ID(vehicle_local_position_setpoint), _local_pos_sp_pub, &_local_pos_sp);
其中orb_publish(_attitude_setpoint_id, _att_sp_pub, &_att_sp);是在main的最下面，应该这个是最后处理的结果
但是全局搜索ORB_ID(vehicle_local_position_setpoint)
没有找到orb_copy(ORB_ID(vehicle_local_position_setpoint) 或者 orb_copy(_attitude_setpoint_id
if (!_attitude_setpoint_id) {
if (_vehicle_status.is_vtol) {
_attitude_setpoint_id = ORB_ID(mc_virtual_attitude_setpoint);
} else {
_attitude_setpoint_id = ORB_ID(vehicle_attitude_setpoint);
}
}
void
MulticopterPositionControl::task_main()
{
/*
* do subscriptions
*/
_vehicle_status_sub = orb_subscribe(ORB_ID(vehicle_status));
_ctrl_state_sub = orb_subscribe(ORB_ID(control_state));
_att_sp_sub = orb_subscribe(ORB_ID(vehicle_attitude_setpoint));
_control_mode_sub = orb_subscribe(ORB_ID(vehicle_control_mode));
_params_sub = orb_subscribe(ORB_ID(parameter_update));
_manual_sub = orb_subscribe(ORB_ID(manual_control_setpoint));
_arming_sub = orb_subscribe(ORB_ID(actuator_armed));
_local_pos_sub = orb_subscribe(ORB_ID(vehicle_local_position));
_pos_sp_triplet_sub = orb_subscribe(ORB_ID(position_setpoint_triplet));
_local_pos_sp_sub = orb_subscribe(ORB_ID(vehicle_local_position_setpoint));
_global_vel_sp_sub = orb_subscribe(ORB_ID(vehicle_global_velocity_setpoint));
由此可以看出最后mc_pos_control_main.cpp最后处理的结果一方面被自己重新订阅，用于不断跟新；另一方面被mc_att_control订阅，用于控制姿态
在进入姿态前，再来理下光流信息如何流动的
在进入姿态前，再来理下光流信息如何流动的
//ardupilot/modules/PX4Firmware/src/drivers/px4flow/px4flow.cpp
orb_publish(ORB_ID(optical_flow), _px4flow_topic, &report);
//ardupilot/modules/PX4Firmware/src/modules/position_estimator_inav/position_estimator_inav_main.cpp
int optical_flow_sub = orb_subscribe(ORB_ID(optical_flow));
orb_copy(ORB_ID(optical_flow), optical_flow_sub, &flow);
orb_publish(ORB_ID(vehicle_local_position), vehicle_local_position_pub, &local_pos);
//ardupilot/modules/PX4Firmware/src/modules/mc_pos_contol/mc_pos_contol_main.cpp
_local_pos_sub = orb_subscribe(ORB_ID(vehicle_local_position));
_local_pos_sub = orb_subscribe(ORB_ID(vehicle_local_position));
orb_copy(ORB_ID(vehicle_local_position), _local_pos_sub, &_local_pos);
orb_publish(_attitude_setpoint_id, _att_sp_pub, &_att_sp);
_att_sp_sub = orb_subscribe(ORB_ID(vehicle_attitude_setpoint));
orb_copy(ORB_ID(vehicle_attitude_setpoint), _att_sp_sub, &_att_sp);
orb_copy(ORB_ID(vehicle_local_position), _local_pos_sub, &_local_pos);
if (!_attitude_setpoint_id) {
if (_vehicle_status.is_vtol) {
_attitude_setpoint_id = ORB_ID(mc_virtual_attitude_setpoint);
} else {
_attitude_setpoint_id = ORB_ID(vehicle_attitude_setpoint);
}
}
_v_att_sp_sub = orb_subscribe(ORB_ID(vehicle_attitude_setpoint));
void
MulticopterAttitudeControl::vehicle_attitude_setpoint_poll()
{
/* check if there is a new setpoint */
bool updated;
orb_check(_v_att_sp_sub, &updated);
if (updated) {
orb_copy(ORB_ID(vehicle_attitude_setpoint), _v_att_sp_sub, &_v_att_sp);
}
}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);
}
}
字面意思应该是控制垂直起降的，怎么什么信息都会发布，都会接收啊？！暂时先放着
恩，产生一个新的想法：可以把这些订阅和发布的所有信息用图反映出来，大概就能直观的察觉到pixhawk的位置姿态估计和控制的结构，可以先看默认启动的那几个cpp文件
现在进入//ardupilot/modules/PX4Firmware/src/modules/mc_att_control/mc_att_control_main.cpp看看（上一篇写的有点乱，这一篇重新写）
extern "C" __EXPORT int mc_att_control_main(int argc, char *argv[]);int mc_att_control_main(int argc, char *argv[])
{
if (argc < 2) {
warnx("usage: mc_att_control {start|stop|status}");
return 1;
}
if (!strcmp(argv[1], "start")) {
if (mc_att_control::g_control != nullptr) {
warnx("already running");
return 1;
}
mc_att_control::g_control = new MulticopterAttitudeControl;
if (mc_att_control::g_control == nullptr) {
warnx("alloc failed");
return 1;
}
if (OK != mc_att_control::g_control->start()) {
delete mc_att_control::g_control;
mc_att_control::g_control = nullptr;
warnx("start failed");
return 1;
}
return 0;
}
if (!strcmp(argv[1], "stop")) {
if (mc_att_control::g_control == nullptr) {
warnx("not running");
return 1;
}
delete mc_att_control::g_control;
mc_att_control::g_control = nullptr;
return 0;
}
if (!strcmp(argv[1], "status")) {
if (mc_att_control::g_control) {
warnx("running");
return 0;
} else {
warnx("not running");
return 1;
}
}
warnx("unrecognized command");
return 1;
}mc_att_control::g_control = new MulticopterAttitudeControl;相当于是分配空间
里面包含
/* fetch initial parameter values */
parameters_update();接着就进了start
int
MulticopterAttitudeControl::start()
{
ASSERT(_control_task == -1);
/* start the task */
_control_task = px4_task_spawn_cmd("mc_att_control",
SCHED_DEFAULT,
SCHED_PRIORITY_MAX - 5,
1500,
(px4_main_t)&MulticopterAttitudeControl::task_main_trampoline,
nullptr);
if (_control_task < 0) {
warn("task start failed");
return -errno;
}
return OK;
}void
MulticopterAttitudeControl::task_main_trampoline(int argc, char *argv[])
{
mc_att_control::g_control->task_main();
}void
MulticopterAttitudeControl::task_main()
{
/**************************订阅各种信息**************************/
/*
* do subscriptions
*/
_v_att_sp_sub = orb_subscribe(ORB_ID(vehicle_attitude_setpoint));
_v_rates_sp_sub = orb_subscribe(ORB_ID(vehicle_rates_setpoint));
_ctrl_state_sub = orb_subscribe(ORB_ID(control_state));
_v_control_mode_sub = orb_subscribe(ORB_ID(vehicle_control_mode));
_params_sub = orb_subscribe(ORB_ID(parameter_update));
_manual_control_sp_sub = orb_subscribe(ORB_ID(manual_control_setpoint));
_armed_sub = orb_subscribe(ORB_ID(actuator_armed));
_vehicle_status_sub = orb_subscribe(ORB_ID(vehicle_status));
_motor_limits_sub = orb_subscribe(ORB_ID(multirotor_motor_limits));
/**************************初始化各种参数**************************/
/* initialize parameters cache */
parameters_update();
/**************************启动机体姿态原函数**************************/
/* wakeup source: vehicle attitude */
px4_pollfd_struct_t fds[1];
fds[0].fd = _ctrl_state_sub;
fds[0].events = POLLIN;
/**************************大循环**************************/
while (!_task_should_exit) {
/////////////////////////////等待100ms 为了获取数据/////////////////////////
/* wait for up to 100ms for data */
int pret = px4_poll(&fds[0], (sizeof(fds) / sizeof(fds[0])), 100);
/* timed out - periodic check for _task_should_exit */
if (pret == 0) {
continue;
}
/* this is undesirable but not much we can do - might want to flag unhappy status */
if (pret < 0) {
warn("mc att ctrl: poll error %d, %d", pret, errno);
/* sleep a bit before next try */
usleep(100000);
continue;
}
perf_begin(_loop_perf);
///////////////////////运行姿态变化控制器//////////////////////////
/* run controller on attitude changes */
if (fds[0].revents & POLLIN) {
static uint64_t last_run = 0;
float dt = (hrt_absolute_time() - last_run) / 1000000.0f;
last_run = hrt_absolute_time();
/////////////////////防止dt太小或太大 2ms 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;
} /**
* 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 transpose() const
*** {
*** Matrix res;
*** const Matrix &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 emult(const Matrix &other) const
*** {
*** Matrix res;
*** const Matrix &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;
}
}
}
}
} ardupilot/modules/PX4Firmware/src/modules/position_estimator_inav/position_estimator_inav_main.cpp
int parameter_update_sub = orb_subscribe(ORB_ID(parameter_update));
int actuator_sub = orb_subscribe(ORB_ID_VEHICLE_ATTITUDE_CONTROLS);
int armed_sub = orb_subscribe(ORB_ID(actuator_armed));
int sensor_combined_sub = orb_subscribe(ORB_ID(sensor_combined));
int vehicle_attitude_sub = orb_subscribe(ORB_ID(vehicle_attitude));
int optical_flow_sub = orb_subscribe(ORB_ID(optical_flow));
int vehicle_gps_position_sub = orb_subscribe(ORB_ID(vehicle_gps_position));
int vision_position_estimate_sub = orb_subscribe(ORB_ID(vision_position_estimate));
int att_pos_mocap_sub = orb_subscribe(ORB_ID(att_pos_mocap));
int distance_sensor_sub = orb_subscribe(ORB_ID(distance_sensor));
int vehicle_rate_sp_sub = orb_subscribe(ORB_ID(vehicle_rates_setpoint));
orb_copy(ORB_ID(sensor_combined), sensor_combined_sub, &sensor);
orb_copy(ORB_ID(vehicle_attitude), vehicle_attitude_sub, &att);
orb_copy(ORB_ID(parameter_update), parameter_update_sub, &update);
orb_copy(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, actuator_sub, &actuator);//actuator_controls_0
orb_copy(ORB_ID(actuator_armed), armed_sub, &armed);
orb_copy(ORB_ID(distance_sensor), distance_sensor_sub, &lidar);
orb_copy(ORB_ID(optical_flow), optical_flow_sub, &flow);
orb_copy(ORB_ID(vehicle_rates_setpoint), vehicle_rate_sp_sub, &rates_setpoint);
orb_copy(ORB_ID(vision_position_estimate), vision_position_estimate_sub, &vision);
orb_copy(ORB_ID(att_pos_mocap), att_pos_mocap_sub, &mocap);
orb_copy(ORB_ID(vehicle_gps_position), vehicle_gps_position_sub, &gps);
orb_advert_t vehicle_local_position_pub = orb_advertise(ORB_ID(vehicle_local_position), &local_pos);
orb_publish(ORB_ID(vehicle_local_position), vehicle_local_position_pub, &local_pos);
vehicle_global_position_pub = orb_advertise(ORB_ID(vehicle_global_position), &global_pos);
orb_publish(ORB_ID(vehicle_global_position), vehicle_global_position_pub, &global_pos);
ardupilot/modules/PX4Firmware/src/modules/attitude_estimator_q/attitude_estimator_q_main.cpp
_sensors_sub = orb_subscribe(ORB_ID(sensor_combined));
_vision_sub = orb_subscribe(ORB_ID(vision_position_estimate));
_mocap_sub = orb_subscribe(ORB_ID(att_pos_mocap));
_airspeed_sub = orb_subscribe(ORB_ID(airspeed));
_params_sub = orb_subscribe(ORB_ID(parameter_update));
_global_pos_sub = orb_subscribe(ORB_ID(vehicle_global_position));
orb_copy(ORB_ID(sensor_combined), _sensors_sub, &sensors)
orb_copy(ORB_ID(vision_position_estimate), _vision_sub, &_vision);
orb_copy(ORB_ID(att_pos_mocap), _mocap_sub, &_mocap);
orb_copy(ORB_ID(airspeed), _airspeed_sub, &_airspeed);
orb_copy(ORB_ID(vehicle_global_position), _global_pos_sub, &_gpos);
orb_copy(ORB_ID(att_pos_mocap), _mocap_sub, &_mocap);
orb_publish_auto(ORB_ID(vehicle_attitude), &_att_pub, &att, &att_inst, ORB_PRIO_HIGH);
orb_publish_auto(ORB_ID(control_state), &_ctrl_state_pub, &ctrl_state, &ctrl_inst, ORB_PRIO_HIGH);
orb_publish_auto(ORB_ID(estimator_status), &_est_state_pub, &est, &est_inst, ORB_PRIO_HIGH);
ardupilot/modules/PX4Firmware/src/modules/mc_pos_control/mc_pos_control_main.cpp
_vehicle_status_sub = orb_subscribe(ORB_ID(vehicle_status));
_ctrl_state_sub = orb_subscribe(ORB_ID(control_state));
_att_sp_sub = orb_subscribe(ORB_ID(vehicle_attitude_setpoint));
_control_mode_sub = orb_subscribe(ORB_ID(vehicle_control_mode));
_params_sub = orb_subscribe(ORB_ID(parameter_update));
_manual_sub = orb_subscribe(ORB_ID(manual_control_setpoint));
_arming_sub = orb_subscribe(ORB_ID(actuator_armed));
_local_pos_sub = orb_subscribe(ORB_ID(vehicle_local_position));
_pos_sp_triplet_sub = orb_subscribe(ORB_ID(position_setpoint_triplet));
_local_pos_sp_sub = orb_subscribe(ORB_ID(vehicle_local_position_setpoint));
_global_vel_sp_sub = orb_subscribe(ORB_ID(vehicle_global_velocity_setpoint));
orb_copy(ORB_ID(vehicle_status), _vehicle_status_sub, &_vehicle_status);
orb_copy(ORB_ID(control_state), _ctrl_state_sub, &_ctrl_state);
orb_copy(ORB_ID(vehicle_attitude_setpoint), _att_sp_sub, &_att_sp);
orb_copy(ORB_ID(vehicle_control_mode), _control_mode_sub, &_control_mode);
orb_copy(ORB_ID(manual_control_setpoint), _manual_sub, &_manual);
orb_copy(ORB_ID(actuator_armed), _arming_sub, &_arming);
orb_copy(ORB_ID(vehicle_local_position), _local_pos_sub, &_local_pos);
orb_copy(ORB_ID(position_setpoint_triplet), _pos_sp_triplet_sub, &_pos_sp_triplet);
orb_publish(_attitude_setpoint_id, _att_sp_pub, &_att_sp);
_att_sp_pub = orb_advertise(_attitude_setpoint_id, &_att_sp);
if (!_attitude_setpoint_id) {
if (_vehicle_status.is_vtol) {
_attitude_setpoint_id = ORB_ID(mc_virtual_attitude_setpoint);
} else {
_attitude_setpoint_id = ORB_ID(vehicle_attitude_setpoint);
}
}
orb_publish(ORB_ID(vehicle_global_velocity_setpoint), _global_vel_sp_pub, &_global_vel_sp);
_global_vel_sp_pub = orb_advertise(ORB_ID(vehicle_global_velocity_setpoint), &_global_vel_sp);
ardupilot/modules/PX4Firmware/src/modules/mc_att_control/mc_att_control_main.cpp
_v_att_sp_sub = orb_subscribe(ORB_ID(vehicle_attitude_setpoint));
_v_rates_sp_sub = orb_subscribe(ORB_ID(vehicle_rates_setpoint));
_ctrl_state_sub = orb_subscribe(ORB_ID(control_state));
_v_control_mode_sub = orb_subscribe(ORB_ID(vehicle_control_mode));
_params_sub = orb_subscribe(ORB_ID(parameter_update));
_manual_control_sp_sub = orb_subscribe(ORB_ID(manual_control_setpoint));
_armed_sub = orb_subscribe(ORB_ID(actuator_armed));
_vehicle_status_sub = orb_subscribe(ORB_ID(vehicle_status));
_motor_limits_sub = orb_subscribe(ORB_ID(multirotor_motor_limits));
orb_copy(ORB_ID(vehicle_attitude_setpoint), _v_att_sp_sub, &_v_att_sp);
orb_copy(ORB_ID(control_state), _ctrl_state_sub, &_ctrl_state);
orb_copy(ORB_ID(parameter_update), _params_sub, ¶m_update);
orb_copy(ORB_ID(vehicle_control_mode), _v_control_mode_sub, &_v_control_mode);
orb_copy(ORB_ID(actuator_armed), _armed_sub, &_armed);
orb_copy(ORB_ID(manual_control_setpoint), _manual_control_sp_sub, &_manual_control_sp);
orb_copy(ORB_ID(vehicle_status), _vehicle_status_sub, &_vehicle_status);
if (!_rates_sp_id) {
if (_vehicle_status.is_vtol) {
_rates_sp_id = ORB_ID(mc_virtual_rates_setpoint);
_actuators_id = ORB_ID(actuator_controls_virtual_mc);
} else {
_rates_sp_id = ORB_ID(vehicle_rates_setpoint);
_actuators_id = ORB_ID(actuator_controls_0);
}
}
}
orb_copy(ORB_ID(multirotor_motor_limits), _motor_limits_sub, &_motor_limits);
orb_copy(ORB_ID(vehicle_rates_setpoint), _v_rates_sp_sub, &_v_rates_sp);
orb_publish(_rates_sp_id, _v_rates_sp_pub, &_v_rates_sp);
_v_rates_sp_pub = orb_advertise(_rates_sp_id, &_v_rates_sp);
orb_publish(_actuators_id, _actuators_0_pub, &_actuators);
_actuators_0_pub = orb_advertise(_actuators_id, &_actuators);
orb_publish(ORB_ID(mc_att_ctrl_status), _controller_status_pub, &_controller_status);
_controller_status_pub = orb_advertise(ORB_ID(mc_att_ctrl_status), &_controller_status);
上图是
姿态估计 Attitude_estimator_q 位置估计 position_estimator_inav 姿态控制 mc_att_control 位置控制 mc_pos_control
的逻辑图
#ifdef __cplusplus
struct __EXPORT vehicle_local_position_s {
#else
struct vehicle_local_position_s {
#endif
uint64_t timestamp;
bool xy_valid;
bool z_valid;
bool v_xy_valid;
bool v_z_valid;
float x;
float y;
float z;
float vx;
float vy;
float vz;
float yaw;
bool xy_global;
bool z_global;
uint64_t ref_timestamp;
double ref_lat;
double ref_lon;
float ref_alt;
float dist_bottom;
float dist_bottom_rate;
uint64_t surface_bottom_timestamp;
bool dist_bottom_valid;
float eph;
float epv;
#ifdef __cplusplus
#endif
};
#ifdef __cplusplus
struct __EXPORT vehicle_global_position_s {
#else
struct vehicle_global_position_s {
#endif
uint64_t timestamp;
uint64_t time_utc_usec;
double lat;
double lon;
float alt;
float vel_n;
float vel_e;
float vel_d;
float yaw;
float eph;
float epv;
float terrain_alt;
bool terrain_alt_valid;
bool dead_reckoning;
float pressure_alt;
#ifdef __cplusplus
#endif
};
#ifdef __cplusplus
struct __EXPORT vehicle_attitude_s {
#else
struct vehicle_attitude_s {
#endif
uint64_t timestamp;
float roll;
float pitch;
float yaw;
float rollspeed;
float pitchspeed;
float yawspeed;
float rollacc;
float pitchacc;
float yawacc;
float rate_vibration;
float accel_vibration;
float mag_vibration;
float rate_offsets[3];
float R[9];
float q[4];
float g_comp[3];
bool R_valid;
bool q_valid;
#ifdef __cplusplus
#endif
};
#ifdef __cplusplus
struct __EXPORT control_state_s {
#else
struct control_state_s {
#endif
uint64_t timestamp;
float x_acc;
float y_acc;
float z_acc;
float x_vel;
float y_vel;
float z_vel;
float x_pos;
float y_pos;
float z_pos;
float airspeed;
bool airspeed_valid;
float vel_variance[3];
float pos_variance[3];
float q[4];
float roll_rate;
float pitch_rate;
float yaw_rate;
float horz_acc_mag;
#ifdef __cplusplus
#endif
};
#ifdef __cplusplus
struct __EXPORT vehicle_attitude_setpoint_s {
#else
struct vehicle_attitude_setpoint_s {
#endif
uint64_t timestamp;
float roll_body;
float pitch_body;
float yaw_body;
float yaw_sp_move_rate;
float R_body[9];
bool R_valid;
float q_d[4];
bool q_d_valid;
float q_e[4];
bool q_e_valid;
float thrust;
bool roll_reset_integral;
bool pitch_reset_integral;
bool yaw_reset_integral;
bool fw_control_yaw;
bool disable_mc_yaw_control;
bool apply_flaps;
#ifdef __cplusplus
#endif
};
#ifdef __cplusplus
struct __EXPORT vehicle_rates_setpoint_s {
#else
struct vehicle_rates_setpoint_s {
#endif
uint64_t timestamp;
float roll;
float pitch;
float yaw;
float thrust;
#ifdef __cplusplus
#endif
};
#ifdef __cplusplus
struct __EXPORT actuator_controls_0_s {
#else
struct actuator_controls_0_s {
#endif
uint64_t timestamp;
uint64_t timestamp_sample;
float control[8];
#ifdef __cplusplus
static const uint8_t NUM_ACTUATOR_CONTROLS = 8;
static const uint8_t NUM_ACTUATOR_CONTROL_GROUPS = 4;
#endif
};
上述结构还没有包括传感器的数据流，现在还没时间看到每个传感器。
恩，这篇就先到这吧，内容还挺多的。^_^
至于传感器系统、pwm系统，再另外写一篇blog。
至于传感器系统、pwm系统，再另外写一篇blog。
如果您觉得此文对您的发展有用，请随意打赏。
您的鼓励将是笔者书写高质量文章的最大动力^_^!!
您的鼓励将是笔者书写高质量文章的最大动力^_^!!
