fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-04-14 10:07:39 +08:00
Code Issues Packages Projects Releases Wiki Activity

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

Browse Source
This commit is contained in:
Lorenz Meier 2014-03-21 18:15:37 +01:00
commit df11310994
30 changed files with 603 additions and 323 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

25
ROMFS/px4fmu_common/init.d/3033_wingwing
View File

@ -7,4 +7,29 @@
sh /etc/init.d/rc.fw_defaults
if [ $DO_AUTOCONFIG == yes ]
then
param set BAT_N_CELLS 2
param set FW_AIRSPD_MAX 15
param set FW_AIRSPD_MIN 7
param set FW_AIRSPD_TRIM 11
param set FW_ATT_TC 0.3
param set FW_L1_DAMPING 0.74
param set FW_L1_PERIOD 12
param set FW_LND_ANG 15
param set FW_LND_FLALT 5
param set FW_LND_HHDIST 15
param set FW_LND_HVIRT 13
param set FW_LND_TLALT 5
param set FW_THR_LND_MAX 0
param set FW_P_ROLLFF 2
param set FW_PR_FF 0.6
param set FW_PR_IMAX 0.2
param set FW_PR_P 0.06
param set FW_RR_FF 0.6
param set FW_RR_IMA 0.2
param set FW_RR_P 0.09
param set FW_THR_CRUISE 0.65
fi
set MIXER FMU_Q

2
ROMFS/px4fmu_common/init.d/rc.fw_defaults
View File

@ -11,4 +11,6 @@ then
param set NAV_RTL_ALT 100
param set NAV_RTL_LAND_T -1
param set NAV_ACCEPT_RAD 50
param set RC_SCALE_ROLL 1
param set RC_SCALE_PITCH 1
fi

4
ROMFS/px4fmu_common/init.d/rc.usb
View File

@ -6,6 +6,10 @@
echo "Starting MAVLink on this USB console"
mavlink start -r 10000 -d /dev/ttyACM0
# Enable a number of interesting streams we want via USB
mavlink stream -d /dev/ttyACM0 -s NAMED_VALUE_FLOAT -r 10
mavlink stream -d /dev/ttyACM0 -s OPTICAL_FLOW -r 10
mavlink stream -d /dev/ttyACM0 -s ATTITUDE_CONTROLS -r 30
# Exit shell to make it available to MAVLink
exit

4
src/drivers/md25/md25.cpp
View File

@ -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

4
src/drivers/md25/md25.hpp
View File

@ -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;

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

@ -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>

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

@ -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;

6
src/modules/controllib/block/List.hpp → src/include/containers/List.hpp
View File

@ -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; }

16
src/modules/att_pos_estimator_ekf/KalmanNav.hpp
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 */

9
src/modules/controllib/block/Block.cpp
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) {

18
src/modules/controllib/block/Block.hpp
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;
};

25
src/modules/controllib/block/BlockParam.cpp
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

41
src/modules/controllib/block/BlockParam.hpp
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

2
src/modules/controllib/module.mk
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

22
src/modules/controllib/uorb/blocks.hpp
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();

6
src/modules/fixedwing_backside/fixedwing.cpp
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

63
src/modules/fw_att_control/fw_att_control_main.cpp
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

25
src/modules/mavlink/mavlink.c
View File

@ -45,31 +45,6 @@
#include <string.h>
#include "mavlink_bridge_header.h"
#include <systemlib/param/param.h>
// #include <drivers/drv_hrt.h>
// #include <time.h>
// #include <float.h>
// #include <unistd.h>
// #include <nuttx/sched.h>
// #include <sys/prctl.h>
// #include <termios.h>
// #include <errno.h>
// #include <stdlib.h>
// #include <poll.h>
//
// #include <systemlib/systemlib.h>
// #include <systemlib/err.h>
// #include <mavlink/mavlink_log.h>
// #include <commander/px4_custom_mode.h>
// #include "waypoints.h"
// #include "orb_topics.h"
// #include "mavlink_hil.h"
// #include "util.h"
// #include "waypoints.h"
// #include "mavlink_parameters.h"
// #include <uORB/topics/mission_result.h>
/* define MAVLink specific parameters */
/**

99
src/modules/mavlink/mavlink_main.cpp
View File

@ -151,8 +151,9 @@ mavlink_send_uart_bytes(mavlink_channel_t channel, const uint8_t *ch, int length
}
/* no valid instance, bail */
if (!instance)
if (!instance) {
return;
}
int uart = instance->get_uart_fd();
@ -165,12 +166,12 @@ mavlink_send_uart_bytes(mavlink_channel_t channel, const uint8_t *ch, int length
int buf_free = 0;
if (instance->get_flow_control_enabled()
&& ioctl(uart, FIONWRITE, (unsigned long)&buf_free) == 0) {
&& ioctl(uart, FIONWRITE, (unsigned long)&buf_free) == 0) {
if (buf_free == 0) {
if (last_write_times[(unsigned)channel] != 0 &&
hrt_elapsed_time(&last_write_times[(unsigned)channel]) > 500*1000UL) {
hrt_elapsed_time(&last_write_times[(unsigned)channel]) > 500 * 1000UL) {
warnx("DISABLING HARDWARE FLOW CONTROL");
instance->enable_flow_control(false);
@ -225,36 +226,42 @@ Mavlink::Mavlink() :
case 0:
_channel = MAVLINK_COMM_0;
break;
case 1:
_channel = MAVLINK_COMM_1;
break;
case 2:
_channel = MAVLINK_COMM_2;
break;
case 3:
_channel = MAVLINK_COMM_3;
break;
#ifdef MAVLINK_COMM_4
case 4:
_channel = MAVLINK_COMM_4;
break;
#endif
#ifdef MAVLINK_COMM_5
case 5:
case 5:
_channel = MAVLINK_COMM_5;
break;
#endif
#ifdef MAVLINK_COMM_6
case 6:
_channel = MAVLINK_COMM_6;
break;
#endif
default:
errx(1, "instance ID is out of range");
break;
}
LL_APPEND(_mavlink_instances, this);
}
Mavlink::~Mavlink()
@ -280,6 +287,7 @@ Mavlink::~Mavlink()
}
} while (_task_running);
}
LL_DELETE(_mavlink_instances, this);
}
@ -589,7 +597,7 @@ int Mavlink::mavlink_open_uart(int baud, const char *uart_name, struct termios *
/* setup output flow control */
if (enable_flow_control(true)) {
warnx("ERR FLOW CTRL EN");
warnx("hardware flow control not supported");
}
}
@ -605,12 +613,16 @@ Mavlink::enable_flow_control(bool enabled)
}
struct termios uart_config;
int ret = tcgetattr(_uart_fd, &uart_config);
if (enabled) {
uart_config.c_cflag |= CRTSCTS;
} else {
uart_config.c_cflag &= ~CRTSCTS;
}
ret = tcsetattr(_uart_fd, TCSANOW, &uart_config);
if (!ret) {
@ -1653,6 +1665,9 @@ Mavlink::task_main(int argc, char *argv[])
case 'm':
if (strcmp(optarg, "custom") == 0) {
_mode = MAVLINK_MODE_CUSTOM;
} else if (strcmp(optarg, "camera") == 0) {
_mode = MAVLINK_MODE_CAMERA;
}
break;
@ -1696,6 +1711,10 @@ Mavlink::task_main(int argc, char *argv[])
warnx("mode: CUSTOM");
break;
case MAVLINK_MODE_CAMERA:
warnx("mode: CAMERA");
break;
default:
warnx("ERROR: Unknown mode");
break;
@ -1703,7 +1722,7 @@ Mavlink::task_main(int argc, char *argv[])
_mavlink_wpm_comp_id = MAV_COMP_ID_MISSIONPLANNER;
warnx("data rate: %d bps, port: %s, baud: %d", _datarate, _device_name, (int)_baudrate);
warnx("data rate: %d Bytes/s, port: %s, baud: %d", _datarate, _device_name, _baudrate);
/* flush stdout in case MAVLink is about to take it over */
fflush(stdout);
@ -1763,6 +1782,14 @@ Mavlink::task_main(int argc, char *argv[])
configure_stream("GLOBAL_POSITION_INT", 3.0f * rate_mult);
configure_stream("LOCAL_POSITION_NED", 3.0f * rate_mult);
configure_stream("RC_CHANNELS_RAW", 1.0f * rate_mult);
configure_stream("NAMED_VALUE_FLOAT", 1.0f * rate_mult);
break;
case MAVLINK_MODE_CAMERA:
configure_stream("SYS_STATUS", 1.0f);
configure_stream("ATTITUDE", 15.0f * rate_mult);
configure_stream("GLOBAL_POSITION_INT", 15.0f * rate_mult);
configure_stream("CAMERA_CAPTURE", 1.0f);
break;
default:
@ -1773,11 +1800,14 @@ Mavlink::task_main(int argc, char *argv[])
_mavlink_param_queue_index = param_count();
MavlinkRateLimiter slow_rate_limiter(2000000.0f / rate_mult);
MavlinkRateLimiter fast_rate_limiter(20000.0f / rate_mult);
MavlinkRateLimiter fast_rate_limiter(30000.0f / rate_mult);
/* set main loop delay depending on data rate to minimize CPU overhead */
_main_loop_delay = MAIN_LOOP_DELAY / rate_mult;
/* now the instance is fully initialized and we can bump the instance count */
LL_APPEND(_mavlink_instances, this);
while (!_task_should_exit) {
/* main loop */
usleep(_main_loop_delay);
@ -1924,10 +1954,18 @@ int Mavlink::start_helper(int argc, char *argv[])
int
Mavlink::start(int argc, char *argv[])
{
// Wait for the instance count to go up one
// before returning to the shell
int ic = Mavlink::instance_count();
// Instantiate thread
char buf[24];
sprintf(buf, "mavlink_if%d", Mavlink::instance_count());
sprintf(buf, "mavlink_if%d", ic);
// This is where the control flow splits
// between the starting task and the spawned
// task - start_helper() only returns
// when the started task exits.
task_spawn_cmd(buf,
SCHED_DEFAULT,
SCHED_PRIORITY_DEFAULT,
@ -1935,6 +1973,26 @@ Mavlink::start(int argc, char *argv[])
(main_t)&Mavlink::start_helper,
(const char **)argv);
// Ensure that this shell command
// does not return before the instance
// is fully initialized. As this is also
// the only path to create a new instance,
// this is effectively a lock on concurrent
// instance starting. XXX do a real lock.
// Sleep 500 us between each attempt
const unsigned sleeptime = 500;
// Wait 100 ms max for the startup.
const unsigned limit = 100 * 1000 / sleeptime;
unsigned count = 0;
while (ic == Mavlink::instance_count() && count < limit) {
::usleep(sleeptime);
count++;
}
return OK;
}
@ -1960,20 +2018,26 @@ Mavlink::stream(int argc, char *argv[])
bool err_flag = false;
int i = 0;
while (i < argc) {
if (0 == strcmp(argv[i], "-r") && i < argc - 1 ) {
rate = strtod(argv[i+1], nullptr);
if (0 == strcmp(argv[i], "-r") && i < argc - 1) {
rate = strtod(argv[i + 1], nullptr);
if (rate < 0.0f) {
err_flag = true;
}
i++;
} else if (0 == strcmp(argv[i], "-d") && i < argc - 1 ) {
device_name = argv[i+1];
} else if (0 == strcmp(argv[i], "-d") && i < argc - 1) {
device_name = argv[i + 1];
i++;
} else if (0 == strcmp(argv[i], "-s") && i < argc - 1 ) {
stream_name = argv[i+1];
} else if (0 == strcmp(argv[i], "-s") && i < argc - 1) {
stream_name = argv[i + 1];
i++;
} else {
err_flag = true;
}
@ -1988,7 +2052,10 @@ Mavlink::stream(int argc, char *argv[])
inst->configure_stream_threadsafe(stream_name, rate);
} else {
errx(1, "mavlink for device %s is not running", device_name);
// If the link is not running we should complain, but not fall over
// because this is so easy to get wrong and not fatal. Warning is sufficient.
errx(0, "mavlink for device %s is not running", device_name);
}
} else {

3
src/modules/mavlink/mavlink_main.h
View File

@ -146,7 +146,8 @@ public:
enum MAVLINK_MODE {
MAVLINK_MODE_NORMAL = 0,
MAVLINK_MODE_CUSTOM
MAVLINK_MODE_CUSTOM,
MAVLINK_MODE_CAMERA
};
void set_mode(enum MAVLINK_MODE);

247
src/modules/mavlink/mavlink_messages.cpp
View File

@ -262,7 +262,6 @@ protected:
void send(const hrt_abstime t)
{
if (status_sub->update(t)) {
mavlink_msg_sys_status_send(_channel,
status->onboard_control_sensors_present,
status->onboard_control_sensors_enabled,
@ -318,7 +317,6 @@ protected:
void send(const hrt_abstime t)
{
if (sensor_sub->update(t)) {
uint16_t fields_updated = 0;
if (accel_timestamp != sensor->accelerometer_timestamp) {
@ -385,7 +383,6 @@ protected:
void send(const hrt_abstime t)
{
if (att_sub->update(t)) {
mavlink_msg_attitude_send(_channel,
att->timestamp / 1000,
att->roll, att->pitch, att->yaw,
@ -422,7 +419,6 @@ protected:
void send(const hrt_abstime t)
{
if (att_sub->update(t)) {
mavlink_msg_attitude_quaternion_send(_channel,
att->timestamp / 1000,
att->q[0],
@ -494,7 +490,6 @@ protected:
updated |= airspeed_sub->update(t);
if (updated) {
float groundspeed = sqrtf(pos->vel_n * pos->vel_n + pos->vel_e * pos->vel_e);
uint16_t heading = _wrap_2pi(att->yaw) * M_RAD_TO_DEG_F;
float throttle = armed->armed ? act->control[3] * 100.0f : 0.0f;
@ -538,7 +533,6 @@ protected:
void send(const hrt_abstime t)
{
if (gps_sub->update(t)) {
mavlink_msg_gps_raw_int_send(_channel,
gps->timestamp_position,
gps->fix_type,
@ -592,15 +586,15 @@ protected:
if (updated) {
mavlink_msg_global_position_int_send(_channel,
pos->timestamp / 1000,
pos->lat * 1e7,
pos->lon * 1e7,
pos->alt * 1000.0f,
(pos->alt - home->alt) * 1000.0f,
pos->vel_n * 100.0f,
pos->vel_e * 100.0f,
pos->vel_d * 100.0f,
_wrap_2pi(pos->yaw) * M_RAD_TO_DEG_F * 100.0f);
pos->timestamp / 1000,
pos->lat * 1e7,
pos->lon * 1e7,
pos->alt * 1000.0f,
(pos->alt - home->alt) * 1000.0f,
pos->vel_n * 100.0f,
pos->vel_e * 100.0f,
pos->vel_d * 100.0f,
_wrap_2pi(pos->yaw) * M_RAD_TO_DEG_F * 100.0f);
}
}
};
@ -633,7 +627,6 @@ protected:
void send(const hrt_abstime t)
{
if (pos_sub->update(t)) {
mavlink_msg_local_position_ned_send(_channel,
pos->timestamp / 1000,
pos->x,
@ -730,7 +723,6 @@ protected:
void send(const hrt_abstime t)
{
if (act_sub->update(t)) {
mavlink_msg_servo_output_raw_send(_channel,
act->timestamp / 1000,
_n,
@ -790,7 +782,6 @@ protected:
updated |= act_sub->update(t);
if (updated) {
/* translate the current syste state to mavlink state and mode */
uint8_t mavlink_state;
uint8_t mavlink_base_mode;
@ -870,7 +861,6 @@ protected:
void send(const hrt_abstime t)
{
if (pos_sp_triplet_sub->update(t)) {
mavlink_msg_global_position_setpoint_int_send(_channel,
MAV_FRAME_GLOBAL,
(int32_t)(pos_sp_triplet->current.lat * 1e7),
@ -908,14 +898,14 @@ protected:
void send(const hrt_abstime t)
{
pos_sp_sub->update(t);
mavlink_msg_local_position_setpoint_send(_channel,
MAV_FRAME_LOCAL_NED,
pos_sp->x,
pos_sp->y,
pos_sp->z,
pos_sp->yaw);
if (pos_sp_sub->update(t)) {
mavlink_msg_local_position_setpoint_send(_channel,
MAV_FRAME_LOCAL_NED,
pos_sp->x,
pos_sp->y,
pos_sp->z,
pos_sp->yaw);
}
}
};
@ -947,7 +937,6 @@ protected:
void send(const hrt_abstime t)
{
if (att_sp_sub->update(t)) {
mavlink_msg_roll_pitch_yaw_thrust_setpoint_send(_channel,
att_sp->timestamp / 1000,
att_sp->roll_body,
@ -986,7 +975,6 @@ protected:
void send(const hrt_abstime t)
{
if (att_rates_sp_sub->update(t)) {
mavlink_msg_roll_pitch_yaw_rates_thrust_setpoint_send(_channel,
att_rates_sp->timestamp / 1000,
att_rates_sp->roll,
@ -1025,7 +1013,6 @@ protected:
void send(const hrt_abstime t)
{
if (rc_sub->update(t)) {
const unsigned port_width = 8;
for (unsigned i = 0; (i * port_width) < rc->channel_count; i++) {
@ -1075,7 +1062,6 @@ protected:
void send(const hrt_abstime t)
{
if (manual_sub->update(t)) {
mavlink_msg_manual_control_send(_channel,
mavlink_system.sysid,
manual->roll * 1000,
@ -1115,7 +1101,6 @@ protected:
void send(const hrt_abstime t)
{
if (flow_sub->update(t)) {
mavlink_msg_optical_flow_send(_channel,
flow->timestamp,
flow->sensor_id,
@ -1127,6 +1112,134 @@ protected:
}
};
class MavlinkStreamAttitudeControls : public MavlinkStream
{
public:
const char *get_name()
{
return "ATTITUDE_CONTROLS";
}
MavlinkStream *new_instance()
{
return new MavlinkStreamAttitudeControls();
}
private:
MavlinkOrbSubscription *att_ctrl_sub;
struct actuator_controls_s *att_ctrl;
protected:
void subscribe(Mavlink *mavlink)
{
att_ctrl_sub = mavlink->add_orb_subscription(ORB_ID_VEHICLE_ATTITUDE_CONTROLS);
att_ctrl = (struct actuator_controls_s *)att_ctrl_sub->get_data();
}
void send(const hrt_abstime t)
{
if (att_ctrl_sub->update(t)) {
/* send, add spaces so that string buffer is at least 10 chars long */
mavlink_msg_named_value_float_send(_channel,
att_ctrl->timestamp / 1000,
"rll ctrl ",
att_ctrl->control[0]);
mavlink_msg_named_value_float_send(_channel,
att_ctrl->timestamp / 1000,
"ptch ctrl ",
att_ctrl->control[1]);
mavlink_msg_named_value_float_send(_channel,
att_ctrl->timestamp / 1000,
"yaw ctrl ",
att_ctrl->control[2]);
mavlink_msg_named_value_float_send(_channel,
att_ctrl->timestamp / 1000,
"thr ctrl ",
att_ctrl->control[3]);
}
}
};
class MavlinkStreamNamedValueFloat : public MavlinkStream
{
public:
const char *get_name()
{
return "NAMED_VALUE_FLOAT";
}
MavlinkStream *new_instance()
{
return new MavlinkStreamNamedValueFloat();
}
private:
MavlinkOrbSubscription *debug_sub;
struct debug_key_value_s *debug;
protected:
void subscribe(Mavlink *mavlink)
{
debug_sub = mavlink->add_orb_subscription(ORB_ID(debug_key_value));
debug = (struct debug_key_value_s *)debug_sub->get_data();
}
void send(const hrt_abstime t)
{
if (debug_sub->update(t)) {
/* enforce null termination */
debug->key[sizeof(debug->key) - 1] = '\0';
mavlink_msg_named_value_float_send(_channel,
debug->timestamp_ms,
debug->key,
debug->value);
}
}
};
class MavlinkStreamCameraCapture : public MavlinkStream
{
public:
const char *get_name()
{
return "CAMERA_CAPTURE";
}
MavlinkStream *new_instance()
{
return new MavlinkStreamCameraCapture();
}
private:
MavlinkOrbSubscription *status_sub;
struct vehicle_status_s *status;
protected:
void subscribe(Mavlink *mavlink)
{
status_sub = mavlink->add_orb_subscription(ORB_ID(vehicle_status));
status = (struct vehicle_status_s *)status_sub->get_data();
}
void send(const hrt_abstime t)
{
(void)status_sub->update(t);
if (status->arming_state == ARMING_STATE_ARMED
|| status->arming_state == ARMING_STATE_ARMED_ERROR) {
/* send camera capture on */
mavlink_msg_command_long_send(_channel, 42, 30, MAV_CMD_DO_CONTROL_VIDEO, 0, 0, 0, 0, 1, 0, 0, 0);
} else {
/* send camera capture off */
mavlink_msg_command_long_send(_channel, 42, 30, MAV_CMD_DO_CONTROL_VIDEO, 0, 0, 0, 0, 0, 0, 0, 0);
}
}
};
MavlinkStream *streams_list[] = {
new MavlinkStreamHeartbeat(),
@ -1151,72 +1264,8 @@ MavlinkStream *streams_list[] = {
new MavlinkStreamRCChannelsRaw(),
new MavlinkStreamManualControl(),
new MavlinkStreamOpticalFlow(),
new MavlinkStreamAttitudeControls(),
new MavlinkStreamNamedValueFloat(),
new MavlinkStreamCameraCapture(),
nullptr
};
//
//
//
//
//
//
//void
//MavlinkOrbListener::l_vehicle_attitude_controls(const struct listener *l)
//{
// orb_copy(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, l->mavlink->get_subs()->actuators_sub, &l->listener->actuators_0);
//
// if (l->mavlink->get_mode() == Mavlink::MODE_OFFBOARD) {
// /* send, add spaces so that string buffer is at least 10 chars long */
// mavlink_msg_named_value_float_send(l->mavlink->get_chan(),
// l->listener->last_sensor_timestamp / 1000,
// "ctrl0 ",
// l->listener->actuators_0.control[0]);
// mavlink_msg_named_value_float_send(l->mavlink->get_chan(),
// l->listener->last_sensor_timestamp / 1000,
// "ctrl1 ",
// l->listener->actuators_0.control[1]);
// mavlink_msg_named_value_float_send(l->mavlink->get_chan(),
// l->listener->last_sensor_timestamp / 1000,
// "ctrl2 ",
// l->listener->actuators_0.control[2]);
// mavlink_msg_named_value_float_send(l->mavlink->get_chan(),
// l->listener->last_sensor_timestamp / 1000,
// "ctrl3 ",
// l->listener->actuators_0.control[3]);
// }
//}
//
//void
//MavlinkOrbListener::l_debug_key_value(const struct listener *l)
//{
// struct debug_key_value_s debug;
//
// orb_copy(ORB_ID(debug_key_value), l->mavlink->get_subs()->debug_key_value, &debug);
//
// /* Enforce null termination */
// debug.key[sizeof(debug.key) - 1] = '\0';
//
// mavlink_msg_named_value_float_send(l->mavlink->get_chan(),
// l->listener->last_sensor_timestamp / 1000,
// debug.key,
// debug.value);
//}
//
//void
//MavlinkOrbListener::l_nav_cap(const struct listener *l)
//{
//
// orb_copy(ORB_ID(navigation_capabilities), l->mavlink->get_subs()->navigation_capabilities_sub, &l->listener->nav_cap);
//
// mavlink_msg_named_value_float_send(l->mavlink->get_chan(),
// hrt_absolute_time() / 1000,
// "turn dist",
// l->listener->nav_cap.turn_distance);
//
//}

24
src/modules/position_estimator_inav/inertial_filter.c
View File

@ -5,24 +5,30 @@
* Author: Anton Babushkin <rk3dov@gmail.com>
*/
#include <math.h>
#include "inertial_filter.h"
void inertial_filter_predict(float dt, float x[3])
{
x[0] += x[1] * dt + x[2] * dt * dt / 2.0f;
x[1] += x[2] * dt;
if (isfinite(dt)) {
x[0] += x[1] * dt + x[2] * dt * dt / 2.0f;
x[1] += x[2] * dt;
}
}
void inertial_filter_correct(float e, float dt, float x[3], int i, float w)
{
float ewdt = e * w * dt;
x[i] += ewdt;
if (isfinite(e) && isfinite(w) && isfinite(dt)) {
float ewdt = e * w * dt;
x[i] += ewdt;
if (i == 0) {
x[1] += w * ewdt;
x[2] += w * w * ewdt / 3.0;
if (i == 0) {
x[1] += w * ewdt;
x[2] += w * w * ewdt / 3.0;
} else if (i == 1) {
x[2] += w * ewdt;
} else if (i == 1) {
x[2] += w * ewdt;
}
}
}

1
src/modules/sensors/sensors.cpp
View File

@ -1032,6 +1032,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);

45
src/modules/controllib/uorb/UOrbPublication.cpp → src/modules/uORB/Publication.cpp
View File

@ -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>;
}

37
src/modules/controllib/uorb/UOrbPublication.hpp → src/modules/uORB/Publication.hpp
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

103
src/modules/uORB/Subscription.cpp Normal file
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

45
src/modules/controllib/uorb/UOrbSubscription.hpp → src/modules/uORB/Subscription.hpp
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

4
src/modules/uORB/module.mk
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

3
src/modules/uORB/objects_common.cpp
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);

37
src/modules/controllib/uorb/UOrbSubscription.cpp → src/modules/uORB/topics/encoders.h
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