dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

Merge pull request #3030 from dronecrew/control-update 

uORB/ control wrapper update
This commit is contained in:
Lorenz Meier 2015-10-22 00:22:58 +02:00
parent 8fa49d8ac5 f0ad724fda
commit c6b41b4e5e
13 changed files with 314 additions and 222 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
src/drivers/roboclaw/RoboClaw.cpp
View File

@ -299,8 +299,8 @@ int RoboClaw::update()
// if new data, send to motors
if (_actuators.updated()) {
_actuators.update();
setMotorDutyCycle(MOTOR_1, _actuators.control[CH_VOLTAGE_LEFT]);
setMotorDutyCycle(MOTOR_2, _actuators.control[CH_VOLTAGE_RIGHT]);
setMotorDutyCycle(MOTOR_1, _actuators.get().control[CH_VOLTAGE_LEFT]);
setMotorDutyCycle(MOTOR_2, _actuators.get().control[CH_VOLTAGE_RIGHT]);
}
return 0;

20
src/modules/controllib/block/Block.hpp
View File

@ -58,8 +58,8 @@ static const uint16_t maxPublicationsPerBlock = 100;
static const uint8_t blockNameLengthMax = 80;
// forward declaration
class SuperBlock;
class BlockParamBase;
class SuperBlock;
/**
*/
@ -106,24 +106,28 @@ public:
// methods
SuperBlock(SuperBlock *parent, const char *name) :
Block(parent, name),
_children() {
_children()
{
}
virtual ~SuperBlock() {};
virtual void setDt(float dt);
virtual void updateParams() {
virtual void updateParams()
{
Block::updateParams();
if (getChildren().getHead() != NULL) updateChildParams();
if (getChildren().getHead() != NULL) { updateChildParams(); }
}
virtual void updateSubscriptions() {
virtual void updateSubscriptions()
{
Block::updateSubscriptions();
if (getChildren().getHead() != NULL) updateChildSubscriptions();
if (getChildren().getHead() != NULL) { updateChildSubscriptions(); }
}
virtual void updatePublications() {
virtual void updatePublications()
{
Block::updatePublications();
if (getChildren().getHead() != NULL) updateChildPublications();
if (getChildren().getHead() != NULL) { updateChildPublications(); }
}
protected:
// methods

17
src/modules/controllib/block/BlockParam.cpp
View File

@ -65,6 +65,7 @@ BlockParamBase::BlockParamBase(Block *parent, const char *name, bool parent_pref
} else if (parent_prefix) {
snprintf(fullname, blockNameLengthMax, "%s_%s", parentName, name);
} else {
strncpy(fullname, name, blockNameLengthMax);
}
@ -74,15 +75,17 @@ BlockParamBase::BlockParamBase(Block *parent, const char *name, bool parent_pref
_handle = param_find(fullname);
if (_handle == PARAM_INVALID)
if (_handle == PARAM_INVALID) {
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() {
_val()
{
update();
}
@ -93,13 +96,15 @@ 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);
void BlockParam<T>::update()
{
if (_handle != PARAM_INVALID) { param_get(_handle, &_val); }
}
template <class T>
void BlockParam<T>::commit() {
if (_handle != PARAM_INVALID) param_set(_handle, &_val);
void BlockParam<T>::commit()
{
if (_handle != PARAM_INVALID) { param_set(_handle, &_val); }
}
template <class T>

5
src/modules/controllib/blocks.cpp
View File

@ -126,6 +126,7 @@ float BlockLowPass::update(float input)
if (!isfinite(getState())) {
setState(input);
}
float b = 2 * float(M_PI) * getFCut() * getDt();
float a = b / (1 + b);
setState(a * input + (1 - a)*getState());
@ -209,6 +210,7 @@ float BlockLowPass2::update(float input)
if (fabsf(_lp.get_cutoff_freq() - getFCutParam()) > FLT_EPSILON) {
_lp.set_cutoff_frequency(_fs, getFCutParam());
}
_state = _lp.apply(input);
return _state;
}
@ -340,8 +342,10 @@ int blockIntegralTrapTest()
float BlockDerivative::update(float input)
{
float output;
if (_initialized) {
output = _lowPass.update((input - getU()) / getDt());
} else {
// if this is the first call to update
// we have no valid derivative
@ -351,6 +355,7 @@ float BlockDerivative::update(float input)
output = 0.0f;
_initialized = true;
}
setU(input);
return output;
}

30
src/modules/controllib/blocks.hpp
View File

@ -317,7 +317,8 @@ public:
_kP(this, "") // only one param, no need to name
{};
virtual ~BlockP() {};
float update(float input) {
float update(float input)
{
return getKP() * input;
}
// accessors
@ -343,7 +344,8 @@ public:
_kI(this, "I")
{};
virtual ~BlockPI() {};
float update(float input) {
float update(float input)
{
return getKP() * input +
getKI() * getIntegral().update(input);
}
@ -374,7 +376,8 @@ public:
_kD(this, "D")
{};
virtual ~BlockPD() {};
float update(float input) {
float update(float input)
{
return getKP() * input +
getKD() * getDerivative().update(input);
}
@ -407,7 +410,8 @@ public:
_kD(this, "D")
{};
virtual ~BlockPID() {};
float update(float input) {
float update(float input)
{
return getKP() * input +
getKI() * getIntegral().update(input) +
getKD() * getDerivative().update(input);
@ -440,11 +444,13 @@ public:
SuperBlock(parent, name),
_trim(this, "TRIM"),
_limit(this, ""),
_val(0) {
_val(0)
{
update(0);
};
virtual ~BlockOutput() {};
void update(float input) {
void update(float input)
{
_val = _limit.update(input + getTrim());
}
// accessors
@ -472,13 +478,15 @@ public:
const char *name) :
Block(parent, name),
_min(this, "MIN"),
_max(this, "MAX") {
_max(this, "MAX")
{
// seed should be initialized somewhere
// in main program for all calls to rand
// XXX currently in nuttx if you seed to 0, rand breaks
};
virtual ~BlockRandUniform() {};
float update() {
float update()
{
static float rand_max = MAX_RAND;
float rand_val = rand();
float bounds = getMax() - getMin();
@ -503,13 +511,15 @@ public:
const char *name) :
Block(parent, name),
_mean(this, "MEAN"),
_stdDev(this, "DEV") {
_stdDev(this, "DEV")
{
// seed should be initialized somewhere
// in main program for all calls to rand
// XXX currently in nuttx if you seed to 0, rand breaks
};
virtual ~BlockRandGauss() {};
float update() {
float update()
{
static float V1, V2, S;
static int phase = 0;
float X;

27
src/modules/controllib/uorb/blocks.cpp
View File

@ -53,10 +53,11 @@ BlockWaypointGuidance::BlockWaypointGuidance(SuperBlock *parent, const char *nam
BlockWaypointGuidance::~BlockWaypointGuidance() {};
void BlockWaypointGuidance::update(vehicle_global_position_s &pos,
vehicle_attitude_s &att,
position_setpoint_s &missionCmd,
position_setpoint_s &lastMissionCmd)
void BlockWaypointGuidance::update(
const vehicle_global_position_s &pos,
const vehicle_attitude_s &att,
const position_setpoint_s &missionCmd,
const position_setpoint_s &lastMissionCmd)
{
// heading to waypoint
@ -83,16 +84,16 @@ void BlockWaypointGuidance::update(vehicle_global_position_s &pos,
BlockUorbEnabledAutopilot::BlockUorbEnabledAutopilot(SuperBlock *parent, const char *name) :
SuperBlock(parent, name),
// subscriptions
_att(ORB_ID(vehicle_attitude), 20, &getSubscriptions()),
_attCmd(ORB_ID(vehicle_attitude_setpoint), 20, &getSubscriptions()),
_ratesCmd(ORB_ID(vehicle_rates_setpoint), 20, &getSubscriptions()),
_pos(ORB_ID(vehicle_global_position), 20, &getSubscriptions()),
_missionCmd(ORB_ID(position_setpoint_triplet), 20, &getSubscriptions()),
_manual(ORB_ID(manual_control_setpoint), 20, &getSubscriptions()),
_status(ORB_ID(vehicle_status), 20, &getSubscriptions()),
_param_update(ORB_ID(parameter_update), 1000, &getSubscriptions()), // limit to 1 Hz
_att(ORB_ID(vehicle_attitude), 20, 0, &getSubscriptions()),
_attCmd(ORB_ID(vehicle_attitude_setpoint), 20, 0, &getSubscriptions()),
_ratesCmd(ORB_ID(vehicle_rates_setpoint), 20, 0, &getSubscriptions()),
_pos(ORB_ID(vehicle_global_position), 20, 0, &getSubscriptions()),
_missionCmd(ORB_ID(position_setpoint_triplet), 20, 0, &getSubscriptions()),
_manual(ORB_ID(manual_control_setpoint), 20, 0, &getSubscriptions()),
_status(ORB_ID(vehicle_status), 20, 0, &getSubscriptions()),
_param_update(ORB_ID(parameter_update), 1000, 0, &getSubscriptions()), // limit to 1 Hz
// publications
_actuators(ORB_ID(actuator_controls_0), &getPublications())
_actuators(ORB_ID(actuator_controls_0), -1, &getPublications())
{
}

8
src/modules/controllib/uorb/blocks.hpp
View File

@ -80,10 +80,10 @@ private:
public:
BlockWaypointGuidance(SuperBlock *parent, const char *name);
virtual ~BlockWaypointGuidance();
void update(vehicle_global_position_s &pos,
vehicle_attitude_s &att,
position_setpoint_s &missionCmd,
position_setpoint_s &lastMissionCmd);
void update(const vehicle_global_position_s &pos,
const vehicle_attitude_s &att,
const position_setpoint_s &missionCmd,
const position_setpoint_s &lastMissionCmd);
float getPsiCmd() { return _psiCmd; }
};

110
src/modules/fixedwing_backside/fixedwing.cpp
View File

@ -142,7 +142,7 @@ void BlockMultiModeBacksideAutopilot::update()
// store old position command before update if new command sent
if (_missionCmd.updated()) {
_lastMissionCmd = _missionCmd.getData();
_lastMissionCmd = _missionCmd.get();
}
// check for new updates
@ -152,15 +152,17 @@ void BlockMultiModeBacksideAutopilot::update()
updateSubscriptions();
// default all output to zero unless handled by mode
for (unsigned i = 4; i < NUM_ACTUATOR_CONTROLS; i++) {
_actuators.control[i] = 0.0f;
for (unsigned i = 0; i < actuator_controls_s::NUM_ACTUATOR_CONTROLS; i++) {
_actuators.get().control[i] = 0.0f;
}
// only update guidance in auto mode
if (_status.main_state == MAIN_STATE_AUTO) {
if (_status.get().main_state == vehicle_status_s::MAIN_STATE_AUTO_MISSION) {
// TODO use vehicle_control_mode here?
// update guidance
_guide.update(_pos, _att, _missionCmd.current, _lastMissionCmd.current);
_guide.update(_pos.get(), _att.get(),
_missionCmd.get().current,
_lastMissionCmd.current);
}
// XXX handle STABILIZED (loiter on spot) as well
@ -168,32 +170,32 @@ void BlockMultiModeBacksideAutopilot::update()
// the setpoint should update to loitering around this position
// handle autopilot modes
if (_status.main_state == MAIN_STATE_AUTO) {
if (_status.get().main_state == vehicle_status_s::MAIN_STATE_AUTO) {
// calculate velocity, XXX should be airspeed,
// but using ground speed for now for the purpose
// of control we will limit the velocity feedback between
// the min/max velocity
float v = _vLimit.update(sqrtf(
_pos.vel_n * _pos.vel_n +
_pos.vel_e * _pos.vel_e +
_pos.vel_d * _pos.vel_d));
_pos.get().vel_n * _pos.get().vel_n +
_pos.get().vel_e * _pos.get().vel_e +
_pos.get().vel_d * _pos.get().vel_d));
// limit velocity command between min/max velocity
float vCmd = _vLimit.update(_vCmd.get());
// altitude hold
float dThrottle = _h2Thr.update(_missionCmd.current.alt - _pos.alt);
float dThrottle = _h2Thr.update(_missionCmd.get().current.alt - _pos.get().alt);
// heading hold
float psiError = _wrap_pi(_guide.getPsiCmd() - _att.yaw);
float psiError = _wrap_pi(_guide.getPsiCmd() - _att.get().yaw);
float phiCmd = _phiLimit.update(_psi2Phi.update(psiError));
float pCmd = _phi2P.update(phiCmd - _att.roll);
float pCmd = _phi2P.update(phiCmd - _att.get().roll);
// velocity hold
// negative sign because nose over to increase speed
float thetaCmd = _theLimit.update(-_v2Theta.update(vCmd - v));
float qCmd = _theta2Q.update(thetaCmd - _att.pitch);
float qCmd = _theta2Q.update(thetaCmd - _att.get().pitch);
// yaw rate cmd
float rCmd = 0;
@ -203,14 +205,16 @@ void BlockMultiModeBacksideAutopilot::update()
float outputScale = velocityRatio * velocityRatio;
// this term scales the output based on the dynamic pressure change from trim
_stabilization.update(pCmd, qCmd, rCmd,
_att.rollspeed, _att.pitchspeed, _att.yawspeed,
_att.get().rollspeed,
_att.get().pitchspeed,
_att.get().yawspeed,
outputScale);
// output
_actuators.control[CH_AIL] = _stabilization.getAileron() + _trimAil.get();
_actuators.control[CH_ELV] = _stabilization.getElevator() + _trimElv.get();
_actuators.control[CH_RDR] = _stabilization.getRudder() + _trimRdr.get();
_actuators.control[CH_THR] = dThrottle + _trimThr.get();
_actuators.get().control[CH_AIL] = _stabilization.getAileron() + _trimAil.get();
_actuators.get().control[CH_ELV] = _stabilization.getElevator() + _trimElv.get();
_actuators.get().control[CH_RDR] = _stabilization.getRudder() + _trimRdr.get();
_actuators.get().control[CH_THR] = dThrottle + _trimThr.get();
// XXX limit throttle to manual setting (safety) for now.
// If it turns out to be confusing, it can be removed later once
@ -218,28 +222,29 @@ void BlockMultiModeBacksideAutopilot::update()
// This is not a hack, but a design choice.
// do not limit in HIL
if (_status.hil_state != HIL_STATE_ON) {
if (_status.get().hil_state != _vehicle_status::HIL_STATE_ON) {
/* limit to value of manual throttle */
_actuators.control[CH_THR] = (_actuators.control[CH_THR] < _manual.throttle) ?
_actuators.control[CH_THR] : _manual.throttle;
_actuators.get().control[CH_THR] =
(_actuators.get().control[CH_THR] < _manual.get().z) ?
_actuators.get().control[CH_THR] : _manual.get().z;
}
} else if (_status.main_state == MAIN_STATE_MANUAL) {
_actuators.control[CH_AIL] = _manual.roll;
_actuators.control[CH_ELV] = _manual.pitch;
_actuators.control[CH_RDR] = _manual.yaw;
_actuators.control[CH_THR] = _manual.throttle;
} else if (_status.get().main_state == vehicle_status_s::MAIN_STATE_MANUAL) {
_actuators.get().control[CH_AIL] = _manual.get().y;
_actuators.get().control[CH_ELV] = _manual.get().x;
_actuators.get().control[CH_RDR] = _manual.get().r;
_actuators.get().control[CH_THR] = _manual.get().z;
} else if (_status.main_state == MAIN_STATE_ALTCTL ||
_status.main_state == MAIN_STATE_POSCTL /* TODO, implement pos control */) {
} else if (_status.get().main_state == vehicle_status_s::MAIN_STATE_ALTCTL ||
_status.get().main_state == vehicle_status_s::MAIN_STATE_POSCTL /* TODO, implement pos control */) {
// calculate velocity, XXX should be airspeed, but using ground speed for now
// for the purpose of control we will limit the velocity feedback between
// the min/max velocity
float v = _vLimit.update(sqrtf(
_pos.vel_n * _pos.vel_n +
_pos.vel_e * _pos.vel_e +
_pos.vel_d * _pos.vel_d));
_pos.get().vel_n * _pos.get().vel_n +
_pos.get().vel_e * _pos.get().vel_e +
_pos.get().vel_d * _pos.get().vel_d));
// pitch channel -> rate of climb
// TODO, might want to put a gain on this, otherwise commanding
@ -248,33 +253,35 @@ void BlockMultiModeBacksideAutopilot::update()
//_crMax.get()*_manual.pitch - _pos.vz);
// roll channel -> bank angle
float phiCmd = _phiLimit.update(_manual.roll * _phiLimit.getMax());
float pCmd = _phi2P.update(phiCmd - _att.roll);
float phiCmd = _phiLimit.update(_manual.get().y * _phiLimit.getMax());
float pCmd = _phi2P.update(phiCmd - _att.get().roll);
// throttle channel -> velocity
// negative sign because nose over to increase speed
float vCmd = _vLimit.update(_manual.throttle *
float vCmd = _vLimit.update(_manual.get().z *
(_vLimit.getMax() - _vLimit.getMin()) +
_vLimit.getMin());
float thetaCmd = _theLimit.update(-_v2Theta.update(vCmd - v));
float qCmd = _theta2Q.update(thetaCmd - _att.pitch);
float qCmd = _theta2Q.update(thetaCmd - _att.get().pitch);
// yaw rate cmd
float rCmd = 0;
// stabilization
_stabilization.update(pCmd, qCmd, rCmd,
_att.rollspeed, _att.pitchspeed, _att.yawspeed);
_att.get().rollspeed,
_att.get().pitchspeed,
_att.get().yawspeed);
// output
_actuators.control[CH_AIL] = _stabilization.getAileron() + _trimAil.get();
_actuators.control[CH_ELV] = _stabilization.getElevator() + _trimElv.get();
_actuators.control[CH_RDR] = _stabilization.getRudder() + _trimRdr.get();
_actuators.get().control[CH_AIL] = _stabilization.getAileron() + _trimAil.get();
_actuators.get().control[CH_ELV] = _stabilization.getElevator() + _trimElv.get();
_actuators.get().control[CH_RDR] = _stabilization.getRudder() + _trimRdr.get();
// currently using manual throttle
// XXX if you enable this watch out, vz might be very noisy
//_actuators.control[CH_THR] = dThrottle + _trimThr.get();
_actuators.control[CH_THR] = _manual.throttle;
_actuators.get().control[CH_THR] = _manual.get().z;
// XXX limit throttle to manual setting (safety) for now.
// If it turns out to be confusing, it can be removed later once
@ -282,10 +289,11 @@ void BlockMultiModeBacksideAutopilot::update()
// This is not a hack, but a design choice.
/* do not limit in HIL */
if (_status.hil_state != HIL_STATE_ON) {
if (_status.get().hil_state != vehicle_status_s::HIL_STATE_ON) {
/* limit to value of manual throttle */
_actuators.control[CH_THR] = (_actuators.control[CH_THR] < _manual.throttle) ?
_actuators.control[CH_THR] : _manual.throttle;
_actuators.get().control[CH_THR] =
(_actuators.get().control[CH_THR] < _manual.get().z) ?
_actuators.get().control[CH_THR] : _manual.get().z;
}
// body rates controller, disabled for now
@ -294,13 +302,13 @@ void BlockMultiModeBacksideAutopilot::update()
} else if (
0 /*_status.manual_control_mode == VEHICLE_MANUAL_CONTROL_MODE_SAS*/) { // TODO use vehicle_control_mode here?
_stabilization.update(_manual.roll, _manual.pitch, _manual.yaw,
_att.rollspeed, _att.pitchspeed, _att.yawspeed);
_stabilization.update(_manual.get().y, _manual.get().x, _manual.get().r,
_att.get().rollspeed, _att.get().pitchspeed, _att.get().yawspeed);
_actuators.control[CH_AIL] = _stabilization.getAileron();
_actuators.control[CH_ELV] = _stabilization.getElevator();
_actuators.control[CH_RDR] = _stabilization.getRudder();
_actuators.control[CH_THR] = _manual.throttle;
_actuators.get().control[CH_AIL] = _stabilization.getAileron();
_actuators.get().control[CH_ELV] = _stabilization.getElevator();
_actuators.get().control[CH_RDR] = _stabilization.getRudder();
_actuators.get().control[CH_THR] = _manual.get().z;
}
// update all publications
@ -311,8 +319,8 @@ BlockMultiModeBacksideAutopilot::~BlockMultiModeBacksideAutopilot()
{
// send one last publication when destroyed, setting
// all output to zero
for (unsigned i = 0; i < NUM_ACTUATOR_CONTROLS; i++) {
_actuators.control[i] = 0.0f;
for (unsigned i = 0; i < actuator_controls_s::NUM_ACTUATOR_CONTROLS; i++) {
_actuators.get().control[i] = 0.0f;
}
updatePublications();

10
src/modules/fw_pos_control_l1/mtecs/mTecs.cpp
View File

@ -53,7 +53,7 @@ mTecs::mTecs() :
_airspeedMin(this, "FW_AIRSPD_MIN", false),
/* Publications */
#if defined(__PX4_NUTTX)
_status(ORB_ID(tecs_status), &getPublications()),
_status(ORB_ID(tecs_status), -1, &getPublications()),
#endif // defined(__PX4_NUTTX)
/* control blocks */
_controlTotalEnergy(this, "THR"),
@ -111,8 +111,8 @@ int mTecs::updateAltitudeSpeed(float flightPathAngle, float altitude, float alti
#if defined(__PX4_NUTTX)
/* Write part of the status message */
_status.altitudeSp = altitudeSp;
_status.altitude_filtered = altitudeFiltered;
_status.get().altitudeSp = altitudeSp;
_status.get().altitude_filtered = altitudeFiltered;
#endif // defined(__PX4_NUTTX)
@ -149,8 +149,8 @@ int mTecs::updateFlightPathAngleSpeed(float flightPathAngle, float flightPathAng
#if defined(__PX4_NUTTX)
/* Write part of the status message */
_status.airspeedSp = airspeedSp;
_status.airspeed_filtered = airspeedFiltered;
_status.get().airspeedSp = airspeedSp;
_status.get().airspeed_filtered = airspeedFiltered;
#endif // defined(__PX4_NUTTX)

55
src/modules/uORB/Publication.cpp
View File

@ -50,37 +50,63 @@
#include "topics/encoders.h"
#include "topics/tecs_status.h"
#include "topics/rc_channels.h"
#include "topics/filtered_bottom_flow.h"
#include <px4_defines.h>
namespace uORB
{
template<class T>
Publication<T>::Publication(
const struct orb_metadata *meta,
List<PublicationNode *> *list) :
T(), // initialize data structure to zero
PublicationNode(meta, list)
PublicationBase::PublicationBase(const struct orb_metadata *meta,
int priority) :
_meta(meta),
_priority(priority),
_instance(),
_handle(nullptr)
{
}
template<class T>
Publication<T>::~Publication() {}
template<class T>
void *Publication<T>::getDataVoidPtr()
void PublicationBase::update(void *data)
{
return (void *)(T *)(this);
if (_handle != nullptr) {
int ret = orb_publish(getMeta(), getHandle(), data);
if (ret != PX4_OK) { warnx("publish fail"); }
} else {
orb_advert_t handle;
if (_priority > 0) {
handle = orb_advertise_multi(
getMeta(), data,
&_instance, _priority);
} else {
handle = orb_advertise(getMeta(), data);
}
if (int64_t(handle) != PX4_ERROR) {
setHandle(handle);
} else {
warnx("advert fail");
}
}
}
PublicationBase::~PublicationBase()
{
}
PublicationNode::PublicationNode(const struct orb_metadata *meta,
int priority,
List<PublicationNode *> *list) :
PublicationBase(meta)
PublicationBase(meta, priority)
{
if (list != nullptr) { list->add(this); }
}
// explicit template instantiation
template class __EXPORT Publication<vehicle_attitude_s>;
template class __EXPORT Publication<vehicle_local_position_s>;
template class __EXPORT Publication<vehicle_global_position_s>;
@ -94,5 +120,6 @@ template class __EXPORT Publication<actuator_direct_s>;
template class __EXPORT Publication<encoders_s>;
template class __EXPORT Publication<tecs_status_s>;
template class __EXPORT Publication<rc_channels_s>;
template class __EXPORT Publication<filtered_bottom_flow_s>;
}

80
src/modules/uORB/Publication.hpp
View File

@ -42,13 +42,13 @@
#include <uORB/uORB.h>
#include <containers/List.hpp>
#include <systemlib/err.h>
namespace uORB
{
/**
* Base publication warapper class, used in list traversal
* Base publication wrapper class, used in list traversal
* of various publications.
*/
class __EXPORT PublicationBase
@ -58,50 +58,40 @@ public:
/**
* Constructor
*
*
* @param meta The uORB metadata (usually from the ORB_ID()
* macro) for the topic.
* @param meta The uORB metadata (usually from
* the ORB_ID() macro) for the topic.
* @param priority The priority for multi pub/sub, 0-based, -1 means
* don't publish as multi
*/
PublicationBase(const struct orb_metadata *meta) :
_meta(meta),
_handle(nullptr)
{
}
PublicationBase(const struct orb_metadata *meta,
int priority = -1);
/**
* Update the struct
* @param data The uORB message struct we are updating.
*/
void update(void *data)
{
if (_handle != nullptr) {
orb_publish(getMeta(), getHandle(), data);
} else {
setHandle(orb_advertise(getMeta(), data));
}
}
void update(void *data);
/**
* Deconstructor
*/
virtual ~PublicationBase()
{
}
virtual ~PublicationBase();
// accessors
const struct orb_metadata *getMeta() { return _meta; }
orb_advert_t getHandle() { return _handle; }
protected:
// disallow copy
PublicationBase(const PublicationBase &other);
// disallow assignment
PublicationBase &operator=(const PublicationBase &other);
// accessors
void setHandle(orb_advert_t handle) { _handle = handle; }
// attributes
const struct orb_metadata *_meta;
int _priority;
int _instance;
orb_advert_t _handle;
private:
// forbid copy
PublicationBase(const PublicationBase &) : _meta(), _handle() {};
// forbid assignment
PublicationBase &operator = (const PublicationBase &);
};
/**
@ -121,13 +111,14 @@ public:
/**
* Constructor
*
*
* @param meta The uORB metadata (usually from the ORB_ID()
* macro) for the topic.
* @param list A pointer to a list of subscriptions
* that this should be appended to.
* @param meta The uORB metadata (usually from
* the ORB_ID() macro) for the topic.
* @param priority The priority for multi pub, 0-based.
* @param list A list interface for adding to
* list during construction
*/
PublicationNode(const struct orb_metadata *meta,
int priority = -1,
List<PublicationNode *> *list = nullptr);
/**
@ -142,7 +133,6 @@ public:
*/
template<class T>
class __EXPORT Publication :
public T, // this must be first!
public PublicationNode
{
public:
@ -151,33 +141,37 @@ public:
*
* @param meta The uORB metadata (usually from
* the ORB_ID() macro) for the topic.
* @param priority The priority for multi pub, 0-based.
* @param list A list interface for adding to
* list during construction
*/
Publication(const struct orb_metadata *meta,
List<PublicationNode *> *list = nullptr);
int priority = -1,
List<PublicationNode *> *list = nullptr) :
PublicationNode(meta, priority, list),
_data()
{
}
/**
* Deconstructor
**/
virtual ~Publication();
virtual ~Publication() {};
/*
* XXX
* This function gets the T struct, assuming
* the struct is the first base class, this
* should use dynamic cast, but doesn't
* seem to be available
*/
void *getDataVoidPtr();
* This function gets the T struct
* */
T &get() { return _data; }
/**
* Create an update function that uses the embedded struct.
*/
void update()
{
PublicationBase::update(getDataVoidPtr());
PublicationBase::update((void *)(&_data));
}
private:
T _data;
};
} // namespace uORB

80
src/modules/uORB/Subscription.cpp
View File

@ -54,37 +54,89 @@
#include "topics/vehicle_attitude_setpoint.h"
#include "topics/vehicle_rates_setpoint.h"
#include "topics/rc_channels.h"
#include "topics/battery_status.h"
#include "topics/optical_flow.h"
#include "topics/distance_sensor.h"
#include "topics/home_position.h"
#include "topics/vehicle_control_mode.h"
#include "topics/actuator_armed.h"
#include "topics/att_pos_mocap.h"
#include "topics/vision_position_estimate.h"
#include <px4_defines.h>
namespace uORB
{
SubscriptionBase::SubscriptionBase(const struct orb_metadata *meta,
unsigned interval, unsigned instance) :
_meta(meta),
_instance(instance),
_handle()
{
if (_instance > 0) {
_handle = orb_subscribe_multi(
getMeta(), instance);
} else {
_handle = orb_subscribe(getMeta());
}
if (_handle < 0) { warnx("sub failed"); }
orb_set_interval(getHandle(), interval);
}
bool SubscriptionBase::updated()
{
bool isUpdated = false;
int ret = orb_check(_handle, &isUpdated);
if (ret != PX4_OK) { warnx("orb check failed"); }
return isUpdated;
}
void SubscriptionBase::update(void *data)
{
if (updated()) {
int ret = orb_copy(_meta, _handle, data);
if (ret != PX4_OK) { warnx("orb copy failed"); }
}
}
SubscriptionBase::~SubscriptionBase()
{
int ret = orb_unsubscribe(_handle);
if (ret != PX4_OK) { warnx("orb unsubscribe failed"); }
}
template <class T>
Subscription<T>::Subscription(
const struct orb_metadata *meta,
Subscription<T>::Subscription(const struct orb_metadata *meta,
unsigned interval,
int instance,
List<SubscriptionNode *> *list) :
T(), // initialize data structure to zero
SubscriptionNode(meta, interval, list)
SubscriptionNode(meta, interval, instance, list),
_data() // initialize data structure to zero
{
}
template <class T>
Subscription<T>::~Subscription() {}
template<class T>
void *Subscription<T>::getDataVoidPtr()
Subscription<T>::~Subscription()
{
return (void *)(T *)(this);
}
template <class T>
T Subscription<T>::getData()
void Subscription<T>::update()
{
return T(*this);
SubscriptionBase::update((void *)(&_data));
}
template <class T>
const T &Subscription<T>::get() { return _data; }
template class __EXPORT Subscription<parameter_update_s>;
template class __EXPORT Subscription<actuator_controls_s>;
template class __EXPORT Subscription<vehicle_gps_position_s>;
@ -104,5 +156,11 @@ template class __EXPORT Subscription<vehicle_rates_setpoint_s>;
template class __EXPORT Subscription<rc_channels_s>;
template class __EXPORT Subscription<vehicle_control_mode_s>;
template class __EXPORT Subscription<actuator_armed_s>;
template class __EXPORT Subscription<battery_status_s>;
template class __EXPORT Subscription<home_position_s>;
template class __EXPORT Subscription<optical_flow_s>;
template class __EXPORT Subscription<distance_sensor_s>;
template class __EXPORT Subscription<att_pos_mocap_s>;
template class __EXPORT Subscription<vision_position_estimate_s>;
} // namespace uORB

66
src/modules/uORB/Subscription.hpp
View File

@ -42,6 +42,7 @@
#include <uORB/uORB.h>
#include <containers/List.hpp>
#include <systemlib/err.h>
namespace uORB
{
@ -60,47 +61,29 @@ public:
*
* @param meta The uORB metadata (usually from the ORB_ID()
* macro) for the topic.
*
* @param interval The minimum interval in milliseconds
* between updates
* @param instance The instance for multi sub.
*/
SubscriptionBase(const struct orb_metadata *meta,
unsigned interval = 0) :
_meta(meta),
_handle()
{
setHandle(orb_subscribe(getMeta()));
orb_set_interval(getHandle(), interval);
}
unsigned interval = 0, unsigned instance = 0);
/**
* Check if there is a new update.
* */
bool updated()
{
bool isUpdated = false;
orb_check(_handle, &isUpdated);
return isUpdated;
}
bool updated();
/**
* Update the struct
* @param data The uORB message struct we are updating.
*/
void update(void *data)
{
if (updated()) {
orb_copy(_meta, _handle, data);
}
}
void update(void *data);
/**
* Deconstructor
*/
virtual ~SubscriptionBase()
{
orb_unsubscribe(_handle);
}
virtual ~SubscriptionBase();
// accessors
const struct orb_metadata *getMeta() { return _meta; }
int getHandle() { return _handle; }
@ -109,12 +92,13 @@ protected:
void setHandle(int handle) { _handle = handle; }
// attributes
const struct orb_metadata *_meta;
int _instance;
int _handle;
private:
// forbid copy
// disallow copy
SubscriptionBase(const SubscriptionBase &other);
// forbid assignment
SubscriptionBase &operator = (const SubscriptionBase &);
// disallow assignment
SubscriptionBase &operator=(const SubscriptionBase &other);
};
/**
@ -123,7 +107,7 @@ private:
typedef SubscriptionBase SubscriptionTiny;
/**
* The publication base class as a list node.
* The subscription base class as a list node.
*/
class __EXPORT SubscriptionNode :
@ -134,18 +118,19 @@ public:
/**
* Constructor
*
*
* @param meta The uORB metadata (usually from the ORB_ID()
* macro) for the topic.
* @param interval The minimum interval in milliseconds
* between updates
* @param instance The instance for multi sub.
* @param list A pointer to a list of subscriptions
* that this should be appended to.
*/
SubscriptionNode(const struct orb_metadata *meta,
unsigned interval = 0,
int instance = 0,
List<SubscriptionNode *> *list = nullptr) :
SubscriptionBase(meta, interval),
SubscriptionBase(meta, interval, instance),
_interval(interval)
{
if (list != nullptr) { list->add(this); }
@ -169,7 +154,6 @@ protected:
*/
template<class T>
class __EXPORT Subscription :
public T, // this must be first!
public SubscriptionNode
{
public:
@ -185,7 +169,9 @@ public:
*/
Subscription(const struct orb_metadata *meta,
unsigned interval = 0,
int instance = 0,
List<SubscriptionNode *> *list = nullptr);
/**
* Deconstructor
*/
@ -195,20 +181,14 @@ public:
/**
* Create an update function that uses the embedded struct.
*/
void update()
{
SubscriptionBase::update(getDataVoidPtr());
}
void update();
/*
* XXX
* This function gets the T struct, assuming
* the struct is the first base class, this
* should use dynamic cast, but doesn't
* seem to be available
*/
void *getDataVoidPtr();
T getData();
* This function gets the T struct data
* */
const T &get();
private:
T _data;
};
} // namespace uORB