fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-06-26 05:10:34 +08:00
Code Issues Packages Projects Releases Wiki Activity

Merge branch 'master' of github.com:PX4/Firmware into beta_mavlink2

Browse Source
This commit is contained in:
Lorenz Meier
2014-03-21 10:47:36 +01:00
parent 2988136e7e 057bcf3172
commit 193397b228
28 changed files with 418 additions and 220 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/att_pos_estimator_ekf/KalmanNav.hpp
+8 -8
View File
@@ -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 */
src/modules/controllib/block/Block.cpp
+5 -4
View File
@@ -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) {
src/modules/controllib/block/Block.hpp
+11 -7
View File
@@ -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;
};
src/modules/controllib/block/BlockParam.cpp
+25
View File
@@ -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
src/modules/controllib/block/BlockParam.hpp
+12 -29
View File
@@ -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
src/modules/controllib/module.mk
-2
View File
@@ -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
src/modules/controllib/uorb/UOrbPublication.cpp
-39
View File
@@ -1,39 +0,0 @@
/****************************************************************************
*
* 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 UOrbPublication.cpp
*
*/
#include "UOrbPublication.hpp"
src/modules/controllib/uorb/blocks.hpp
+11 -11
View File
@@ -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();
src/modules/fixedwing_backside/fixedwing.cpp
+3 -3
View File
@@ -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
src/modules/fw_att_control/fw_att_control_main.cpp
+27 -36
View File
@@ -108,14 +108,14 @@ private:
bool _task_should_exit; /**< if true, sensor task should exit */
int _control_task; /**< task handle for sensor task */
int _att_sub; /**< vehicle attitude subscription */
int _accel_sub; /**< accelerometer subscription */
int _att_sub; /**< vehicle attitude subscription */
int _accel_sub; /**< accelerometer subscription */
int _att_sp_sub; /**< vehicle attitude setpoint */
int _attitude_sub; /**< raw rc channels data subscription */
int _airspeed_sub; /**< airspeed subscription */
int _vcontrol_mode_sub; /**< vehicle status subscription */
int _params_sub; /**< notification of parameter updates */
int _manual_sub; /**< notification of manual control updates */
int _params_sub; /**< notification of parameter updates */
int _manual_sub; /**< notification of manual control updates */
int _global_pos_sub; /**< global position subscription */
orb_advert_t _rate_sp_pub; /**< rate setpoint publication */
@@ -123,20 +123,19 @@ private:
orb_advert_t _actuators_0_pub; /**< actuator control group 0 setpoint */
orb_advert_t _actuators_1_pub; /**< actuator control group 1 setpoint (Airframe) */
struct vehicle_attitude_s _att; /**< vehicle attitude */
struct accel_report _accel; /**< body frame accelerations */
struct vehicle_attitude_s _att; /**< vehicle attitude */
struct accel_report _accel; /**< body frame accelerations */
struct vehicle_attitude_setpoint_s _att_sp; /**< vehicle attitude setpoint */
struct manual_control_setpoint_s _manual; /**< r/c channel data */
struct airspeed_s _airspeed; /**< airspeed */
struct vehicle_control_mode_s _vcontrol_mode; /**< vehicle control mode */
struct airspeed_s _airspeed; /**< airspeed */
struct vehicle_control_mode_s _vcontrol_mode; /**< vehicle control mode */
struct actuator_controls_s _actuators; /**< actuator control inputs */
struct actuator_controls_s _actuators_airframe; /**< actuator control inputs */
struct vehicle_global_position_s _global_pos; /**< global position */
struct actuator_controls_s _actuators_airframe; /**< actuator control inputs */
struct vehicle_global_position_s _global_pos; /**< global position */
perf_counter_t _loop_perf; /**< loop performance counter */
bool _setpoint_valid; /**< flag if the position control setpoint is valid */
bool _airspeed_valid; /**< flag if the airspeed measurement is valid */
struct {
float tconst;
@@ -245,7 +244,7 @@ private:
/**
* Check for airspeed updates.
*/
bool vehicle_airspeed_poll();
void vehicle_airspeed_poll();
/**
* Check for accel updates.
@@ -308,19 +307,18 @@ FixedwingAttitudeControl::FixedwingAttitudeControl() :
/* performance counters */
_loop_perf(perf_alloc(PC_ELAPSED, "fw att control")),
/* states */
_setpoint_valid(false),
_airspeed_valid(false)
_setpoint_valid(false)
{
/* safely initialize structs */
_att = {0};
_accel = {0};
_att_sp = {0};
_manual = {0};
_airspeed = {0};
_vcontrol_mode = {0};
_actuators = {0};
_actuators_airframe = {0};
_global_pos = {0};
_att = {};
_accel = {};
_att_sp = {};
_manual = {};
_airspeed = {};
_vcontrol_mode = {};
_actuators = {};
_actuators_airframe = {};
_global_pos = {};
_parameter_handles.tconst = param_find("FW_ATT_TC");
@@ -482,7 +480,7 @@ FixedwingAttitudeControl::vehicle_manual_poll()
}
}
bool
void
FixedwingAttitudeControl::vehicle_airspeed_poll()
{
/* check if there is a new position */
@@ -492,10 +490,7 @@ FixedwingAttitudeControl::vehicle_airspeed_poll()
if (airspeed_updated) {
orb_copy(ORB_ID(airspeed), _airspeed_sub, &_airspeed);
// warnx("airspeed poll: ind: %.4f, true: %.4f", _airspeed.indicated_airspeed_m_s, _airspeed.true_airspeed_m_s);
return true;
}
return false;
}
void
@@ -569,7 +564,7 @@ FixedwingAttitudeControl::task_main()
parameters_update();
/* get an initial update for all sensor and status data */
(void)vehicle_airspeed_poll();
vehicle_airspeed_poll();
vehicle_setpoint_poll();
vehicle_accel_poll();
vehicle_control_mode_poll();
@@ -626,7 +621,7 @@ FixedwingAttitudeControl::task_main()
/* load local copies */
orb_copy(ORB_ID(vehicle_attitude), _att_sub, &_att);
_airspeed_valid = vehicle_airspeed_poll();
vehicle_airspeed_poll();
vehicle_setpoint_poll();
@@ -666,9 +661,9 @@ FixedwingAttitudeControl::task_main()
float airspeed;
/* if airspeed is smaller than min, the sensor is not giving good readings */
if (!_airspeed_valid ||
(_airspeed.indicated_airspeed_m_s < 0.5f * _parameters.airspeed_min) ||
!isfinite(_airspeed.indicated_airspeed_m_s)) {
if ((_airspeed.indicated_airspeed_m_s < 0.5f * _parameters.airspeed_min) ||
!isfinite(_airspeed.indicated_airspeed_m_s) ||
hrt_elapsed_time(&_airspeed.timestamp) > 1e6) {
airspeed = _parameters.airspeed_trim;
} else {
@@ -791,10 +786,6 @@ FixedwingAttitudeControl::task_main()
warnx("Non-finite setpoint roll_sp: %.4f, pitch_sp %.4f", roll_sp, pitch_sp);
}
// warnx("aspd: %s: %6.2f, aspd scaling: %6.2f, controls: %5.2f %5.2f %5.2f %5.2f", (_airspeed_valid) ? "valid" : "unknown",
// airspeed, airspeed_scaling, _actuators.control[0], _actuators.control[1],
// _actuators.control[2], _actuators.control[3]);
/*
* Lazily publish the rate setpoint (for analysis, the actuators are published below)
* only once available
src/modules/sensors/sensors.cpp
+1
View File
@@ -1030,6 +1030,7 @@ Sensors::diff_pres_poll(struct sensor_combined_s &raw)
raw.differential_pressure_pa = _diff_pres.differential_pressure_pa;
raw.differential_pressure_timestamp = _diff_pres.timestamp;
_airspeed.timestamp = hrt_absolute_time();
_airspeed.indicated_airspeed_m_s = calc_indicated_airspeed(_diff_pres.differential_pressure_pa);
_airspeed.true_airspeed_m_s = calc_true_airspeed(_diff_pres.differential_pressure_pa + raw.baro_pres_mbar * 1e2f,
raw.baro_pres_mbar * 1e2f, raw.baro_temp_celcius - PCB_TEMP_ESTIMATE_DEG);
src/modules/systemlib/system_params.c
+11
View File
@@ -60,3 +60,14 @@ PARAM_DEFINE_INT32(SYS_AUTOSTART, 0);
* @group System
*/
PARAM_DEFINE_INT32(SYS_AUTOCONFIG, 0);
/**
* Set usage of IO board
*
* Can be used to use a standard startup script but with a FMU only set-up. Set to 0 to force the FMU only set-up.
*
* @min 0
* @max 1
* @group System
*/
PARAM_DEFINE_INT32(SYS_USE_IO, 1);
src/modules/controllib/block/List.hpp → src/modules/uORB/Publication.cpp
+39 -30
View File
@@ -32,40 +32,49 @@
****************************************************************************/
/**
* @file Node.h
* @file Publication.cpp
*
* A node of a linked list.
*/
#pragma once
#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>
class __EXPORT ListNode
{
public:
ListNode() : _sibling(NULL) {
}
void setSibling(T sibling) { _sibling = sibling; }
T getSibling() { return _sibling; }
T get() {
return _sibling;
}
protected:
T _sibling;
};
Publication<T>::Publication(
List<PublicationBase *> * list,
const struct orb_metadata *meta) :
T(), // initialize data structure to zero
PublicationBase(list, meta) {
}
template<class T>
class __EXPORT List
{
public:
List() : _head() {
}
void add(T newNode) {
newNode->setSibling(getHead());
setHead(newNode);
}
T getHead() { return _head; }
private:
void setHead(T &head) { _head = head; }
T _head;
};
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>;
}
src/modules/controllib/uorb/UOrbPublication.hpp → src/modules/uORB/Publication.hpp
+15 -22
View File
@@ -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
src/modules/uORB/Subscription.cpp
+103
View File
@@ -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
src/modules/controllib/uorb/UOrbSubscription.hpp → src/modules/uORB/Subscription.hpp
+18 -27
View File
@@ -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
src/modules/uORB/module.mk
+3 -1
View File
@@ -41,4 +41,6 @@ MODULE_COMMAND = uorb
MODULE_STACKSIZE = 4096
SRCS = uORB.cpp \
objects_common.cpp
objects_common.cpp \
Publication.cpp \
Subscription.cpp
src/modules/uORB/objects_common.cpp
+3
View File
@@ -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);
src/modules/controllib/uorb/UOrbSubscription.cpp → src/modules/uORB/topics/encoders.h
+26 -11
View File
@@ -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