forked from Archive/PX4-Autopilot
Merge remote-tracking branch 'px4/beta_mavlink2' into beta_mavlink2_camera
Conflicts: src/modules/mavlink/mavlink_messages.cpp
This commit is contained in:
commit
3d47ba14f3
|
@ -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
|
|
@ -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_CONTROL -r 30
|
||||
|
||||
# Exit shell to make it available to MAVLink
|
||||
exit
|
||||
|
|
|
@ -108,7 +108,6 @@ then
|
|||
set HIL no
|
||||
set VEHICLE_TYPE none
|
||||
set MIXER none
|
||||
set USE_IO yes
|
||||
set OUTPUT_MODE none
|
||||
set PWM_OUTPUTS none
|
||||
set PWM_RATE none
|
||||
|
@ -131,6 +130,16 @@ then
|
|||
else
|
||||
set DO_AUTOCONFIG no
|
||||
fi
|
||||
|
||||
#
|
||||
# Set USE_IO flag
|
||||
#
|
||||
if param compare SYS_USE_IO 1
|
||||
then
|
||||
set USE_IO yes
|
||||
else
|
||||
set USE_IO no
|
||||
fi
|
||||
|
||||
#
|
||||
# Set parameters and env variables for selected AUTOSTART
|
||||
|
|
|
@ -132,7 +132,6 @@ ETSAirspeed::measure()
|
|||
|
||||
if (OK != ret) {
|
||||
perf_count(_comms_errors);
|
||||
log("i2c::transfer returned %d", ret);
|
||||
}
|
||||
|
||||
return ret;
|
||||
|
@ -308,7 +307,7 @@ fail:
|
|||
g_dev = nullptr;
|
||||
}
|
||||
|
||||
errx(1, "driver start failed");
|
||||
errx(1, "no ETS airspeed sensor connected");
|
||||
}
|
||||
|
||||
/**
|
||||
|
|
|
@ -52,7 +52,7 @@
|
|||
#include <arch/board/board.h>
|
||||
#include <mavlink/mavlink_log.h>
|
||||
|
||||
#include <controllib/uorb/UOrbPublication.hpp>
|
||||
#include <uORB/Publication.hpp>
|
||||
#include <uORB/topics/debug_key_value.h>
|
||||
#include <drivers/drv_hrt.h>
|
||||
|
||||
|
@ -587,7 +587,7 @@ int md25Sine(const char *deviceName, uint8_t bus, uint8_t address, float amplitu
|
|||
float prev_revolution = md25.getRevolutions1();
|
||||
|
||||
// debug publication
|
||||
control::UOrbPublication<debug_key_value_s> debug_msg(NULL,
|
||||
uORB::Publication<debug_key_value_s> debug_msg(NULL,
|
||||
ORB_ID(debug_key_value));
|
||||
|
||||
// sine wave for motor 1
|
||||
|
|
|
@ -46,7 +46,7 @@
|
|||
|
||||
#include <poll.h>
|
||||
#include <stdio.h>
|
||||
#include <controllib/uorb/UOrbSubscription.hpp>
|
||||
#include <uORB/Subscription.hpp>
|
||||
#include <uORB/topics/actuator_controls.h>
|
||||
#include <drivers/device/i2c.h>
|
||||
|
||||
|
@ -270,7 +270,7 @@ private:
|
|||
struct pollfd _controlPoll;
|
||||
|
||||
/** actuator controls subscription */
|
||||
control::UOrbSubscription<actuator_controls_s> _actuators;
|
||||
uORB::Subscription<actuator_controls_s> _actuators;
|
||||
|
||||
// local copy of data from i2c device
|
||||
uint8_t _version;
|
||||
|
|
|
@ -104,7 +104,7 @@
|
|||
class MEASAirspeed : public Airspeed
|
||||
{
|
||||
public:
|
||||
MEASAirspeed(int bus, int address = I2C_ADDRESS_MS4525DO, const char* path = PATH_MS4525);
|
||||
MEASAirspeed(int bus, int address = I2C_ADDRESS_MS4525DO, const char *path = PATH_MS4525);
|
||||
|
||||
protected:
|
||||
|
||||
|
@ -123,7 +123,7 @@ protected:
|
|||
*/
|
||||
extern "C" __EXPORT int meas_airspeed_main(int argc, char *argv[]);
|
||||
|
||||
MEASAirspeed::MEASAirspeed(int bus, int address, const char* path) : Airspeed(bus, address,
|
||||
MEASAirspeed::MEASAirspeed(int bus, int address, const char *path) : Airspeed(bus, address,
|
||||
CONVERSION_INTERVAL, path)
|
||||
{
|
||||
|
||||
|
@ -161,23 +161,25 @@ MEASAirspeed::collect()
|
|||
ret = transfer(nullptr, 0, &val[0], 4);
|
||||
|
||||
if (ret < 0) {
|
||||
perf_count(_comms_errors);
|
||||
perf_end(_sample_perf);
|
||||
perf_count(_comms_errors);
|
||||
perf_end(_sample_perf);
|
||||
return ret;
|
||||
}
|
||||
|
||||
uint8_t status = val[0] & 0xC0;
|
||||
uint8_t status = (val[0] & 0xC0) >> 6;
|
||||
|
||||
if (status == 2) {
|
||||
log("err: stale data");
|
||||
perf_count(_comms_errors);
|
||||
perf_end(_sample_perf);
|
||||
return ret;
|
||||
} else if (status == 3) {
|
||||
log("err: fault");
|
||||
perf_count(_comms_errors);
|
||||
perf_end(_sample_perf);
|
||||
return ret;
|
||||
switch (status) {
|
||||
case 0:
|
||||
break;
|
||||
|
||||
case 1:
|
||||
/* fallthrough */
|
||||
case 2:
|
||||
/* fallthrough */
|
||||
case 3:
|
||||
perf_count(_comms_errors);
|
||||
perf_end(_sample_perf);
|
||||
return -EAGAIN;
|
||||
}
|
||||
|
||||
int16_t dp_raw = 0, dT_raw = 0;
|
||||
|
@ -193,19 +195,21 @@ MEASAirspeed::collect()
|
|||
*/
|
||||
const float P_min = -1.0f;
|
||||
const float P_max = 1.0f;
|
||||
float diff_press_pa = fabsf( ( ((float)dp_raw - 0.1f*16383.0f) * (P_max-P_min)/(0.8f*16383.0f) + P_min) * 6894.8f) - _diff_pres_offset;
|
||||
if (diff_press_pa < 0.0f)
|
||||
diff_press_pa = 0.0f;
|
||||
float diff_press_pa = fabsf((((float)dp_raw - 0.1f * 16383.0f) * (P_max - P_min) / (0.8f * 16383.0f) + P_min) * 6894.8f) - _diff_pres_offset;
|
||||
|
||||
if (diff_press_pa < 0.0f) {
|
||||
diff_press_pa = 0.0f;
|
||||
}
|
||||
|
||||
struct differential_pressure_s report;
|
||||
|
||||
/* track maximum differential pressure measured (so we can work out top speed). */
|
||||
if (diff_press_pa > _max_differential_pressure_pa) {
|
||||
_max_differential_pressure_pa = diff_press_pa;
|
||||
_max_differential_pressure_pa = diff_press_pa;
|
||||
}
|
||||
|
||||
report.timestamp = hrt_absolute_time();
|
||||
report.error_count = perf_event_count(_comms_errors);
|
||||
report.error_count = perf_event_count(_comms_errors);
|
||||
report.temperature = temperature;
|
||||
report.differential_pressure_pa = diff_press_pa;
|
||||
report.voltage = 0;
|
||||
|
@ -261,8 +265,9 @@ MEASAirspeed::cycle()
|
|||
}
|
||||
|
||||
/* measurement phase */
|
||||
if (OK != measure())
|
||||
if (OK != measure()) {
|
||||
log("measure error");
|
||||
}
|
||||
|
||||
/* next phase is collection */
|
||||
_collect_phase = true;
|
||||
|
@ -303,15 +308,17 @@ start(int i2c_bus)
|
|||
{
|
||||
int fd;
|
||||
|
||||
if (g_dev != nullptr)
|
||||
if (g_dev != nullptr) {
|
||||
errx(1, "already started");
|
||||
}
|
||||
|
||||
/* create the driver, try the MS4525DO first */
|
||||
g_dev = new MEASAirspeed(i2c_bus, I2C_ADDRESS_MS4525DO, PATH_MS4525);
|
||||
|
||||
/* check if the MS4525DO was instantiated */
|
||||
if (g_dev == nullptr)
|
||||
if (g_dev == nullptr) {
|
||||
goto fail;
|
||||
}
|
||||
|
||||
/* try the MS5525DSO next if init fails */
|
||||
if (OK != g_dev->Airspeed::init()) {
|
||||
|
@ -319,22 +326,26 @@ start(int i2c_bus)
|
|||
g_dev = new MEASAirspeed(i2c_bus, I2C_ADDRESS_MS5525DSO, PATH_MS5525);
|
||||
|
||||
/* check if the MS5525DSO was instantiated */
|
||||
if (g_dev == nullptr)
|
||||
if (g_dev == nullptr) {
|
||||
goto fail;
|
||||
}
|
||||
|
||||
/* both versions failed if the init for the MS5525DSO fails, give up */
|
||||
if (OK != g_dev->Airspeed::init())
|
||||
if (OK != g_dev->Airspeed::init()) {
|
||||
goto fail;
|
||||
}
|
||||
}
|
||||
|
||||
/* set the poll rate to default, starts automatic data collection */
|
||||
fd = open(AIRSPEED_DEVICE_PATH, O_RDONLY);
|
||||
|
||||
if (fd < 0)
|
||||
if (fd < 0) {
|
||||
goto fail;
|
||||
}
|
||||
|
||||
if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0)
|
||||
if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0) {
|
||||
goto fail;
|
||||
}
|
||||
|
||||
exit(0);
|
||||
|
||||
|
@ -345,7 +356,7 @@ fail:
|
|||
g_dev = nullptr;
|
||||
}
|
||||
|
||||
errx(1, "driver start failed");
|
||||
errx(1, "no MS4525 airspeed sensor connected");
|
||||
}
|
||||
|
||||
/**
|
||||
|
@ -379,21 +390,24 @@ test()
|
|||
|
||||
int fd = open(AIRSPEED_DEVICE_PATH, O_RDONLY);
|
||||
|
||||
if (fd < 0)
|
||||
if (fd < 0) {
|
||||
err(1, "%s open failed (try 'meas_airspeed start' if the driver is not running", AIRSPEED_DEVICE_PATH);
|
||||
}
|
||||
|
||||
/* do a simple demand read */
|
||||
sz = read(fd, &report, sizeof(report));
|
||||
|
||||
if (sz != sizeof(report))
|
||||
if (sz != sizeof(report)) {
|
||||
err(1, "immediate read failed");
|
||||
}
|
||||
|
||||
warnx("single read");
|
||||
warnx("diff pressure: %8.4f pa", (double)report.differential_pressure_pa);
|
||||
|
||||
/* start the sensor polling at 2Hz */
|
||||
if (OK != ioctl(fd, SENSORIOCSPOLLRATE, 2))
|
||||
if (OK != ioctl(fd, SENSORIOCSPOLLRATE, 2)) {
|
||||
errx(1, "failed to set 2Hz poll rate");
|
||||
}
|
||||
|
||||
/* read the sensor 5x and report each value */
|
||||
for (unsigned i = 0; i < 5; i++) {
|
||||
|
@ -404,14 +418,16 @@ test()
|
|||
fds.events = POLLIN;
|
||||
ret = poll(&fds, 1, 2000);
|
||||
|
||||
if (ret != 1)
|
||||
if (ret != 1) {
|
||||
errx(1, "timed out waiting for sensor data");
|
||||
}
|
||||
|
||||
/* now go get it */
|
||||
sz = read(fd, &report, sizeof(report));
|
||||
|
||||
if (sz != sizeof(report))
|
||||
if (sz != sizeof(report)) {
|
||||
err(1, "periodic read failed");
|
||||
}
|
||||
|
||||
warnx("periodic read %u", i);
|
||||
warnx("diff pressure: %8.4f pa", (double)report.differential_pressure_pa);
|
||||
|
@ -419,8 +435,9 @@ test()
|
|||
}
|
||||
|
||||
/* reset the sensor polling to its default rate */
|
||||
if (OK != ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT))
|
||||
if (OK != ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT)) {
|
||||
errx(1, "failed to set default rate");
|
||||
}
|
||||
|
||||
errx(0, "PASS");
|
||||
}
|
||||
|
@ -433,14 +450,17 @@ reset()
|
|||
{
|
||||
int fd = open(AIRSPEED_DEVICE_PATH, O_RDONLY);
|
||||
|
||||
if (fd < 0)
|
||||
if (fd < 0) {
|
||||
err(1, "failed ");
|
||||
}
|
||||
|
||||
if (ioctl(fd, SENSORIOCRESET, 0) < 0)
|
||||
if (ioctl(fd, SENSORIOCRESET, 0) < 0) {
|
||||
err(1, "driver reset failed");
|
||||
}
|
||||
|
||||
if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0)
|
||||
if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0) {
|
||||
err(1, "driver poll restart failed");
|
||||
}
|
||||
|
||||
exit(0);
|
||||
}
|
||||
|
@ -451,8 +471,9 @@ reset()
|
|||
void
|
||||
info()
|
||||
{
|
||||
if (g_dev == nullptr)
|
||||
if (g_dev == nullptr) {
|
||||
errx(1, "driver not running");
|
||||
}
|
||||
|
||||
printf("state @ %p\n", g_dev);
|
||||
g_dev->print_info();
|
||||
|
@ -491,32 +512,37 @@ meas_airspeed_main(int argc, char *argv[])
|
|||
/*
|
||||
* Start/load the driver.
|
||||
*/
|
||||
if (!strcmp(argv[1], "start"))
|
||||
if (!strcmp(argv[1], "start")) {
|
||||
meas_airspeed::start(i2c_bus);
|
||||
}
|
||||
|
||||
/*
|
||||
* Stop the driver
|
||||
*/
|
||||
if (!strcmp(argv[1], "stop"))
|
||||
if (!strcmp(argv[1], "stop")) {
|
||||
meas_airspeed::stop();
|
||||
}
|
||||
|
||||
/*
|
||||
* Test the driver/device.
|
||||
*/
|
||||
if (!strcmp(argv[1], "test"))
|
||||
if (!strcmp(argv[1], "test")) {
|
||||
meas_airspeed::test();
|
||||
}
|
||||
|
||||
/*
|
||||
* Reset the driver.
|
||||
*/
|
||||
if (!strcmp(argv[1], "reset"))
|
||||
if (!strcmp(argv[1], "reset")) {
|
||||
meas_airspeed::reset();
|
||||
}
|
||||
|
||||
/*
|
||||
* Print driver information.
|
||||
*/
|
||||
if (!strcmp(argv[1], "info") || !strcmp(argv[1], "status"))
|
||||
if (!strcmp(argv[1], "info") || !strcmp(argv[1], "status")) {
|
||||
meas_airspeed::info();
|
||||
}
|
||||
|
||||
meas_airspeed_usage();
|
||||
exit(0);
|
||||
|
|
|
@ -74,7 +74,7 @@
|
|||
|
||||
/* Configuration Constants */
|
||||
#define PX4FLOW_BUS PX4_I2C_BUS_EXPANSION
|
||||
#define I2C_FLOW_ADDRESS 0x45 //* 7-bit address. 8-bit address is 0x8A
|
||||
#define I2C_FLOW_ADDRESS 0x42 //* 7-bit address. 8-bit address is 0x84
|
||||
//range 0x42 - 0x49
|
||||
|
||||
/* PX4FLOW Registers addresses */
|
||||
|
|
|
@ -53,7 +53,7 @@
|
|||
#include <arch/board/board.h>
|
||||
#include <mavlink/mavlink_log.h>
|
||||
|
||||
#include <controllib/uorb/UOrbPublication.hpp>
|
||||
#include <uORB/Publication.hpp>
|
||||
#include <uORB/topics/debug_key_value.h>
|
||||
#include <drivers/drv_hrt.h>
|
||||
|
||||
|
|
|
@ -45,7 +45,7 @@
|
|||
|
||||
#include <poll.h>
|
||||
#include <stdio.h>
|
||||
#include <controllib/uorb/UOrbSubscription.hpp>
|
||||
#include <uORB/Subscription.hpp>
|
||||
#include <uORB/topics/actuator_controls.h>
|
||||
#include <drivers/device/i2c.h>
|
||||
|
||||
|
@ -169,7 +169,7 @@ private:
|
|||
struct pollfd _controlPoll;
|
||||
|
||||
/** actuator controls subscription */
|
||||
control::UOrbSubscription<actuator_controls_s> _actuators;
|
||||
uORB::Subscription<actuator_controls_s> _actuators;
|
||||
|
||||
// private data
|
||||
float _motor1Position;
|
||||
|
|
|
@ -32,9 +32,9 @@
|
|||
****************************************************************************/
|
||||
|
||||
/**
|
||||
* @file Node.h
|
||||
* @file List.hpp
|
||||
*
|
||||
* A node of a linked list.
|
||||
* A linked list.
|
||||
*/
|
||||
|
||||
#pragma once
|
||||
|
@ -43,7 +43,7 @@ template<class T>
|
|||
class __EXPORT ListNode
|
||||
{
|
||||
public:
|
||||
ListNode() : _sibling(NULL) {
|
||||
ListNode() : _sibling(nullptr) {
|
||||
}
|
||||
void setSibling(T sibling) { _sibling = sibling; }
|
||||
T getSibling() { return _sibling; }
|
|
@ -47,8 +47,8 @@
|
|||
#include <mathlib/mathlib.h>
|
||||
#include <controllib/blocks.hpp>
|
||||
#include <controllib/block/BlockParam.hpp>
|
||||
#include <controllib/uorb/UOrbSubscription.hpp>
|
||||
#include <controllib/uorb/UOrbPublication.hpp>
|
||||
#include <uORB/Subscription.hpp>
|
||||
#include <uORB/Publication.hpp>
|
||||
|
||||
#include <uORB/topics/vehicle_attitude.h>
|
||||
#include <uORB/topics/vehicle_global_position.h>
|
||||
|
@ -138,13 +138,13 @@ protected:
|
|||
math::Matrix<3,3> C_nb; /**< direction cosine matrix from body to nav frame */
|
||||
math::Quaternion q; /**< quaternion from body to nav frame */
|
||||
// subscriptions
|
||||
control::UOrbSubscription<sensor_combined_s> _sensors; /**< sensors sub. */
|
||||
control::UOrbSubscription<vehicle_gps_position_s> _gps; /**< gps sub. */
|
||||
control::UOrbSubscription<parameter_update_s> _param_update; /**< parameter update sub. */
|
||||
uORB::Subscription<sensor_combined_s> _sensors; /**< sensors sub. */
|
||||
uORB::Subscription<vehicle_gps_position_s> _gps; /**< gps sub. */
|
||||
uORB::Subscription<parameter_update_s> _param_update; /**< parameter update sub. */
|
||||
// publications
|
||||
control::UOrbPublication<vehicle_global_position_s> _pos; /**< position pub. */
|
||||
control::UOrbPublication<vehicle_local_position_s> _localPos; /**< local position pub. */
|
||||
control::UOrbPublication<vehicle_attitude_s> _att; /**< attitude pub. */
|
||||
uORB::Publication<vehicle_global_position_s> _pos; /**< position pub. */
|
||||
uORB::Publication<vehicle_local_position_s> _localPos; /**< local position pub. */
|
||||
uORB::Publication<vehicle_attitude_s> _att; /**< attitude pub. */
|
||||
// time stamps
|
||||
uint64_t _pubTimeStamp; /**< output data publication time stamp */
|
||||
uint64_t _predictTimeStamp; /**< prediction time stamp */
|
||||
|
|
|
@ -41,10 +41,11 @@
|
|||
#include <string.h>
|
||||
#include <stdio.h>
|
||||
|
||||
#include <uORB/Subscription.hpp>
|
||||
#include <uORB/Publication.hpp>
|
||||
|
||||
#include "Block.hpp"
|
||||
#include "BlockParam.hpp"
|
||||
#include "../uorb/UOrbSubscription.hpp"
|
||||
#include "../uorb/UOrbPublication.hpp"
|
||||
|
||||
namespace control
|
||||
{
|
||||
|
@ -100,7 +101,7 @@ void Block::updateParams()
|
|||
|
||||
void Block::updateSubscriptions()
|
||||
{
|
||||
UOrbSubscriptionBase *sub = getSubscriptions().getHead();
|
||||
uORB::SubscriptionBase *sub = getSubscriptions().getHead();
|
||||
int count = 0;
|
||||
|
||||
while (sub != NULL) {
|
||||
|
@ -118,7 +119,7 @@ void Block::updateSubscriptions()
|
|||
|
||||
void Block::updatePublications()
|
||||
{
|
||||
UOrbPublicationBase *pub = getPublications().getHead();
|
||||
uORB::PublicationBase *pub = getPublications().getHead();
|
||||
int count = 0;
|
||||
|
||||
while (pub != NULL) {
|
||||
|
|
|
@ -42,7 +42,13 @@
|
|||
#include <stdint.h>
|
||||
#include <inttypes.h>
|
||||
|
||||
#include "List.hpp"
|
||||
#include <containers/List.hpp>
|
||||
|
||||
// forward declaration
|
||||
namespace uORB {
|
||||
class SubscriptionBase;
|
||||
class PublicationBase;
|
||||
}
|
||||
|
||||
namespace control
|
||||
{
|
||||
|
@ -55,8 +61,6 @@ static const uint8_t blockNameLengthMax = 80;
|
|||
|
||||
// forward declaration
|
||||
class BlockParamBase;
|
||||
class UOrbSubscriptionBase;
|
||||
class UOrbPublicationBase;
|
||||
class SuperBlock;
|
||||
|
||||
/**
|
||||
|
@ -79,15 +83,15 @@ public:
|
|||
protected:
|
||||
// accessors
|
||||
SuperBlock *getParent() { return _parent; }
|
||||
List<UOrbSubscriptionBase *> & getSubscriptions() { return _subscriptions; }
|
||||
List<UOrbPublicationBase *> & getPublications() { return _publications; }
|
||||
List<uORB::SubscriptionBase *> & getSubscriptions() { return _subscriptions; }
|
||||
List<uORB::PublicationBase *> & getPublications() { return _publications; }
|
||||
List<BlockParamBase *> & getParams() { return _params; }
|
||||
// attributes
|
||||
const char *_name;
|
||||
SuperBlock *_parent;
|
||||
float _dt;
|
||||
List<UOrbSubscriptionBase *> _subscriptions;
|
||||
List<UOrbPublicationBase *> _publications;
|
||||
List<uORB::SubscriptionBase *> _subscriptions;
|
||||
List<uORB::PublicationBase *> _publications;
|
||||
List<BlockParamBase *> _params;
|
||||
};
|
||||
|
||||
|
|
|
@ -76,4 +76,29 @@ BlockParamBase::BlockParamBase(Block *parent, const char *name, bool parent_pref
|
|||
printf("error finding param: %s\n", fullname);
|
||||
};
|
||||
|
||||
template <class T>
|
||||
BlockParam<T>::BlockParam(Block *block, const char *name,
|
||||
bool parent_prefix) :
|
||||
BlockParamBase(block, name, parent_prefix),
|
||||
_val() {
|
||||
update();
|
||||
}
|
||||
|
||||
template <class T>
|
||||
T BlockParam<T>::get() { return _val; }
|
||||
|
||||
template <class T>
|
||||
void BlockParam<T>::set(T val) { _val = val; }
|
||||
|
||||
template <class T>
|
||||
void BlockParam<T>::update() {
|
||||
if (_handle != PARAM_INVALID) param_get(_handle, &_val);
|
||||
}
|
||||
|
||||
template <class T>
|
||||
BlockParam<T>::~BlockParam() {};
|
||||
|
||||
template class __EXPORT BlockParam<float>;
|
||||
template class __EXPORT BlockParam<int>;
|
||||
|
||||
} // namespace control
|
||||
|
|
|
@ -42,7 +42,7 @@
|
|||
#include <systemlib/param/param.h>
|
||||
|
||||
#include "Block.hpp"
|
||||
#include "List.hpp"
|
||||
#include <containers/List.hpp>
|
||||
|
||||
namespace control
|
||||
{
|
||||
|
@ -70,38 +70,21 @@ protected:
|
|||
* Parameters that are tied to blocks for updating and nameing.
|
||||
*/
|
||||
|
||||
class __EXPORT BlockParamFloat : public BlockParamBase
|
||||
template <class T>
|
||||
class BlockParam : public BlockParamBase
|
||||
{
|
||||
public:
|
||||
BlockParamFloat(Block *block, const char *name, bool parent_prefix=true) :
|
||||
BlockParamBase(block, name, parent_prefix),
|
||||
_val() {
|
||||
update();
|
||||
}
|
||||
float get() { return _val; }
|
||||
void set(float val) { _val = val; }
|
||||
void update() {
|
||||
if (_handle != PARAM_INVALID) param_get(_handle, &_val);
|
||||
}
|
||||
BlockParam(Block *block, const char *name,
|
||||
bool parent_prefix=true);
|
||||
T get();
|
||||
void set(T val);
|
||||
void update();
|
||||
virtual ~BlockParam();
|
||||
protected:
|
||||
float _val;
|
||||
T _val;
|
||||
};
|
||||
|
||||
class __EXPORT BlockParamInt : public BlockParamBase
|
||||
{
|
||||
public:
|
||||
BlockParamInt(Block *block, const char *name, bool parent_prefix=true) :
|
||||
BlockParamBase(block, name, parent_prefix),
|
||||
_val() {
|
||||
update();
|
||||
}
|
||||
int get() { return _val; }
|
||||
void set(int val) { _val = val; }
|
||||
void update() {
|
||||
if (_handle != PARAM_INVALID) param_get(_handle, &_val);
|
||||
}
|
||||
protected:
|
||||
int _val;
|
||||
};
|
||||
typedef BlockParam<float> BlockParamFloat;
|
||||
typedef BlockParam<int> BlockParamInt;
|
||||
|
||||
} // namespace control
|
||||
|
|
|
@ -37,7 +37,5 @@
|
|||
SRCS = test_params.c \
|
||||
block/Block.cpp \
|
||||
block/BlockParam.cpp \
|
||||
uorb/UOrbPublication.cpp \
|
||||
uorb/UOrbSubscription.cpp \
|
||||
uorb/blocks.cpp \
|
||||
blocks.cpp
|
||||
|
|
|
@ -62,8 +62,8 @@ extern "C" {
|
|||
}
|
||||
|
||||
#include "../blocks.hpp"
|
||||
#include "UOrbSubscription.hpp"
|
||||
#include "UOrbPublication.hpp"
|
||||
#include <uORB/Subscription.hpp>
|
||||
#include <uORB/Publication.hpp>
|
||||
|
||||
namespace control
|
||||
{
|
||||
|
@ -94,16 +94,16 @@ class __EXPORT BlockUorbEnabledAutopilot : public SuperBlock
|
|||
{
|
||||
protected:
|
||||
// subscriptions
|
||||
UOrbSubscription<vehicle_attitude_s> _att;
|
||||
UOrbSubscription<vehicle_attitude_setpoint_s> _attCmd;
|
||||
UOrbSubscription<vehicle_rates_setpoint_s> _ratesCmd;
|
||||
UOrbSubscription<vehicle_global_position_s> _pos;
|
||||
UOrbSubscription<position_setpoint_triplet_s> _missionCmd;
|
||||
UOrbSubscription<manual_control_setpoint_s> _manual;
|
||||
UOrbSubscription<vehicle_status_s> _status;
|
||||
UOrbSubscription<parameter_update_s> _param_update;
|
||||
uORB::Subscription<vehicle_attitude_s> _att;
|
||||
uORB::Subscription<vehicle_attitude_setpoint_s> _attCmd;
|
||||
uORB::Subscription<vehicle_rates_setpoint_s> _ratesCmd;
|
||||
uORB::Subscription<vehicle_global_position_s> _pos;
|
||||
uORB::Subscription<position_setpoint_triplet_s> _missionCmd;
|
||||
uORB::Subscription<manual_control_setpoint_s> _manual;
|
||||
uORB::Subscription<vehicle_status_s> _status;
|
||||
uORB::Subscription<parameter_update_s> _param_update;
|
||||
// publications
|
||||
UOrbPublication<actuator_controls_s> _actuators;
|
||||
uORB::Publication<actuator_controls_s> _actuators;
|
||||
public:
|
||||
BlockUorbEnabledAutopilot(SuperBlock *parent, const char *name);
|
||||
virtual ~BlockUorbEnabledAutopilot();
|
||||
|
|
|
@ -175,14 +175,14 @@ void BlockMultiModeBacksideAutopilot::update()
|
|||
// the min/max velocity
|
||||
float v = _vLimit.update(sqrtf(
|
||||
_pos.vel_n * _pos.vel_n +
|
||||
_pos.vy * _pos.vy +
|
||||
_pos.vel_e * _pos.vel_e +
|
||||
_pos.vel_d * _pos.vel_d));
|
||||
|
||||
// limit velocity command between min/max velocity
|
||||
float vCmd = _vLimit.update(_vCmd.get());
|
||||
|
||||
// altitude hold
|
||||
float dThrottle = _h2Thr.update(_missionCmd.current.altitude - _pos.alt);
|
||||
float dThrottle = _h2Thr.update(_missionCmd.current.alt - _pos.alt);
|
||||
|
||||
// heading hold
|
||||
float psiError = _wrap_pi(_guide.getPsiCmd() - _att.yaw);
|
||||
|
@ -237,7 +237,7 @@ void BlockMultiModeBacksideAutopilot::update()
|
|||
// the min/max velocity
|
||||
float v = _vLimit.update(sqrtf(
|
||||
_pos.vel_n * _pos.vel_n +
|
||||
_pos.vy * _pos.vy +
|
||||
_pos.vel_e * _pos.vel_e +
|
||||
_pos.vel_d * _pos.vel_d));
|
||||
|
||||
// pitch channel -> rate of climb
|
||||
|
|
|
@ -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
|
||||
|
|
|
@ -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 */
|
||||
/**
|
||||
|
|
|
@ -254,7 +254,6 @@ Mavlink::Mavlink() :
|
|||
break;
|
||||
}
|
||||
|
||||
LL_APPEND(_mavlink_instances, this);
|
||||
}
|
||||
|
||||
Mavlink::~Mavlink()
|
||||
|
@ -589,7 +588,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");
|
||||
}
|
||||
}
|
||||
|
||||
|
@ -1769,6 +1768,7 @@ 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:
|
||||
|
@ -1791,6 +1791,9 @@ Mavlink::task_main(int argc, char *argv[])
|
|||
/* 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);
|
||||
|
@ -1937,10 +1940,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,
|
||||
|
@ -1948,6 +1959,25 @@ 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;
|
||||
}
|
||||
|
||||
|
@ -2001,7 +2031,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 {
|
||||
|
|
|
@ -230,6 +230,7 @@ protected:
|
|||
mavlink_base_mode,
|
||||
mavlink_custom_mode,
|
||||
mavlink_state);
|
||||
|
||||
}
|
||||
};
|
||||
|
||||
|
@ -1126,6 +1127,94 @@ 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:
|
||||
|
@ -1168,7 +1257,6 @@ protected:
|
|||
}
|
||||
};
|
||||
|
||||
|
||||
MavlinkStream *streams_list[] = {
|
||||
new MavlinkStreamHeartbeat(),
|
||||
new MavlinkStreamSysStatus(),
|
||||
|
@ -1192,73 +1280,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);
|
||||
//
|
||||
//}
|
||||
|
|
|
@ -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);
|
||||
|
|
|
@ -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);
|
||||
|
|
|
@ -32,8 +32,49 @@
|
|||
****************************************************************************/
|
||||
|
||||
/**
|
||||
* @file UOrbPublication.cpp
|
||||
* @file Publication.cpp
|
||||
*
|
||||
*/
|
||||
|
||||
#include "UOrbPublication.hpp"
|
||||
#include "Publication.hpp"
|
||||
#include "topics/vehicle_attitude.h"
|
||||
#include "topics/vehicle_local_position.h"
|
||||
#include "topics/vehicle_global_position.h"
|
||||
#include "topics/debug_key_value.h"
|
||||
#include "topics/actuator_controls.h"
|
||||
#include "topics/vehicle_global_velocity_setpoint.h"
|
||||
#include "topics/vehicle_attitude_setpoint.h"
|
||||
#include "topics/vehicle_rates_setpoint.h"
|
||||
#include "topics/actuator_outputs.h"
|
||||
#include "topics/encoders.h"
|
||||
|
||||
namespace uORB {
|
||||
|
||||
template<class T>
|
||||
Publication<T>::Publication(
|
||||
List<PublicationBase *> * list,
|
||||
const struct orb_metadata *meta) :
|
||||
T(), // initialize data structure to zero
|
||||
PublicationBase(list, meta) {
|
||||
}
|
||||
|
||||
template<class T>
|
||||
Publication<T>::~Publication() {}
|
||||
|
||||
template<class T>
|
||||
void * Publication<T>::getDataVoidPtr() {
|
||||
return (void *)(T *)(this);
|
||||
}
|
||||
|
||||
template class __EXPORT Publication<vehicle_attitude_s>;
|
||||
template class __EXPORT Publication<vehicle_local_position_s>;
|
||||
template class __EXPORT Publication<vehicle_global_position_s>;
|
||||
template class __EXPORT Publication<debug_key_value_s>;
|
||||
template class __EXPORT Publication<actuator_controls_s>;
|
||||
template class __EXPORT Publication<vehicle_global_velocity_setpoint_s>;
|
||||
template class __EXPORT Publication<vehicle_attitude_setpoint_s>;
|
||||
template class __EXPORT Publication<vehicle_rates_setpoint_s>;
|
||||
template class __EXPORT Publication<actuator_outputs_s>;
|
||||
template class __EXPORT Publication<encoders_s>;
|
||||
|
||||
}
|
|
@ -32,32 +32,29 @@
|
|||
****************************************************************************/
|
||||
|
||||
/**
|
||||
* @file UOrbPublication.h
|
||||
* @file Publication.h
|
||||
*
|
||||
*/
|
||||
|
||||
#pragma once
|
||||
|
||||
#include <uORB/uORB.h>
|
||||
#include "../block/Block.hpp"
|
||||
#include "../block/List.hpp"
|
||||
#include <containers/List.hpp>
|
||||
|
||||
|
||||
namespace control
|
||||
namespace uORB
|
||||
{
|
||||
|
||||
class Block;
|
||||
|
||||
/**
|
||||
* Base publication warapper class, used in list traversal
|
||||
* of various publications.
|
||||
*/
|
||||
class __EXPORT UOrbPublicationBase : public ListNode<control::UOrbPublicationBase *>
|
||||
class __EXPORT PublicationBase : public ListNode<uORB::PublicationBase *>
|
||||
{
|
||||
public:
|
||||
|
||||
UOrbPublicationBase(
|
||||
List<UOrbPublicationBase *> * list,
|
||||
PublicationBase(
|
||||
List<PublicationBase *> * list,
|
||||
const struct orb_metadata *meta) :
|
||||
_meta(meta),
|
||||
_handle(-1) {
|
||||
|
@ -71,7 +68,7 @@ public:
|
|||
}
|
||||
}
|
||||
virtual void *getDataVoidPtr() = 0;
|
||||
virtual ~UOrbPublicationBase() {
|
||||
virtual ~PublicationBase() {
|
||||
orb_unsubscribe(getHandle());
|
||||
}
|
||||
const struct orb_metadata *getMeta() { return _meta; }
|
||||
|
@ -83,12 +80,12 @@ protected:
|
|||
};
|
||||
|
||||
/**
|
||||
* UOrb Publication wrapper class
|
||||
* Publication wrapper class
|
||||
*/
|
||||
template<class T>
|
||||
class UOrbPublication :
|
||||
class Publication :
|
||||
public T, // this must be first!
|
||||
public UOrbPublicationBase
|
||||
public PublicationBase
|
||||
{
|
||||
public:
|
||||
/**
|
||||
|
@ -98,13 +95,9 @@ public:
|
|||
* @param meta The uORB metadata (usually from the ORB_ID() macro)
|
||||
* for the topic.
|
||||
*/
|
||||
UOrbPublication(
|
||||
List<UOrbPublicationBase *> * list,
|
||||
const struct orb_metadata *meta) :
|
||||
T(), // initialize data structure to zero
|
||||
UOrbPublicationBase(list, meta) {
|
||||
}
|
||||
virtual ~UOrbPublication() {}
|
||||
Publication(List<PublicationBase *> * list,
|
||||
const struct orb_metadata *meta);
|
||||
virtual ~Publication();
|
||||
/*
|
||||
* XXX
|
||||
* This function gets the T struct, assuming
|
||||
|
@ -112,7 +105,7 @@ public:
|
|||
* should use dynamic cast, but doesn't
|
||||
* seem to be available
|
||||
*/
|
||||
void *getDataVoidPtr() { return (void *)(T *)(this); }
|
||||
void *getDataVoidPtr();
|
||||
};
|
||||
|
||||
} // namespace control
|
||||
} // namespace uORB
|
|
@ -0,0 +1,103 @@
|
|||
/****************************************************************************
|
||||
*
|
||||
* Copyright (C) 2012 PX4 Development Team. All rights reserved.
|
||||
*
|
||||
* Redistribution and use in source and binary forms, with or without
|
||||
* modification, are permitted provided that the following conditions
|
||||
* are met:
|
||||
*
|
||||
* 1. Redistributions of source code must retain the above copyright
|
||||
* notice, this list of conditions and the following disclaimer.
|
||||
* 2. Redistributions in binary form must reproduce the above copyright
|
||||
* notice, this list of conditions and the following disclaimer in
|
||||
* the documentation and/or other materials provided with the
|
||||
* distribution.
|
||||
* 3. Neither the name PX4 nor the names of its contributors may be
|
||||
* used to endorse or promote products derived from this software
|
||||
* without specific prior written permission.
|
||||
*
|
||||
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
|
||||
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
|
||||
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
|
||||
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
|
||||
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
|
||||
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
|
||||
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
|
||||
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
|
||||
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
|
||||
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
|
||||
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||
* POSSIBILITY OF SUCH DAMAGE.
|
||||
*
|
||||
****************************************************************************/
|
||||
|
||||
/**
|
||||
* @file Subscription.cpp
|
||||
*
|
||||
*/
|
||||
|
||||
#include "Subscription.hpp"
|
||||
#include "topics/parameter_update.h"
|
||||
#include "topics/actuator_controls.h"
|
||||
#include "topics/vehicle_gps_position.h"
|
||||
#include "topics/sensor_combined.h"
|
||||
#include "topics/vehicle_attitude.h"
|
||||
#include "topics/vehicle_global_position.h"
|
||||
#include "topics/encoders.h"
|
||||
#include "topics/position_setpoint_triplet.h"
|
||||
#include "topics/vehicle_status.h"
|
||||
#include "topics/manual_control_setpoint.h"
|
||||
#include "topics/vehicle_local_position_setpoint.h"
|
||||
#include "topics/vehicle_local_position.h"
|
||||
#include "topics/vehicle_attitude_setpoint.h"
|
||||
#include "topics/vehicle_rates_setpoint.h"
|
||||
|
||||
namespace uORB
|
||||
{
|
||||
|
||||
bool __EXPORT SubscriptionBase::updated()
|
||||
{
|
||||
bool isUpdated = false;
|
||||
orb_check(_handle, &isUpdated);
|
||||
return isUpdated;
|
||||
}
|
||||
|
||||
template<class T>
|
||||
Subscription<T>::Subscription(
|
||||
List<SubscriptionBase *> * list,
|
||||
const struct orb_metadata *meta, unsigned interval) :
|
||||
T(), // initialize data structure to zero
|
||||
SubscriptionBase(list, meta) {
|
||||
setHandle(orb_subscribe(getMeta()));
|
||||
orb_set_interval(getHandle(), interval);
|
||||
}
|
||||
|
||||
template<class T>
|
||||
Subscription<T>::~Subscription() {}
|
||||
|
||||
template<class T>
|
||||
void * Subscription<T>::getDataVoidPtr() {
|
||||
return (void *)(T *)(this);
|
||||
}
|
||||
|
||||
template<class T>
|
||||
T Subscription<T>::getData() {
|
||||
return T(*this);
|
||||
}
|
||||
|
||||
template class __EXPORT Subscription<parameter_update_s>;
|
||||
template class __EXPORT Subscription<actuator_controls_s>;
|
||||
template class __EXPORT Subscription<vehicle_gps_position_s>;
|
||||
template class __EXPORT Subscription<sensor_combined_s>;
|
||||
template class __EXPORT Subscription<vehicle_attitude_s>;
|
||||
template class __EXPORT Subscription<vehicle_global_position_s>;
|
||||
template class __EXPORT Subscription<encoders_s>;
|
||||
template class __EXPORT Subscription<position_setpoint_triplet_s>;
|
||||
template class __EXPORT Subscription<vehicle_status_s>;
|
||||
template class __EXPORT Subscription<manual_control_setpoint_s>;
|
||||
template class __EXPORT Subscription<vehicle_local_position_setpoint_s>;
|
||||
template class __EXPORT Subscription<vehicle_local_position_s>;
|
||||
template class __EXPORT Subscription<vehicle_attitude_setpoint_s>;
|
||||
template class __EXPORT Subscription<vehicle_rates_setpoint_s>;
|
||||
|
||||
} // namespace uORB
|
|
@ -32,28 +32,25 @@
|
|||
****************************************************************************/
|
||||
|
||||
/**
|
||||
* @file UOrbSubscription.h
|
||||
* @file Subscription.h
|
||||
*
|
||||
*/
|
||||
|
||||
#pragma once
|
||||
|
||||
#include <uORB/uORB.h>
|
||||
#include "../block/Block.hpp"
|
||||
#include "../block/List.hpp"
|
||||
#include <containers/List.hpp>
|
||||
|
||||
|
||||
namespace control
|
||||
namespace uORB
|
||||
{
|
||||
|
||||
class Block;
|
||||
|
||||
/**
|
||||
* Base subscription warapper class, used in list traversal
|
||||
* of various subscriptions.
|
||||
*/
|
||||
class __EXPORT UOrbSubscriptionBase :
|
||||
public ListNode<control::UOrbSubscriptionBase *>
|
||||
class __EXPORT SubscriptionBase :
|
||||
public ListNode<SubscriptionBase *>
|
||||
{
|
||||
public:
|
||||
// methods
|
||||
|
@ -64,8 +61,8 @@ public:
|
|||
* @param meta The uORB metadata (usually from the ORB_ID() macro)
|
||||
* for the topic.
|
||||
*/
|
||||
UOrbSubscriptionBase(
|
||||
List<UOrbSubscriptionBase *> * list,
|
||||
SubscriptionBase(
|
||||
List<SubscriptionBase *> * list,
|
||||
const struct orb_metadata *meta) :
|
||||
_meta(meta),
|
||||
_handle() {
|
||||
|
@ -78,7 +75,7 @@ public:
|
|||
}
|
||||
}
|
||||
virtual void *getDataVoidPtr() = 0;
|
||||
virtual ~UOrbSubscriptionBase() {
|
||||
virtual ~SubscriptionBase() {
|
||||
orb_unsubscribe(_handle);
|
||||
}
|
||||
// accessors
|
||||
|
@ -93,12 +90,12 @@ protected:
|
|||
};
|
||||
|
||||
/**
|
||||
* UOrb Subscription wrapper class
|
||||
* Subscription wrapper class
|
||||
*/
|
||||
template<class T>
|
||||
class __EXPORT UOrbSubscription :
|
||||
class __EXPORT Subscription :
|
||||
public T, // this must be first!
|
||||
public UOrbSubscriptionBase
|
||||
public SubscriptionBase
|
||||
{
|
||||
public:
|
||||
/**
|
||||
|
@ -109,19 +106,13 @@ public:
|
|||
* for the topic.
|
||||
* @param interval The minimum interval in milliseconds between updates
|
||||
*/
|
||||
UOrbSubscription(
|
||||
List<UOrbSubscriptionBase *> * list,
|
||||
const struct orb_metadata *meta, unsigned interval) :
|
||||
T(), // initialize data structure to zero
|
||||
UOrbSubscriptionBase(list, meta) {
|
||||
setHandle(orb_subscribe(getMeta()));
|
||||
orb_set_interval(getHandle(), interval);
|
||||
}
|
||||
|
||||
Subscription(
|
||||
List<SubscriptionBase *> * list,
|
||||
const struct orb_metadata *meta, unsigned interval);
|
||||
/**
|
||||
* Deconstructor
|
||||
*/
|
||||
virtual ~UOrbSubscription() {}
|
||||
virtual ~Subscription();
|
||||
|
||||
/*
|
||||
* XXX
|
||||
|
@ -130,8 +121,8 @@ public:
|
|||
* should use dynamic cast, but doesn't
|
||||
* seem to be available
|
||||
*/
|
||||
void *getDataVoidPtr() { return (void *)(T *)(this); }
|
||||
T getData() { return T(*this); }
|
||||
void *getDataVoidPtr();
|
||||
T getData();
|
||||
};
|
||||
|
||||
} // namespace control
|
||||
} // namespace uORB
|
|
@ -41,4 +41,6 @@ MODULE_COMMAND = uorb
|
|||
MODULE_STACKSIZE = 4096
|
||||
|
||||
SRCS = uORB.cpp \
|
||||
objects_common.cpp
|
||||
objects_common.cpp \
|
||||
Publication.cpp \
|
||||
Subscription.cpp
|
||||
|
|
|
@ -190,3 +190,6 @@ ORB_DEFINE(navigation_capabilities, struct navigation_capabilities_s);
|
|||
|
||||
#include "topics/esc_status.h"
|
||||
ORB_DEFINE(esc_status, struct esc_status_s);
|
||||
|
||||
#include "topics/encoders.h"
|
||||
ORB_DEFINE(encoders, struct encoders_s);
|
||||
|
|
|
@ -32,20 +32,35 @@
|
|||
****************************************************************************/
|
||||
|
||||
/**
|
||||
* @file UOrbSubscription.cpp
|
||||
* @file encoders.h
|
||||
*
|
||||
* Encoders topic.
|
||||
*
|
||||
*/
|
||||
|
||||
#include "UOrbSubscription.hpp"
|
||||
#ifndef TOPIC_ENCODERS_H
|
||||
#define TOPIC_ENCODERS_H
|
||||
|
||||
namespace control
|
||||
{
|
||||
#include <stdint.h>
|
||||
#include "../uORB.h"
|
||||
|
||||
bool __EXPORT UOrbSubscriptionBase::updated()
|
||||
{
|
||||
bool isUpdated = false;
|
||||
orb_check(_handle, &isUpdated);
|
||||
return isUpdated;
|
||||
}
|
||||
/**
|
||||
* @addtogroup topics
|
||||
* @{
|
||||
*/
|
||||
|
||||
} // namespace control
|
||||
#define NUM_ENCODERS 4
|
||||
|
||||
struct encoders_s {
|
||||
uint64_t timestamp;
|
||||
int64_t counts[NUM_ENCODERS]; // counts of encoder
|
||||
float velocity[NUM_ENCODERS]; // counts of encoder/ second
|
||||
};
|
||||
|
||||
/**
|
||||
* @}
|
||||
*/
|
||||
|
||||
ORB_DECLARE(encoders);
|
||||
|
||||
#endif
|
Loading…
Reference in New Issue