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 beta_mavlink2

Browse Source
This commit is contained in:
Lorenz Meier 2014-03-21 10:47:36 +01:00
commit 193397b228
28 changed files with 418 additions and 220 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

11
ROMFS/px4fmu_common/init.d/rcS
View File

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

3
src/drivers/ets_airspeed/ets_airspeed.cpp
View File

@ -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");
}
/**

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;

112
src/drivers/meas_airspeed/meas_airspeed.cpp
View File

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

2
src/drivers/px4flow/px4flow.cpp
View File

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

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

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

@ -1030,6 +1030,7 @@ Sensors::diff_pres_poll(struct sensor_combined_s &raw)
raw.differential_pressure_pa = _diff_pres.differential_pressure_pa;
raw.differential_pressure_timestamp = _diff_pres.timestamp;
_airspeed.timestamp = hrt_absolute_time();
_airspeed.indicated_airspeed_m_s = calc_indicated_airspeed(_diff_pres.differential_pressure_pa);
_airspeed.true_airspeed_m_s = calc_true_airspeed(_diff_pres.differential_pressure_pa + raw.baro_pres_mbar * 1e2f,
raw.baro_pres_mbar * 1e2f, raw.baro_temp_celcius - PCB_TEMP_ESTIMATE_DEG);

11
src/modules/systemlib/system_params.c
View File

@ -60,3 +60,14 @@ PARAM_DEFINE_INT32(SYS_AUTOSTART, 0);
* @group System
*/
PARAM_DEFINE_INT32(SYS_AUTOCONFIG, 0);
/**
* Set usage of IO board
*
* Can be used to use a standard startup script but with a FMU only set-up. Set to 0 to force the FMU only set-up.
*
* @min 0
* @max 1
* @group System
*/
PARAM_DEFINE_INT32(SYS_USE_IO, 1);

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