Nuttx下移植uorb笔记
之前接触过ros下的消息机制（生产者/消费者）模型，第一感觉是灵活好用，但是在资源有限的嵌入式环境里面，邮箱、消息…显得就有点不那么灵活，后来开发飞控逐渐了解到了nuttx以及uorb发现了这与ros下的消息机制异曲同工，但是px4是一个高度定制化（解决飞行控制问题）的系统，但是如果仅仅想使用nuttx作为os做开发的话，就不那么友好了，所以开始移植吧。下面的一些关键词是移植的重点，需要提前了解；
- c++编译器编译c代码
- px4下的CDev类
- nuttx驱动模型（上半部分、下半部分）
- makefile
uORB
关于uORB的使用方法网上有很多介绍了，就不在重复了，这里主要是梳理下uORB的软件框架：
namespace uORB
{
class DeviceNode;
class DeviceMaster;
class Manager;
}
这三个类是实现整个uORB的主要元素：
class Manager：服务应用，提供消息的操作接口（订阅/发布/取消/检查等等），面向的是使用uORB的应用；
class DeviceNode：消息的最小实体单元（对应nuttx下的字符驱动），每个消息都会是以/obj/xxx下的字符驱动形式存在；
class DeviceMaster：管理DeviceNode，所有消息的创建、参数检查、记录都是是有它来负责完成；
所以到这里为止，其实我们差不多能窥见uORB的框架了；用别人总结的一句话就是“多个任务打开同一个设备”；
class CDev
这个是px4上面做的一个字符驱动的基类，实现了在nuttx下一个字符驱动的上/下部分，通俗一点的话就是，把nuttx定义的字符驱动框架都填满了；后面需要实现的驱动直接公共继承它就OK了。这样做的目的是减少了开发者的重复性工作，也直接规范了驱动编码（高明的做法）
说这个的意义是在于我们上面介绍的关于uorb的DeviceNode、DeviceMaster都是公共继承了CDev类的，所以我们移植也需要去移植这个框架；
这里多说一句，github上也有移植uorb的开源代码，不过是基于ringbuffer来实现，不依赖一固定的某一个OS，这个和我们这次移植的有点不一样（基于Nuttx字符驱动来实现）
具体请参考作者开源的代码地址Github
移植CDev
这里只是梳理整个移植的思路和关键点，具体的移植需要开发者自己实现。
第一阶段的目标是我们需要把CDev的框架在nuttx上搭建起来
移植的顺序：CDev===>Manager===>DeviceNode===>DeviceMaster
1、首先在app目录下建立uORB文件夹并在路径下继续建立如下三个文件夹：device、topic、uorb；
2、然后将下列文件拷贝在device文件夹下
CDev.cxx cdev_platform.cxx device.h drv_device.h
CDev.hpp cdev_platform.hpp Device.hpp drv_orb_dev.h
这些文件都可以在px4的源代码里找到，后面在说各自的作用；
3、接下来就是makefile
./uORB/Makefile 【把uORB文件加入到app的编译路径】
############################################################################
# apps/uORB/Makefile
#
# Copyright (C) 2011-2015 Gregory Nutt. All rights reserved.
# Author: Gregory Nutt <gnutt@nuttx.org>
#
# Redistribution and use in source and binary forms, with or without
# modification, are permitted provided that the following conditions
# are met:
#
# 1. Redistributions of source code must retain the above copyright
# notice, this list of conditions and the following disclaimer.
# 2. Redistributions in binary form must reproduce the above copyright
# notice, this list of conditions and the following disclaimer in
# the documentation and/or other materials provided with the
# distribution.
# 3. Neither the name NuttX nor the names of its contributors may be
# used to endorse or promote products derived from this software
# without specific prior written permission.
#
# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
# POSSIBILITY OF SUCH DAMAGE.
#
############################################################################
MENUDESC = "uORB"
include $(APPDIR)/Directory.mk
./uORB/Make.defs 【包含uORB下一级文件的makefile】
############################################################################
# apps/uORB/Make.defs
# Adds selected applications to apps/ build
#
# Copyright (C) 2012, 2015 Gregory Nutt. All rights reserved.
# Author: Gregory Nutt <gnutt@nuttx.org>
#
# Redistribution and use in source and binary forms, with or without
# modification, are permitted provided that the following conditions
# are met:
#
# 1. Redistributions of source code must retain the above copyright
# notice, this list of conditions and the following disclaimer.
# 2. Redistributions in binary form must reproduce the above copyright
# notice, this list of conditions and the following disclaimer in
# the documentation and/or other materials provided with the
# distribution.
# 3. Neither the name NuttX nor the names of its contributors may be
# used to endorse or promote products derived from this software
# without specific prior written permission.
#
# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
# POSSIBILITY OF SUCH DAMAGE.
#
############################################################################
include $(wildcard uORB/*/Make.defs)
通过上面的makefile我们就可以实现把我们新建的文件路径加入编译系统
4、再说回CDev的编译先来看看makefile
############################################################################
# apps/uORB/uorb/Makefile
#
# Copyright (C) 2009-2012 Gregory Nutt. All rights reserved.
# Author: Gregory Nutt <gnutt@nuttx.org>
#
# Redistribution and use in source and binary forms, with or without
# modification, are permitted provided that the following conditions
# are met:
#
# 1. Redistributions of source code must retain the above copyright
# notice, this list of conditions and the following disclaimer.
# 2. Redistributions in binary form must reproduce the above copyright
# notice, this list of conditions and the following disclaimer in
# the documentation and/or other materials provided with the
# distribution.
# 3. Neither the name NuttX nor the names of its contributors may be
# used to endorse or promote products derived from this software
# without specific prior written permission.
#
# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
# POSSIBILITY OF SUCH DAMAGE.
#
############################################################################
-include $(TOPDIR)/Make.defs
# uorb
ASRCS =
CSRCS =
#CXXSRCS就是我们接下来需要编译的源文件
CXXSRCS = ../device/cdev_platform.cxx ../device/CDev.cxx uORBManager.cxx uORBDevices.cxx uORB.cxx uORBUtils.cxx
MAINSRC = uORBMain.cxx
CONFIG_UORB_PROGNAME ?= uorb$(EXEEXT)
PROGNAME = $(CONFIG_UORB_PROGNAME)
# uorb built-in application info
APPNAME = uorb
PRIORITY = SCHED_PRIORITY_DEFAULT
STACKSIZE = 2048
include $(APPDIR)/Application.mk
对于编译CDev这个框架来说，我们初始的时候只需要添加…/device/cdev_platform.cxx …/device/CDev.cxx这两个文件就OK了，在移植的时候需要注意一下几点：
！框架里面只保留PX4NUTTX宏定义的代码片段，其余平台的例如POSIX的全干掉
！出现没有的头文件缺啥补啥
直到编译不会出现警告和错误为止
移植uORB
将px4源码里的uorb整个拷贝过来，就是下面这些文件
ORBMap.hpp Subscription.hpp uORBDevices.hpp uORBTopics.h
ORBSet.hpp uORBCommon.hpp uORB.h uORBUtils.cxx
Publication.cxx uORBCommunicator.hpp uORBMain.cxx uORBUtils.hpp
Publication.hpp uORB.cxx uORBManager.cxx
Subscription.cxx uORBDevices.cxx uORBManager.hpp
Subscription.hpp Subscription.cxx Publication.cxx Publication.hpp这4个文件暂时没有用到，所以在makefile里面也不用加入编译；在移植的时候同样需要注意一下几点：
！框架里面只保留PX4NUTTX宏定义的代码片段，其余平台的例如POSIX的全干掉
！出现没有的头文件缺啥补啥
直到编译不会出现警告和错误为止
移植hrt
这个高精度定时器是独立与nuttx驱动建立的，所以直接把源码放到…/arch/arm/src/stm32f7下面（注意要在makefile里面也加入），我用的是f7，移植的时候对应自己的MCU
/****************************************************************************
*
* Copyright (c) 2012, 2013 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name PX4 nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
/**
* @file drv_hrt.c
*
* High-resolution timer callouts and timekeeping.
*
* This can use any general or advanced STM32 timer.
*
* Note that really, this could use systick too, but that's
* monopolised by NuttX and stealing it would just be awkward.
*
* We don't use the NuttX STM32 driver per se; rather, we
* claim the timer and then drive it directly.
*/
#include <nuttx/config.h>
#include <nuttx/arch.h>
#include <nuttx/irq.h>
#include <sys/types.h>
#include <stdbool.h>
#include <assert.h>
#include <debug.h>
#include <time.h>
#include <queue.h>
#include <errno.h>
#include <string.h>
#include <nuttx/timers/drv_hrt.h>
#include <arch/board/board.h>
#include "stm32_gpio.h"
#include "stm32_tim.h"
/* High-resolution timer */
#define HRT_TIMER 9 /* use timer8 for the HRT */
#define HRT_TIMER_CHANNEL 1 /* use capture/compare channel */
#ifdef CONFIG_DEBUG_HRT
# define hrtinfo _info
#else
# define hrtinfo(x...)
#endif
#ifdef HRT_TIMER
/* HRT configuration */
#if HRT_TIMER == 1
# define HRT_TIMER_BASE STM32_TIM1_BASE
# define HRT_TIMER_POWER_REG STM32_RCC_APB2ENR
# define HRT_TIMER_POWER_BIT RCC_APB2ENR_TIM1EN
# define HRT_TIMER_VECTOR STM32_IRQ_TIM1CC
# define HRT_TIMER_CLOCK STM32_APB2_TIM1_CLKIN
# if CONFIG_STM32F7_TIM1
# error must not set CONFIG_STM32_TIM1=y and HRT_TIMER=1
# endif
#elif HRT_TIMER == 2
# define HRT_TIMER_BASE STM32_TIM2_BASE
# define HRT_TIMER_POWER_REG STM32_RCC_APB1ENR
# define HRT_TIMER_POWER_BIT RCC_APB1ENR_TIM2EN
# define HRT_TIMER_VECTOR STM32_IRQ_TIM2
# define HRT_TIMER_CLOCK STM32_APB1_TIM2_CLKIN
# if CONFIG_STM32F7_TIM2
# error must not set CONFIG_STM32_TIM2=y and HRT_TIMER=2
# endif
#elif HRT_TIMER == 3
# define HRT_TIMER_BASE STM32_TIM3_BASE
# define HRT_TIMER_POWER_REG STM32_RCC_APB1ENR
# define HRT_TIMER_POWER_BIT RCC_APB1ENR_TIM3EN
# define HRT_TIMER_VECTOR STM32_IRQ_TIM3
# define HRT_TIMER_CLOCK STM32_APB1_TIM3_CLKIN
# if CONFIG_STM32F7_TIM3
# error must not set CONFIG_STM32_TIM3=y and HRT_TIMER=3
# endif
#elif HRT_TIMER == 4
# define HRT_TIMER_BASE STM32_TIM4_BASE
# define HRT_TIMER_POWER_REG STM32_RCC_APB1ENR
# define HRT_TIMER_POWER_BIT RCC_APB1ENR_TIM4EN
# define HRT_TIMER_VECTOR STM32_IRQ_TIM4
# define HRT_TIMER_CLOCK STM32_APB1_TIM4_CLKIN
# if CONFIG_STM32F7_TIM4
# error must not set CONFIG_STM32_TIM4=y and HRT_TIMER=4
# endif
#elif HRT_TIMER == 5
# define HRT_TIMER_BASE STM32_TIM5_BASE
# define HRT_TIMER_POWER_REG STM32_RCC_APB1ENR
# define HRT_TIMER_POWER_BIT RCC_APB1ENR_TIM5EN
# define HRT_TIMER_VECTOR STM32_IRQ_TIM5
# define HRT_TIMER_CLOCK STM32_APB1_TIM5_CLKIN
# if CONFIG_STM32F7_TIM5
# error must not set CONFIG_STM32_TIM5=y and HRT_TIMER=5
# endif
#elif HRT_TIMER == 8
# define HRT_TIMER_BASE STM32_TIM8_BASE
# define HRT_TIMER_POWER_REG STM32_RCC_APB2ENR
# define HRT_TIMER_POWER_BIT RCC_APB2ENR_TIM8EN
# define HRT_TIMER_VECTOR STM32_IRQ_TIM8CC
# define HRT_TIMER_CLOCK STM32_APB2_TIM8_CLKIN
# if CONFIG_STM32F7_TIM8
# error must not set CONFIG_STM32_TIM8=y and HRT_TIMER=8
# endif
#elif HRT_TIMER == 9
# define HRT_TIMER_BASE STM32_TIM9_BASE
# define HRT_TIMER_POWER_REG STM32_RCC_APB2ENR
# define HRT_TIMER_POWER_BIT RCC_APB2ENR_TIM9EN
# define HRT_TIMER_VECTOR STM32_IRQ_TIM1BRK
# define HRT_TIMER_CLOCK STM32_APB2_TIM9_CLKIN
# ifdef CONFIG_STM32F7_TIM9
# error must not set CONFIG_STM32_TIM9=y and HRT_TIMER=9
# endif
#elif HRT_TIMER == 10
# define HRT_TIMER_BASE STM32_TIM10_BASE
# define HRT_TIMER_POWER_REG STM32_RCC_APB2ENR
# define HRT_TIMER_POWER_BIT RCC_APB2ENR_TIM10EN
# define HRT_TIMER_VECTOR STM32_IRQ_TIM1UP
# define HRT_TIMER_CLOCK STM32_APB2_TIM10_CLKIN
# if CONFIG_STM32F7_TIM10
# error must not set CONFIG_STM32_TIM11=y and HRT_TIMER=10
# endif
#elif HRT_TIMER == 11
# define HRT_TIMER_BASE STM32_TIM11_BASE
# define HRT_TIMER_POWER_REG STM32_RCC_APB2ENR
# define HRT_TIMER_POWER_BIT RCC_APB2ENR_TIM11EN
# define HRT_TIMER_VECTOR STM32_IRQ_TIM1TRGCOM
# define HRT_TIMER_CLOCK STM32_APB2_TIM11_CLKIN
# if CONFIG_STM32F7_TIM11
# error must not set CONFIG_STM32_TIM11=y and HRT_TIMER=11
# endif
#else
# error HRT_TIMER must be a value between 1 and 11
#endif
/*
* HRT clock must be a multiple of 1MHz greater than 1MHz
*/
#if (HRT_TIMER_CLOCK % 1000000) != 0
# error HRT_TIMER_CLOCK must be a multiple of 1MHz
#endif
#if HRT_TIMER_CLOCK <= 1000000
# error HRT_TIMER_CLOCK must be greater than 1MHz
#endif
/**
* Minimum/maximum deadlines.
*
* These are suitable for use with a 16-bit timer/counter clocked
* at 1MHz. The high-resolution timer need only guarantee that it
* not wrap more than once in the 50ms period for absolute time to
* be consistently maintained.
*
* The minimum deadline must be such that the time taken between
* reading a time and writing a deadline to the timer cannot
* result in missing the deadline.
*/
#define HRT_INTERVAL_MIN 50
#define HRT_INTERVAL_MAX 50000
/*
* Period of the free-running counter, in microseconds.
*/
#define HRT_COUNTER_PERIOD 65536
/*
* Scaling factor(s) for the free-running counter; convert an input
* in counts to a time in microseconds.
*/
#define HRT_COUNTER_SCALE(_c) (_c)
/*
* Timer register accessors
*/
#define REG(_reg) (*(volatile uint32_t *)(HRT_TIMER_BASE + _reg))
#define rCR1 REG(STM32_GTIM_CR1_OFFSET)
#define rCR2 REG(STM32_GTIM_CR2_OFFSET)
#define rSMCR REG(STM32_GTIM_SMCR_OFFSET)
#define rDIER REG(STM32_GTIM_DIER_OFFSET)
#define rSR REG(STM32_GTIM_SR_OFFSET)
#define rEGR REG(STM32_GTIM_EGR_OFFSET)
#define rCCMR1 REG(STM32_GTIM_CCMR1_OFFSET)
#define rCCMR2 REG(STM32_GTIM_CCMR2_OFFSET)
#define rCCER REG(STM32_GTIM_CCER_OFFSET)
#define rCNT REG(STM32_GTIM_CNT_OFFSET)
#define rPSC REG(STM32_GTIM_PSC_OFFSET)
#define rARR REG(STM32_GTIM_ARR_OFFSET)
#define rCCR1 REG(STM32_GTIM_CCR1_OFFSET)
#define rCCR2 REG(STM32_GTIM_CCR2_OFFSET)
#define rCCR3 REG(STM32_GTIM_CCR3_OFFSET)
#define rCCR4 REG(STM32_GTIM_CCR4_OFFSET)
#define rDCR REG(STM32_GTIM_DCR_OFFSET)
#define rDMAR REG(STM32_GTIM_DMAR_OFFSET)
/*
* Specific registers and bits used by HRT sub-functions
*/
/* FIXME! There is an interaction in the CCMR registers that prevents using Chan 1 as the timer and chan 2 as the PPM*/
#if HRT_TIMER_CHANNEL == 1
# define rCCR_HRT rCCR1 /* compare register for HRT */
# define DIER_HRT GTIM_DIER_CC1IE /* interrupt enable for HRT */
# define SR_INT_HRT GTIM_SR_CC1IF /* interrupt status for HRT */
#elif HRT_TIMER_CHANNEL == 2
# define rCCR_HRT rCCR2 /* compare register for HRT */
# define DIER_HRT GTIM_DIER_CC2IE /* interrupt enable for HRT */
# define SR_INT_HRT GTIM_SR_CC2IF /* interrupt status for HRT */
#elif HRT_TIMER_CHANNEL == 3
# define rCCR_HRT rCCR3 /* compare register for HRT */
# define DIER_HRT GTIM_DIER_CC3IE /* interrupt enable for HRT */
# define SR_INT_HRT GTIM_SR_CC3IF /* interrupt status for HRT */
#elif HRT_TIMER_CHANNEL == 4
# define rCCR_HRT rCCR4 /* compare register for HRT */
# define DIER_HRT GTIM_DIER_CC4IE /* interrupt enable for HRT */
# define SR_INT_HRT GTIM_SR_CC4IF /* interrupt status for HRT */
#else
# error HRT_TIMER_CHANNEL must be a value between 1 and 4
#endif
/*
* Queue of callout entries.
*/
static struct sq_queue_s callout_queue;
/* latency baseline (last compare value applied) */
static uint16_t latency_baseline;
/* timer count at interrupt (for latency purposes) */
static uint16_t latency_actual;
/* latency histogram */
#define LATENCY_BUCKET_COUNT 8
//__EXPORT const uint16_t latency_bucket_count = LATENCY_BUCKET_COUNT;
//__EXPORT const uint16_t latency_buckets[LATENCY_BUCKET_COUNT] = { 1, 2, 5, 10, 20, 50, 100, 1000 };
//__EXPORT uint32_t latency_counters[LATENCY_BUCKET_COUNT + 1];
const uint16_t latency_bucket_count = LATENCY_BUCKET_COUNT;
const uint16_t latency_buckets[LATENCY_BUCKET_COUNT] = { 1, 2, 5, 10, 20, 50, 100, 1000 };
uint32_t latency_counters[LATENCY_BUCKET_COUNT + 1];
/* timer-specific functions */
static void hrt_tim_init(void);
static int hrt_tim_isr(int irq, void *context, void *arg);
static void hrt_latency_update(void);
/* callout list manipulation */
static void hrt_call_internal(struct hrt_call *entry,
hrt_abstime deadline,
hrt_abstime interval,
hrt_callout callout,
void *arg);
static void hrt_call_enter(struct hrt_call *entry);
static void hrt_call_reschedule(void);
static void hrt_call_invoke(void);
/**
* Initialise the timer we are going to use.
*
* We expect that we'll own one of the reduced-function STM32 general
* timers, and that we can use channel 1 in compare mode.
*/
static void
hrt_tim_init(void)
{
/* claim our interrupt vector */
irq_attach(HRT_TIMER_VECTOR, hrt_tim_isr, NULL);
/* clock/power on our timer */
modifyreg32(HRT_TIMER_POWER_REG, 0, HRT_TIMER_POWER_BIT);
/* disable and configure the timer */
rCR1 = 0;
rCR2 = 0;
rSMCR = 0;
rDIER = DIER_HRT ;
rCCER = 0; /* unlock CCMR* registers */
rCCMR1 = 0;
rCCMR2 = 0;
rCCER = 0;
rDCR = 0;
/* configure the timer to free-run at 1MHz */
rPSC = (HRT_TIMER_CLOCK / 1000000) - 1; /* this really only works for whole-MHz clocks */
/* run the full span of the counter */
rARR = 0xffff;
/* set an initial capture a little ways off */
rCCR_HRT = 1000;
/* generate an update event; reloads the counter, all registers */
rEGR = GTIM_EGR_UG;
/* enable the timer */
rCR1 = GTIM_CR1_CEN;
/* enable interrupts */
up_enable_irq(HRT_TIMER_VECTOR);
}
/**
* Handle the compare interrupt by calling the callout dispatcher
* and then re-scheduling the next deadline.
*/
static int
hrt_tim_isr(int irq, void *context, void *arg)
{
uint32_t status;
/* grab the timer for latency tracking purposes */
latency_actual = rCNT;
/* copy interrupt status */
status = rSR;
/* ack the interrupts we just read */
rSR = ~status;
/* was this a timer tick? */
if (status & SR_INT_HRT) {
/* do latency calculations */
hrt_latency_update();
/* run any callouts that have met their deadline */
hrt_call_invoke();
/* and schedule the next interrupt */
hrt_call_reschedule();
}
return OK;
}
/**
* Fetch a never-wrapping absolute time value in microseconds from
* some arbitrary epoch shortly after system start.
*/
hrt_abstime
hrt_absolute_time(void)
{
hrt_abstime abstime;
uint32_t count;
irqstate_t flags;
/*
* Counter state. Marked volatile as they may change
* inside this routine but outside the irqsave/restore
* pair. Discourage the compiler from moving loads/stores
* to these outside of the protected range.
*/
static volatile hrt_abstime base_time;
static volatile uint32_t last_count;
/* prevent re-entry */
flags = enter_critical_section();
/* get the current counter value */
count = rCNT;
/*
* Determine whether the counter has wrapped since the
* last time we're called.
*
* This simple test is sufficient due to the guarantee that
* we are always called at least once per counter period.
*/
if (count < last_count) {
base_time += HRT_COUNTER_PERIOD;
}
/* save the count for next time */
last_count = count;
/* compute the current time */
abstime = HRT_COUNTER_SCALE(base_time + count);
leave_critical_section(flags);
return abstime;
}
/**
* Convert a timespec to absolute time
*/
hrt_abstime
ts_to_abstime(struct timespec *ts)
{
hrt_abstime result;
result = (hrt_abstime)(ts->tv_sec) * 1000000;
result += ts->tv_nsec / 1000;
return result;
}
/**
* Convert absolute time to a timespec.
*/
void
abstime_to_ts(struct timespec *ts, hrt_abstime abstime)
{
ts->tv_sec = abstime / 1000000;
abstime -= ts->tv_sec * 1000000;
ts->tv_nsec = abstime * 1000;
}
/**
* Compare a time value with the current time.
*/
hrt_abstime
hrt_elapsed_time(const volatile hrt_abstime *then)
{
irqstate_t flags = enter_critical_section();
hrt_abstime delta = hrt_absolute_time() - *then;
leave_critical_section(flags);
return delta;
}
/**
* Store the absolute time in an interrupt-safe fashion
*/
hrt_abstime
hrt_store_absolute_time(volatile hrt_abstime *now)
{
irqstate_t flags = enter_critical_section();
hrt_abstime ts = hrt_absolute_time();
leave_critical_section(flags);
return ts;
}
/**
* Initialise the high-resolution timing module.
*/
void
hrt_init(void)
{
sq_init(&callout_queue);
hrt_tim_init();
#ifdef HRT_PPM_CHANNEL
/* configure the PPM input pin */
px4_arch_configgpio(GPIO_PPM_IN);
#endif
}
/**
* Call callout(arg) after interval has elapsed.
*/
void
hrt_call_after(struct hrt_call *entry, hrt_abstime delay, hrt_callout callout, void *arg)
{
hrt_call_internal(entry,
hrt_absolute_time() + delay,
0,
callout,
arg);
}
/**
* Call callout(arg) at calltime.
*/
void
hrt_call_at(struct hrt_call *entry, hrt_abstime calltime, hrt_callout callout, void *arg)
{
hrt_call_internal(entry, calltime, 0, callout, arg);
}
/**
* Call callout(arg) every period.
*/
void
hrt_call_every(struct hrt_call *entry, hrt_abstime delay, hrt_abstime interval, hrt_callout callout, void *arg)
{
hrt_call_internal(entry,
hrt_absolute_time() + delay,
interval,
callout,
arg);
}
static void
hrt_call_internal(struct hrt_call *entry, hrt_abstime deadline, hrt_abstime interval, hrt_callout callout, void *arg)
{
irqstate_t flags = enter_critical_section();
/* if the entry is currently queued, remove it */
/* note that we are using a potentially uninitialised
entry->link here, but it is safe as sq_rem() doesn't
dereference the passed node unless it is found in the
list. So we potentially waste a bit of time searching the
queue for the uninitialised entry->link but we don't do
anything actually unsafe.
*/
if (entry->deadline != 0) {
sq_rem(&entry->link, &callout_queue);
}
entry->deadline = deadline;
entry->period = interval;
entry->callout = callout;
entry->arg = arg;
hrt_call_enter(entry);
leave_critical_section(flags);
}
/**
* If this returns true, the call has been invoked and removed from the callout list.
*
* Always returns false for repeating callouts.
*/
bool
hrt_called(struct hrt_call *entry)
{
return (entry->deadline == 0);
}
/**
* Remove the entry from the callout list.
*/
void
hrt_cancel(struct hrt_call *entry)
{
irqstate_t flags = enter_critical_section();
sq_rem(&entry->link, &callout_queue);
entry->deadline = 0;
/* if this is a periodic call being removed by the callout, prevent it from
* being re-entered when the callout returns.
*/
entry->period = 0;
leave_critical_section(flags);
}
static void
hrt_call_enter(struct hrt_call *entry)
{
struct hrt_call *call, *next;
call = (struct hrt_call *)sq_peek(&callout_queue);
if ((call == NULL) || (entry->deadline < call->deadline)) {
sq_addfirst(&entry->link, &callout_queue);
hrtinfo("call enter at head, reschedule\n");
/* we changed the next deadline, reschedule the timer event */
hrt_call_reschedule();
} else {
do {
next = (struct hrt_call *)sq_next(&call->link);
if ((next == NULL) || (entry->deadline < next->deadline)) {
hrtinfo("call enter after head\n");
sq_addafter(&call->link, &entry->link, &callout_queue);
break;
}
} while ((call = next) != NULL);
}
hrtinfo("scheduled\n");
}
static void
hrt_call_invoke(void)
{
struct hrt_call *call;
hrt_abstime deadline;
while (true) {
/* get the current time */
hrt_abstime now = hrt_absolute_time();
call = (struct hrt_call *)sq_peek(&callout_queue);
if (call == NULL) {
break;
}
if (call->deadline > now) {
break;
}
sq_rem(&call->link, &callout_queue);
hrtinfo("call pop\n");
/* save the intended deadline for periodic calls */
deadline = call->deadline;
/* zero the deadline, as the call has occurred */
call->deadline = 0;
/* invoke the callout (if there is one) */
if (call->callout) {
hrtinfo("call %p: %p(%p)\n", call, call->callout, call->arg);
call->callout(call->arg);
}
/* if the callout has a non-zero period, it has to be re-entered */
if (call->period != 0) {
// re-check call->deadline to allow for
// callouts to re-schedule themselves
// using hrt_call_delay()
if (call->deadline <= now) {
call->deadline = deadline + call->period;
}
hrt_call_enter(call);
}
}
}
/**
* Reschedule the next timer interrupt.
*
* This routine must be called with interrupts disabled.
*/
static void
hrt_call_reschedule()
{
hrt_abstime now = hrt_absolute_time();
struct hrt_call *next = (struct hrt_call *)sq_peek(&callout_queue);
hrt_abstime deadline = now + HRT_INTERVAL_MAX;
/*
* Determine what the next deadline will be.
*
* Note that we ensure that this will be within the counter
* period, so that when we truncate all but the low 16 bits
* the next time the compare matches it will be the deadline
* we want.
*
* It is important for accurate timekeeping that the compare
* interrupt fires sufficiently often that the base_time update in
* hrt_absolute_time runs at least once per timer period.
*/
if (next != NULL) {
hrtinfo("entry in queue\n");
if (next->deadline <= (now + HRT_INTERVAL_MIN)) {
hrtinfo("pre-expired\n");
/* set a minimal deadline so that we call ASAP */
deadline = now + HRT_INTERVAL_MIN;
} else if (next->deadline < deadline) {
hrtinfo("due soon\n");
deadline = next->deadline;
}
}
hrtinfo("schedule for %u at %u\n", (unsigned)(deadline & 0xffffffff), (unsigned)(now & 0xffffffff));
/* set the new compare value and remember it for latency tracking */
rCCR_HRT = latency_baseline = deadline & 0xffff;
}
static void
hrt_latency_update(void)
{
uint16_t latency = latency_actual - latency_baseline;
unsigned index;
/* bounded buckets */
for (index = 0; index < LATENCY_BUCKET_COUNT; index++) {
if (latency <= latency_buckets[index]) {
latency_counters[index]++;
return;
}
}
/* catch-all at the end */
latency_counters[index]++;
}
void
hrt_call_init(struct hrt_call *entry)
{
memset(entry, 0, sizeof(*entry));
}
void
hrt_call_delay(struct hrt_call *entry, hrt_abstime delay)
{
entry->deadline = hrt_absolute_time() + delay;
}
#endif /* HRT_TIMER */
移植topic
应为PX4的消息是通过python自动生成的，但是我们只关注结果就好了，直接按照最终生成的头文件编写就OK的，然后放在头文件下，下面贴一个代码
/****************************************************************************
*
* Copyright (C) 2013-2016 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name PX4 nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
#pragma once
#include <stdint.h>
#ifdef __cplusplus
#include <cstring>
#else
#include <string.h>
#endif
#include "../uorb/uORB.h"
#ifndef __cplusplus
#define RELATIVE_TIMESTAMP_INVALID 2147483647
#endif
#ifdef __cplusplus
struct __EXPORT sensor_combined_s {
#else
struct sensor_combined_s {
#endif
uint64_t timestamp; // required for logger
float gyro_rad[3];
uint32_t gyro_integral_dt;
int32_t accelerometer_timestamp_relative;
float accelerometer_m_s2[3];
uint32_t accelerometer_integral_dt;
int32_t magnetometer_timestamp_relative;
float magnetometer_ga[3];
int32_t baro_timestamp_relative;
float baro_alt_meter;
float baro_temp_celcius;
#ifdef __cplusplus
static constexpr int32_t RELATIVE_TIMESTAMP_INVALID = 2147483647;
#endif
};
/* register this as object request broker structure */
ORB_DEFINE(sensor_combined, struct sensor_combined_s,0,0);
ORB_DECLARE(sensor_combined);
到这里基本上就大功告成了，最终的文件结构就是这样的：
$ tree -L 2
.
├── device
│ ├── CDev.cxx
│ ├── CDev.hpp
│ ├── cdev_platform.cxx
│ ├── cdev_platform.hpp
│ ├── device.h
│ ├── Device.hpp
│ ├── drv_device.h
│ └── drv_orb_dev.h
├── Kconfig
├── Make.defs
├── Makefile
├── topic
│ └── sensor_combined.h
└── uorb
├── Kconfig
├── Make.defs
├── Make.dep
├── Makefile
├── ORBMap.hpp
├── ORBSet.hpp
├── Publication.cxx
├── Publication.hpp
├── Subscription.cxx
├── Subscription.hpp
├── uORBCommon.hpp
├── uORBCommunicator.hpp
├── uORB.cxx
├── uORBDevices.cxx
├── uORBDevices.hpp
├── uORB.h
├── uORBMain.cxx
├── uORBManager.cxx
├── uORBManager.hpp
├── uORBTopics.h
├── uORBUtils.cxx
└── uORBUtils.hpp
3 directories, 34 files
最后再贴一下main函数：
/****************************************************************************
*
* Copyright (c) 2012-2015 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name PX4 nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
#include <nuttx/config.h>
#include <nuttx/init.h>
#include <string.h>
#include <syslog.h>
#include <stdio.h>
#include <cstdio>
#include <debug.h>
#include <errno.h>
#include "platform/cxxinitialize.h"
#include "uORBDevices.hpp"
#include "uORBManager.hpp"
#include "uORB.h"
#include "uORBCommon.hpp"
extern "C" { int uorb_main(int argc, char *argv[]); }
static uORB::DeviceMaster *g_dev = nullptr;
static void usage()
{
printf("uorb communication\n");
printf("start\n");
printf("Print topic statistics\n");
printf("Monitor topic publication rates\n");
printf("print all instead of only currently publishing topics\n");
printf("<filter1> [<filter2>] topic(s) to match (implies -a)\n");
}
int
uorb_main(int argc, char *argv[])
{
if (argc < 2) {
usage();
return -EINVAL;
}
/*
* Start/load the driver.
*/
if (!strcmp(argv[1], "start")) {
if (g_dev != nullptr) {
syslog(LOG_INFO,"already loaded");
/* user wanted to start uorb, its already running, no error */
return 0;
}
if (!uORB::Manager::initialize()) {
syslog(LOG_INFO,"uorb manager alloc failed");
return -ENOMEM;
}
/* create the driver */
g_dev = uORB::Manager::get_instance()->get_device_master();
if (g_dev == nullptr) {
return -errno;
}
return OK;
}
/*
* Print driver information.
*/
if (!strcmp(argv[1], "status")) {
if (g_dev != nullptr) {
g_dev->printStatistics(true);
} else {
syslog(LOG_INFO,"uorb is not running\n");
}
return OK;
}
if (!strcmp(argv[1], "top")) {
if (g_dev != nullptr) {
g_dev->showTop(argv + 2, argc - 2);
} else {
syslog(LOG_INFO,"uorb is not running\n");
}
return OK;
}
usage();
return -EINVAL;
}
到此整个移植的工作就结束了。这篇文章只是提供了一种移植思路，肯定还有很多地方值得优化和改正，欢迎交流指正。
1万+
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
