forked from Archive/PX4-Autopilot
Merge pull request #748 from jgoppert/tmpl_fix
GCC 4.7 Template Use Fix
This commit is contained in:
commit
49bdfa8cfb
|
@ -52,7 +52,7 @@
|
|||
#include <arch/board/board.h>
|
||||
#include <mavlink/mavlink_log.h>
|
||||
|
||||
#include <controllib/uorb/UOrbPublication.hpp>
|
||||
#include <uORB/Publication.hpp>
|
||||
#include <uORB/topics/debug_key_value.h>
|
||||
#include <drivers/drv_hrt.h>
|
||||
|
||||
|
@ -587,7 +587,7 @@ int md25Sine(const char *deviceName, uint8_t bus, uint8_t address, float amplitu
|
|||
float prev_revolution = md25.getRevolutions1();
|
||||
|
||||
// debug publication
|
||||
control::UOrbPublication<debug_key_value_s> debug_msg(NULL,
|
||||
uORB::Publication<debug_key_value_s> debug_msg(NULL,
|
||||
ORB_ID(debug_key_value));
|
||||
|
||||
// sine wave for motor 1
|
||||
|
|
|
@ -46,7 +46,7 @@
|
|||
|
||||
#include <poll.h>
|
||||
#include <stdio.h>
|
||||
#include <controllib/uorb/UOrbSubscription.hpp>
|
||||
#include <uORB/Subscription.hpp>
|
||||
#include <uORB/topics/actuator_controls.h>
|
||||
#include <drivers/device/i2c.h>
|
||||
|
||||
|
@ -270,7 +270,7 @@ private:
|
|||
struct pollfd _controlPoll;
|
||||
|
||||
/** actuator controls subscription */
|
||||
control::UOrbSubscription<actuator_controls_s> _actuators;
|
||||
uORB::Subscription<actuator_controls_s> _actuators;
|
||||
|
||||
// local copy of data from i2c device
|
||||
uint8_t _version;
|
||||
|
|
|
@ -53,7 +53,7 @@
|
|||
#include <arch/board/board.h>
|
||||
#include <mavlink/mavlink_log.h>
|
||||
|
||||
#include <controllib/uorb/UOrbPublication.hpp>
|
||||
#include <uORB/Publication.hpp>
|
||||
#include <uORB/topics/debug_key_value.h>
|
||||
#include <drivers/drv_hrt.h>
|
||||
|
||||
|
|
|
@ -45,7 +45,7 @@
|
|||
|
||||
#include <poll.h>
|
||||
#include <stdio.h>
|
||||
#include <controllib/uorb/UOrbSubscription.hpp>
|
||||
#include <uORB/Subscription.hpp>
|
||||
#include <uORB/topics/actuator_controls.h>
|
||||
#include <drivers/device/i2c.h>
|
||||
|
||||
|
@ -169,7 +169,7 @@ private:
|
|||
struct pollfd _controlPoll;
|
||||
|
||||
/** actuator controls subscription */
|
||||
control::UOrbSubscription<actuator_controls_s> _actuators;
|
||||
uORB::Subscription<actuator_controls_s> _actuators;
|
||||
|
||||
// private data
|
||||
float _motor1Position;
|
||||
|
|
|
@ -32,9 +32,9 @@
|
|||
****************************************************************************/
|
||||
|
||||
/**
|
||||
* @file Node.h
|
||||
* @file List.hpp
|
||||
*
|
||||
* A node of a linked list.
|
||||
* A linked list.
|
||||
*/
|
||||
|
||||
#pragma once
|
||||
|
@ -43,7 +43,7 @@ template<class T>
|
|||
class __EXPORT ListNode
|
||||
{
|
||||
public:
|
||||
ListNode() : _sibling(NULL) {
|
||||
ListNode() : _sibling(nullptr) {
|
||||
}
|
||||
void setSibling(T sibling) { _sibling = sibling; }
|
||||
T getSibling() { return _sibling; }
|
|
@ -47,8 +47,8 @@
|
|||
#include <mathlib/mathlib.h>
|
||||
#include <controllib/blocks.hpp>
|
||||
#include <controllib/block/BlockParam.hpp>
|
||||
#include <controllib/uorb/UOrbSubscription.hpp>
|
||||
#include <controllib/uorb/UOrbPublication.hpp>
|
||||
#include <uORB/Subscription.hpp>
|
||||
#include <uORB/Publication.hpp>
|
||||
|
||||
#include <uORB/topics/vehicle_attitude.h>
|
||||
#include <uORB/topics/vehicle_global_position.h>
|
||||
|
@ -138,13 +138,13 @@ protected:
|
|||
math::Matrix<3,3> C_nb; /**< direction cosine matrix from body to nav frame */
|
||||
math::Quaternion q; /**< quaternion from body to nav frame */
|
||||
// subscriptions
|
||||
control::UOrbSubscription<sensor_combined_s> _sensors; /**< sensors sub. */
|
||||
control::UOrbSubscription<vehicle_gps_position_s> _gps; /**< gps sub. */
|
||||
control::UOrbSubscription<parameter_update_s> _param_update; /**< parameter update sub. */
|
||||
uORB::Subscription<sensor_combined_s> _sensors; /**< sensors sub. */
|
||||
uORB::Subscription<vehicle_gps_position_s> _gps; /**< gps sub. */
|
||||
uORB::Subscription<parameter_update_s> _param_update; /**< parameter update sub. */
|
||||
// publications
|
||||
control::UOrbPublication<vehicle_global_position_s> _pos; /**< position pub. */
|
||||
control::UOrbPublication<vehicle_local_position_s> _localPos; /**< local position pub. */
|
||||
control::UOrbPublication<vehicle_attitude_s> _att; /**< attitude pub. */
|
||||
uORB::Publication<vehicle_global_position_s> _pos; /**< position pub. */
|
||||
uORB::Publication<vehicle_local_position_s> _localPos; /**< local position pub. */
|
||||
uORB::Publication<vehicle_attitude_s> _att; /**< attitude pub. */
|
||||
// time stamps
|
||||
uint64_t _pubTimeStamp; /**< output data publication time stamp */
|
||||
uint64_t _predictTimeStamp; /**< prediction time stamp */
|
||||
|
|
|
@ -41,10 +41,11 @@
|
|||
#include <string.h>
|
||||
#include <stdio.h>
|
||||
|
||||
#include <uORB/Subscription.hpp>
|
||||
#include <uORB/Publication.hpp>
|
||||
|
||||
#include "Block.hpp"
|
||||
#include "BlockParam.hpp"
|
||||
#include "../uorb/UOrbSubscription.hpp"
|
||||
#include "../uorb/UOrbPublication.hpp"
|
||||
|
||||
namespace control
|
||||
{
|
||||
|
@ -100,7 +101,7 @@ void Block::updateParams()
|
|||
|
||||
void Block::updateSubscriptions()
|
||||
{
|
||||
UOrbSubscriptionBase *sub = getSubscriptions().getHead();
|
||||
uORB::SubscriptionBase *sub = getSubscriptions().getHead();
|
||||
int count = 0;
|
||||
|
||||
while (sub != NULL) {
|
||||
|
@ -118,7 +119,7 @@ void Block::updateSubscriptions()
|
|||
|
||||
void Block::updatePublications()
|
||||
{
|
||||
UOrbPublicationBase *pub = getPublications().getHead();
|
||||
uORB::PublicationBase *pub = getPublications().getHead();
|
||||
int count = 0;
|
||||
|
||||
while (pub != NULL) {
|
||||
|
|
|
@ -42,7 +42,13 @@
|
|||
#include <stdint.h>
|
||||
#include <inttypes.h>
|
||||
|
||||
#include "List.hpp"
|
||||
#include <containers/List.hpp>
|
||||
|
||||
// forward declaration
|
||||
namespace uORB {
|
||||
class SubscriptionBase;
|
||||
class PublicationBase;
|
||||
}
|
||||
|
||||
namespace control
|
||||
{
|
||||
|
@ -55,8 +61,6 @@ static const uint8_t blockNameLengthMax = 80;
|
|||
|
||||
// forward declaration
|
||||
class BlockParamBase;
|
||||
class UOrbSubscriptionBase;
|
||||
class UOrbPublicationBase;
|
||||
class SuperBlock;
|
||||
|
||||
/**
|
||||
|
@ -79,15 +83,15 @@ public:
|
|||
protected:
|
||||
// accessors
|
||||
SuperBlock *getParent() { return _parent; }
|
||||
List<UOrbSubscriptionBase *> & getSubscriptions() { return _subscriptions; }
|
||||
List<UOrbPublicationBase *> & getPublications() { return _publications; }
|
||||
List<uORB::SubscriptionBase *> & getSubscriptions() { return _subscriptions; }
|
||||
List<uORB::PublicationBase *> & getPublications() { return _publications; }
|
||||
List<BlockParamBase *> & getParams() { return _params; }
|
||||
// attributes
|
||||
const char *_name;
|
||||
SuperBlock *_parent;
|
||||
float _dt;
|
||||
List<UOrbSubscriptionBase *> _subscriptions;
|
||||
List<UOrbPublicationBase *> _publications;
|
||||
List<uORB::SubscriptionBase *> _subscriptions;
|
||||
List<uORB::PublicationBase *> _publications;
|
||||
List<BlockParamBase *> _params;
|
||||
};
|
||||
|
||||
|
|
|
@ -76,4 +76,29 @@ BlockParamBase::BlockParamBase(Block *parent, const char *name, bool parent_pref
|
|||
printf("error finding param: %s\n", fullname);
|
||||
};
|
||||
|
||||
template <class T>
|
||||
BlockParam<T>::BlockParam(Block *block, const char *name,
|
||||
bool parent_prefix) :
|
||||
BlockParamBase(block, name, parent_prefix),
|
||||
_val() {
|
||||
update();
|
||||
}
|
||||
|
||||
template <class T>
|
||||
T BlockParam<T>::get() { return _val; }
|
||||
|
||||
template <class T>
|
||||
void BlockParam<T>::set(T val) { _val = val; }
|
||||
|
||||
template <class T>
|
||||
void BlockParam<T>::update() {
|
||||
if (_handle != PARAM_INVALID) param_get(_handle, &_val);
|
||||
}
|
||||
|
||||
template <class T>
|
||||
BlockParam<T>::~BlockParam() {};
|
||||
|
||||
template class __EXPORT BlockParam<float>;
|
||||
template class __EXPORT BlockParam<int>;
|
||||
|
||||
} // namespace control
|
||||
|
|
|
@ -42,7 +42,7 @@
|
|||
#include <systemlib/param/param.h>
|
||||
|
||||
#include "Block.hpp"
|
||||
#include "List.hpp"
|
||||
#include <containers/List.hpp>
|
||||
|
||||
namespace control
|
||||
{
|
||||
|
@ -70,38 +70,21 @@ protected:
|
|||
* Parameters that are tied to blocks for updating and nameing.
|
||||
*/
|
||||
|
||||
class __EXPORT BlockParamFloat : public BlockParamBase
|
||||
template <class T>
|
||||
class BlockParam : public BlockParamBase
|
||||
{
|
||||
public:
|
||||
BlockParamFloat(Block *block, const char *name, bool parent_prefix=true) :
|
||||
BlockParamBase(block, name, parent_prefix),
|
||||
_val() {
|
||||
update();
|
||||
}
|
||||
float get() { return _val; }
|
||||
void set(float val) { _val = val; }
|
||||
void update() {
|
||||
if (_handle != PARAM_INVALID) param_get(_handle, &_val);
|
||||
}
|
||||
BlockParam(Block *block, const char *name,
|
||||
bool parent_prefix=true);
|
||||
T get();
|
||||
void set(T val);
|
||||
void update();
|
||||
virtual ~BlockParam();
|
||||
protected:
|
||||
float _val;
|
||||
T _val;
|
||||
};
|
||||
|
||||
class __EXPORT BlockParamInt : public BlockParamBase
|
||||
{
|
||||
public:
|
||||
BlockParamInt(Block *block, const char *name, bool parent_prefix=true) :
|
||||
BlockParamBase(block, name, parent_prefix),
|
||||
_val() {
|
||||
update();
|
||||
}
|
||||
int get() { return _val; }
|
||||
void set(int val) { _val = val; }
|
||||
void update() {
|
||||
if (_handle != PARAM_INVALID) param_get(_handle, &_val);
|
||||
}
|
||||
protected:
|
||||
int _val;
|
||||
};
|
||||
typedef BlockParam<float> BlockParamFloat;
|
||||
typedef BlockParam<int> BlockParamInt;
|
||||
|
||||
} // namespace control
|
||||
|
|
|
@ -37,7 +37,5 @@
|
|||
SRCS = test_params.c \
|
||||
block/Block.cpp \
|
||||
block/BlockParam.cpp \
|
||||
uorb/UOrbPublication.cpp \
|
||||
uorb/UOrbSubscription.cpp \
|
||||
uorb/blocks.cpp \
|
||||
blocks.cpp
|
||||
|
|
|
@ -62,8 +62,8 @@ extern "C" {
|
|||
}
|
||||
|
||||
#include "../blocks.hpp"
|
||||
#include "UOrbSubscription.hpp"
|
||||
#include "UOrbPublication.hpp"
|
||||
#include <uORB/Subscription.hpp>
|
||||
#include <uORB/Publication.hpp>
|
||||
|
||||
namespace control
|
||||
{
|
||||
|
@ -94,16 +94,16 @@ class __EXPORT BlockUorbEnabledAutopilot : public SuperBlock
|
|||
{
|
||||
protected:
|
||||
// subscriptions
|
||||
UOrbSubscription<vehicle_attitude_s> _att;
|
||||
UOrbSubscription<vehicle_attitude_setpoint_s> _attCmd;
|
||||
UOrbSubscription<vehicle_rates_setpoint_s> _ratesCmd;
|
||||
UOrbSubscription<vehicle_global_position_s> _pos;
|
||||
UOrbSubscription<position_setpoint_triplet_s> _missionCmd;
|
||||
UOrbSubscription<manual_control_setpoint_s> _manual;
|
||||
UOrbSubscription<vehicle_status_s> _status;
|
||||
UOrbSubscription<parameter_update_s> _param_update;
|
||||
uORB::Subscription<vehicle_attitude_s> _att;
|
||||
uORB::Subscription<vehicle_attitude_setpoint_s> _attCmd;
|
||||
uORB::Subscription<vehicle_rates_setpoint_s> _ratesCmd;
|
||||
uORB::Subscription<vehicle_global_position_s> _pos;
|
||||
uORB::Subscription<position_setpoint_triplet_s> _missionCmd;
|
||||
uORB::Subscription<manual_control_setpoint_s> _manual;
|
||||
uORB::Subscription<vehicle_status_s> _status;
|
||||
uORB::Subscription<parameter_update_s> _param_update;
|
||||
// publications
|
||||
UOrbPublication<actuator_controls_s> _actuators;
|
||||
uORB::Publication<actuator_controls_s> _actuators;
|
||||
public:
|
||||
BlockUorbEnabledAutopilot(SuperBlock *parent, const char *name);
|
||||
virtual ~BlockUorbEnabledAutopilot();
|
||||
|
|
|
@ -175,14 +175,14 @@ void BlockMultiModeBacksideAutopilot::update()
|
|||
// the min/max velocity
|
||||
float v = _vLimit.update(sqrtf(
|
||||
_pos.vel_n * _pos.vel_n +
|
||||
_pos.vy * _pos.vy +
|
||||
_pos.vel_e * _pos.vel_e +
|
||||
_pos.vel_d * _pos.vel_d));
|
||||
|
||||
// limit velocity command between min/max velocity
|
||||
float vCmd = _vLimit.update(_vCmd.get());
|
||||
|
||||
// altitude hold
|
||||
float dThrottle = _h2Thr.update(_missionCmd.current.altitude - _pos.alt);
|
||||
float dThrottle = _h2Thr.update(_missionCmd.current.alt - _pos.alt);
|
||||
|
||||
// heading hold
|
||||
float psiError = _wrap_pi(_guide.getPsiCmd() - _att.yaw);
|
||||
|
@ -237,7 +237,7 @@ void BlockMultiModeBacksideAutopilot::update()
|
|||
// the min/max velocity
|
||||
float v = _vLimit.update(sqrtf(
|
||||
_pos.vel_n * _pos.vel_n +
|
||||
_pos.vy * _pos.vy +
|
||||
_pos.vel_e * _pos.vel_e +
|
||||
_pos.vel_d * _pos.vel_d));
|
||||
|
||||
// pitch channel -> rate of climb
|
||||
|
|
|
@ -32,8 +32,49 @@
|
|||
****************************************************************************/
|
||||
|
||||
/**
|
||||
* @file UOrbPublication.cpp
|
||||
* @file Publication.cpp
|
||||
*
|
||||
*/
|
||||
|
||||
#include "UOrbPublication.hpp"
|
||||
#include "Publication.hpp"
|
||||
#include "topics/vehicle_attitude.h"
|
||||
#include "topics/vehicle_local_position.h"
|
||||
#include "topics/vehicle_global_position.h"
|
||||
#include "topics/debug_key_value.h"
|
||||
#include "topics/actuator_controls.h"
|
||||
#include "topics/vehicle_global_velocity_setpoint.h"
|
||||
#include "topics/vehicle_attitude_setpoint.h"
|
||||
#include "topics/vehicle_rates_setpoint.h"
|
||||
#include "topics/actuator_outputs.h"
|
||||
#include "topics/encoders.h"
|
||||
|
||||
namespace uORB {
|
||||
|
||||
template<class T>
|
||||
Publication<T>::Publication(
|
||||
List<PublicationBase *> * list,
|
||||
const struct orb_metadata *meta) :
|
||||
T(), // initialize data structure to zero
|
||||
PublicationBase(list, meta) {
|
||||
}
|
||||
|
||||
template<class T>
|
||||
Publication<T>::~Publication() {}
|
||||
|
||||
template<class T>
|
||||
void * Publication<T>::getDataVoidPtr() {
|
||||
return (void *)(T *)(this);
|
||||
}
|
||||
|
||||
template class __EXPORT Publication<vehicle_attitude_s>;
|
||||
template class __EXPORT Publication<vehicle_local_position_s>;
|
||||
template class __EXPORT Publication<vehicle_global_position_s>;
|
||||
template class __EXPORT Publication<debug_key_value_s>;
|
||||
template class __EXPORT Publication<actuator_controls_s>;
|
||||
template class __EXPORT Publication<vehicle_global_velocity_setpoint_s>;
|
||||
template class __EXPORT Publication<vehicle_attitude_setpoint_s>;
|
||||
template class __EXPORT Publication<vehicle_rates_setpoint_s>;
|
||||
template class __EXPORT Publication<actuator_outputs_s>;
|
||||
template class __EXPORT Publication<encoders_s>;
|
||||
|
||||
}
|
|
@ -32,32 +32,29 @@
|
|||
****************************************************************************/
|
||||
|
||||
/**
|
||||
* @file UOrbPublication.h
|
||||
* @file Publication.h
|
||||
*
|
||||
*/
|
||||
|
||||
#pragma once
|
||||
|
||||
#include <uORB/uORB.h>
|
||||
#include "../block/Block.hpp"
|
||||
#include "../block/List.hpp"
|
||||
#include <containers/List.hpp>
|
||||
|
||||
|
||||
namespace control
|
||||
namespace uORB
|
||||
{
|
||||
|
||||
class Block;
|
||||
|
||||
/**
|
||||
* Base publication warapper class, used in list traversal
|
||||
* of various publications.
|
||||
*/
|
||||
class __EXPORT UOrbPublicationBase : public ListNode<control::UOrbPublicationBase *>
|
||||
class __EXPORT PublicationBase : public ListNode<uORB::PublicationBase *>
|
||||
{
|
||||
public:
|
||||
|
||||
UOrbPublicationBase(
|
||||
List<UOrbPublicationBase *> * list,
|
||||
PublicationBase(
|
||||
List<PublicationBase *> * list,
|
||||
const struct orb_metadata *meta) :
|
||||
_meta(meta),
|
||||
_handle(-1) {
|
||||
|
@ -71,7 +68,7 @@ public:
|
|||
}
|
||||
}
|
||||
virtual void *getDataVoidPtr() = 0;
|
||||
virtual ~UOrbPublicationBase() {
|
||||
virtual ~PublicationBase() {
|
||||
orb_unsubscribe(getHandle());
|
||||
}
|
||||
const struct orb_metadata *getMeta() { return _meta; }
|
||||
|
@ -83,12 +80,12 @@ protected:
|
|||
};
|
||||
|
||||
/**
|
||||
* UOrb Publication wrapper class
|
||||
* Publication wrapper class
|
||||
*/
|
||||
template<class T>
|
||||
class UOrbPublication :
|
||||
class Publication :
|
||||
public T, // this must be first!
|
||||
public UOrbPublicationBase
|
||||
public PublicationBase
|
||||
{
|
||||
public:
|
||||
/**
|
||||
|
@ -98,13 +95,9 @@ public:
|
|||
* @param meta The uORB metadata (usually from the ORB_ID() macro)
|
||||
* for the topic.
|
||||
*/
|
||||
UOrbPublication(
|
||||
List<UOrbPublicationBase *> * list,
|
||||
const struct orb_metadata *meta) :
|
||||
T(), // initialize data structure to zero
|
||||
UOrbPublicationBase(list, meta) {
|
||||
}
|
||||
virtual ~UOrbPublication() {}
|
||||
Publication(List<PublicationBase *> * list,
|
||||
const struct orb_metadata *meta);
|
||||
virtual ~Publication();
|
||||
/*
|
||||
* XXX
|
||||
* This function gets the T struct, assuming
|
||||
|
@ -112,7 +105,7 @@ public:
|
|||
* should use dynamic cast, but doesn't
|
||||
* seem to be available
|
||||
*/
|
||||
void *getDataVoidPtr() { return (void *)(T *)(this); }
|
||||
void *getDataVoidPtr();
|
||||
};
|
||||
|
||||
} // namespace control
|
||||
} // namespace uORB
|
|
@ -0,0 +1,103 @@
|
|||
/****************************************************************************
|
||||
*
|
||||
* Copyright (C) 2012 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 Subscription.cpp
|
||||
*
|
||||
*/
|
||||
|
||||
#include "Subscription.hpp"
|
||||
#include "topics/parameter_update.h"
|
||||
#include "topics/actuator_controls.h"
|
||||
#include "topics/vehicle_gps_position.h"
|
||||
#include "topics/sensor_combined.h"
|
||||
#include "topics/vehicle_attitude.h"
|
||||
#include "topics/vehicle_global_position.h"
|
||||
#include "topics/encoders.h"
|
||||
#include "topics/position_setpoint_triplet.h"
|
||||
#include "topics/vehicle_status.h"
|
||||
#include "topics/manual_control_setpoint.h"
|
||||
#include "topics/vehicle_local_position_setpoint.h"
|
||||
#include "topics/vehicle_local_position.h"
|
||||
#include "topics/vehicle_attitude_setpoint.h"
|
||||
#include "topics/vehicle_rates_setpoint.h"
|
||||
|
||||
namespace uORB
|
||||
{
|
||||
|
||||
bool __EXPORT SubscriptionBase::updated()
|
||||
{
|
||||
bool isUpdated = false;
|
||||
orb_check(_handle, &isUpdated);
|
||||
return isUpdated;
|
||||
}
|
||||
|
||||
template<class T>
|
||||
Subscription<T>::Subscription(
|
||||
List<SubscriptionBase *> * list,
|
||||
const struct orb_metadata *meta, unsigned interval) :
|
||||
T(), // initialize data structure to zero
|
||||
SubscriptionBase(list, meta) {
|
||||
setHandle(orb_subscribe(getMeta()));
|
||||
orb_set_interval(getHandle(), interval);
|
||||
}
|
||||
|
||||
template<class T>
|
||||
Subscription<T>::~Subscription() {}
|
||||
|
||||
template<class T>
|
||||
void * Subscription<T>::getDataVoidPtr() {
|
||||
return (void *)(T *)(this);
|
||||
}
|
||||
|
||||
template<class T>
|
||||
T Subscription<T>::getData() {
|
||||
return T(*this);
|
||||
}
|
||||
|
||||
template class __EXPORT Subscription<parameter_update_s>;
|
||||
template class __EXPORT Subscription<actuator_controls_s>;
|
||||
template class __EXPORT Subscription<vehicle_gps_position_s>;
|
||||
template class __EXPORT Subscription<sensor_combined_s>;
|
||||
template class __EXPORT Subscription<vehicle_attitude_s>;
|
||||
template class __EXPORT Subscription<vehicle_global_position_s>;
|
||||
template class __EXPORT Subscription<encoders_s>;
|
||||
template class __EXPORT Subscription<position_setpoint_triplet_s>;
|
||||
template class __EXPORT Subscription<vehicle_status_s>;
|
||||
template class __EXPORT Subscription<manual_control_setpoint_s>;
|
||||
template class __EXPORT Subscription<vehicle_local_position_setpoint_s>;
|
||||
template class __EXPORT Subscription<vehicle_local_position_s>;
|
||||
template class __EXPORT Subscription<vehicle_attitude_setpoint_s>;
|
||||
template class __EXPORT Subscription<vehicle_rates_setpoint_s>;
|
||||
|
||||
} // namespace uORB
|
|
@ -32,28 +32,25 @@
|
|||
****************************************************************************/
|
||||
|
||||
/**
|
||||
* @file UOrbSubscription.h
|
||||
* @file Subscription.h
|
||||
*
|
||||
*/
|
||||
|
||||
#pragma once
|
||||
|
||||
#include <uORB/uORB.h>
|
||||
#include "../block/Block.hpp"
|
||||
#include "../block/List.hpp"
|
||||
#include <containers/List.hpp>
|
||||
|
||||
|
||||
namespace control
|
||||
namespace uORB
|
||||
{
|
||||
|
||||
class Block;
|
||||
|
||||
/**
|
||||
* Base subscription warapper class, used in list traversal
|
||||
* of various subscriptions.
|
||||
*/
|
||||
class __EXPORT UOrbSubscriptionBase :
|
||||
public ListNode<control::UOrbSubscriptionBase *>
|
||||
class __EXPORT SubscriptionBase :
|
||||
public ListNode<SubscriptionBase *>
|
||||
{
|
||||
public:
|
||||
// methods
|
||||
|
@ -64,8 +61,8 @@ public:
|
|||
* @param meta The uORB metadata (usually from the ORB_ID() macro)
|
||||
* for the topic.
|
||||
*/
|
||||
UOrbSubscriptionBase(
|
||||
List<UOrbSubscriptionBase *> * list,
|
||||
SubscriptionBase(
|
||||
List<SubscriptionBase *> * list,
|
||||
const struct orb_metadata *meta) :
|
||||
_meta(meta),
|
||||
_handle() {
|
||||
|
@ -78,7 +75,7 @@ public:
|
|||
}
|
||||
}
|
||||
virtual void *getDataVoidPtr() = 0;
|
||||
virtual ~UOrbSubscriptionBase() {
|
||||
virtual ~SubscriptionBase() {
|
||||
orb_unsubscribe(_handle);
|
||||
}
|
||||
// accessors
|
||||
|
@ -93,12 +90,12 @@ protected:
|
|||
};
|
||||
|
||||
/**
|
||||
* UOrb Subscription wrapper class
|
||||
* Subscription wrapper class
|
||||
*/
|
||||
template<class T>
|
||||
class __EXPORT UOrbSubscription :
|
||||
class __EXPORT Subscription :
|
||||
public T, // this must be first!
|
||||
public UOrbSubscriptionBase
|
||||
public SubscriptionBase
|
||||
{
|
||||
public:
|
||||
/**
|
||||
|
@ -109,19 +106,13 @@ public:
|
|||
* for the topic.
|
||||
* @param interval The minimum interval in milliseconds between updates
|
||||
*/
|
||||
UOrbSubscription(
|
||||
List<UOrbSubscriptionBase *> * list,
|
||||
const struct orb_metadata *meta, unsigned interval) :
|
||||
T(), // initialize data structure to zero
|
||||
UOrbSubscriptionBase(list, meta) {
|
||||
setHandle(orb_subscribe(getMeta()));
|
||||
orb_set_interval(getHandle(), interval);
|
||||
}
|
||||
|
||||
Subscription(
|
||||
List<SubscriptionBase *> * list,
|
||||
const struct orb_metadata *meta, unsigned interval);
|
||||
/**
|
||||
* Deconstructor
|
||||
*/
|
||||
virtual ~UOrbSubscription() {}
|
||||
virtual ~Subscription();
|
||||
|
||||
/*
|
||||
* XXX
|
||||
|
@ -130,8 +121,8 @@ public:
|
|||
* should use dynamic cast, but doesn't
|
||||
* seem to be available
|
||||
*/
|
||||
void *getDataVoidPtr() { return (void *)(T *)(this); }
|
||||
T getData() { return T(*this); }
|
||||
void *getDataVoidPtr();
|
||||
T getData();
|
||||
};
|
||||
|
||||
} // namespace control
|
||||
} // namespace uORB
|
|
@ -41,4 +41,6 @@ MODULE_COMMAND = uorb
|
|||
MODULE_STACKSIZE = 4096
|
||||
|
||||
SRCS = uORB.cpp \
|
||||
objects_common.cpp
|
||||
objects_common.cpp \
|
||||
Publication.cpp \
|
||||
Subscription.cpp
|
||||
|
|
|
@ -190,3 +190,6 @@ ORB_DEFINE(navigation_capabilities, struct navigation_capabilities_s);
|
|||
|
||||
#include "topics/esc_status.h"
|
||||
ORB_DEFINE(esc_status, struct esc_status_s);
|
||||
|
||||
#include "topics/encoders.h"
|
||||
ORB_DEFINE(encoders, struct encoders_s);
|
||||
|
|
|
@ -32,20 +32,35 @@
|
|||
****************************************************************************/
|
||||
|
||||
/**
|
||||
* @file UOrbSubscription.cpp
|
||||
* @file encoders.h
|
||||
*
|
||||
* Encoders topic.
|
||||
*
|
||||
*/
|
||||
|
||||
#include "UOrbSubscription.hpp"
|
||||
#ifndef TOPIC_ENCODERS_H
|
||||
#define TOPIC_ENCODERS_H
|
||||
|
||||
namespace control
|
||||
{
|
||||
#include <stdint.h>
|
||||
#include "../uORB.h"
|
||||
|
||||
bool __EXPORT UOrbSubscriptionBase::updated()
|
||||
{
|
||||
bool isUpdated = false;
|
||||
orb_check(_handle, &isUpdated);
|
||||
return isUpdated;
|
||||
}
|
||||
/**
|
||||
* @addtogroup topics
|
||||
* @{
|
||||
*/
|
||||
|
||||
} // namespace control
|
||||
#define NUM_ENCODERS 4
|
||||
|
||||
struct encoders_s {
|
||||
uint64_t timestamp;
|
||||
int64_t counts[NUM_ENCODERS]; // counts of encoder
|
||||
float velocity[NUM_ENCODERS]; // counts of encoder/ second
|
||||
};
|
||||
|
||||
/**
|
||||
* @}
|
||||
*/
|
||||
|
||||
ORB_DECLARE(encoders);
|
||||
|
||||
#endif
|
Loading…
Reference in New Issue