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

Merge remote-tracking branch 'upstream/master' into ros_messagelayer_merge2_attctrl_posctrl 

Conflicts:
	src/drivers/px4fmu/fmu.cpp
This commit is contained in:
Thomas Gubler 2015-02-01 11:06:47 +01:00
parent 7c958a2cca a2a244584e
commit 84ff3c671d
63 changed files with 807 additions and 484 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

BIN
Documentation/px4_block_diagram.odg
View File

Binary file not shown.

2
NuttX

@ -1 +1 @@
Subproject commit e4c914e261d2647e44d05222afa7aa3cc90d3c67
Subproject commit 37cbc3e8ae6b75e9b7e79996d30fe8ed0beb9704

7
ROMFS/px4fmu_test/init.d/rcS
View File

@ -97,6 +97,13 @@ else
set unit_test_failure_list "${unit_test_failure_list} commander_tests"
fi
if uorb test
then
else
set unit_test_failure 1
set unit_test_failure_list "${unit_test_failure_list} uorb_tests"
fi
if hmc5883 -I start
then
# This is an FMUv3

1
makefiles/config_px4fmu-v2_test.mk
View File

@ -32,6 +32,7 @@ MODULES += systemcmds/tests
MODULES += systemcmds/nshterm
MODULES += systemcmds/mtd
MODULES += systemcmds/ver
MODULES += systemcmds/top
#
# System commands

2
nuttx-configs/px4fmu-v1/nsh/defconfig
View File

@ -92,7 +92,7 @@ CONFIG_ARCH_HAVE_MPU=y
#
# CONFIG_ARMV7M_TOOLCHAIN_BUILDROOT is not set
CONFIG_ARMV7M_TOOLCHAIN_GNU_EABI=y
CONFIG_ARMV7M_STACKCHECK=y
CONFIG_ARMV7M_STACKCHECK=n
CONFIG_SERIAL_TERMIOS=y
#

2
nuttx-configs/px4io-v1/nsh/defconfig
View File

@ -93,7 +93,7 @@ CONFIG_ARCH_DMA=y
CONFIG_ARCH_MATH_H=y
CONFIG_ARMV7M_CMNVECTOR=y
# CONFIG_ARMV7M_STACKCHECK is not set
CONFIG_ARMV7M_STACKCHECK=n
#
# JTAG Enable settings (by default JTAG-DP and SW-DP are disabled):

2
nuttx-configs/px4io-v2/nsh/defconfig
View File

@ -89,7 +89,7 @@ CONFIG_ARCH_DMA=y
CONFIG_ARCH_MATH_H=y
CONFIG_ARMV7M_CMNVECTOR=y
CONFIG_ARMV7M_STACKCHECK=n
#
# JTAG Enable settings (by default JTAG-DP and SW-DP are disabled):
#

5
src/drivers/ardrone_interface/ardrone_motor_control.c
View File

@ -339,7 +339,8 @@ int ardrone_write_motor_commands(int ardrone_fd, uint16_t motor1, uint16_t motor
outputs.output[3] = motor4;
static orb_advert_t pub = 0;
if (pub == 0) {
pub = orb_advertise(ORB_ID_VEHICLE_CONTROLS, &outputs);
/* advertise to channel 0 / primary */
pub = orb_advertise(ORB_ID(actuator_outputs), &outputs);
}
if (hrt_absolute_time() - last_motor_time > min_motor_interval) {
@ -350,7 +351,7 @@ int ardrone_write_motor_commands(int ardrone_fd, uint16_t motor1, uint16_t motor
fsync(ardrone_fd);
/* publish just written values */
orb_publish(ORB_ID_VEHICLE_CONTROLS, pub, &outputs);
orb_publish(ORB_ID(actuator_outputs), pub, &outputs);
if (ret == sizeof(buf)) {
return OK;

6
src/drivers/drv_accel.h
View File

@ -1,6 +1,6 @@
/****************************************************************************
*
* Copyright (C) 2012 PX4 Development Team. All rights reserved.
* Copyright (c) 2012-2015 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
@ -81,9 +81,7 @@ struct accel_scale {
/*
* ObjDev tag for raw accelerometer data.
*/
ORB_DECLARE(sensor_accel0);
ORB_DECLARE(sensor_accel1);
ORB_DECLARE(sensor_accel2);
ORB_DECLARE(sensor_accel);
/*
* ioctl() definitions

6
src/drivers/drv_baro.h
View File

@ -1,6 +1,6 @@
/****************************************************************************
*
* Copyright (C) 2012 PX4 Development Team. All rights reserved.
* Copyright (c) 2012-2015 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
@ -63,9 +63,7 @@ struct baro_report {
/*
* ObjDev tag for raw barometer data.
*/
ORB_DECLARE(sensor_baro0);
ORB_DECLARE(sensor_baro1);
ORB_DECLARE(sensor_baro2);
ORB_DECLARE(sensor_baro);
/*
* ioctl() definitions

2
src/drivers/drv_blinkm.h
View File

@ -1,6 +1,6 @@
/****************************************************************************
*
* Copyright (C) 2012 PX4 Development Team. All rights reserved.
* Copyright (c) 2012-2015 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

6
src/drivers/drv_gyro.h
View File

@ -1,6 +1,6 @@
/****************************************************************************
*
* Copyright (C) 2012 PX4 Development Team. All rights reserved.
* Copyright (c) 2012-2015 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
@ -81,9 +81,7 @@ struct gyro_scale {
/*
* ObjDev tag for raw gyro data.
*/
ORB_DECLARE(sensor_gyro0);
ORB_DECLARE(sensor_gyro1);
ORB_DECLARE(sensor_gyro2);
ORB_DECLARE(sensor_gyro);
/*
* ioctl() definitions

12
src/drivers/drv_mag.h
View File

@ -1,6 +1,6 @@
/****************************************************************************
*
* Copyright (C) 2012 PX4 Development Team. All rights reserved.
* Copyright (c) 2012-2015 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
@ -41,7 +41,6 @@
#include <stdint.h>
#include <sys/ioctl.h>
#include "drv_device.h"
#include "drv_sensor.h"
#include "drv_orb_dev.h"
@ -81,15 +80,8 @@ struct mag_scale {
/*
* ObjDev tag for raw magnetometer data.
*/
ORB_DECLARE(sensor_mag0);
ORB_DECLARE(sensor_mag1);
ORB_DECLARE(sensor_mag2);
ORB_DECLARE(sensor_mag);
/*
* mag device types, for _device_id
*/
#define DRV_MAG_DEVTYPE_HMC5883 1
#define DRV_MAG_DEVTYPE_LSM303D 2
/*
* ioctl() definitions

9
src/drivers/drv_orb_dev.h
View File

@ -1,6 +1,6 @@
/****************************************************************************
*
* Copyright (C) 2012 PX4 Development Team. All rights reserved.
* Copyright (c) 2012-2015 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
@ -35,7 +35,9 @@
#define _DRV_UORB_H
/**
* @file uORB published object driver.
* @file drv_orb_dev.h
*
* uORB published object driver.
*/
#include <sys/types.h>
@ -84,4 +86,7 @@
/** Get the global advertiser handle for the topic */
#define ORBIOCGADVERTISER _ORBIOC(13)
/** Get the priority for the topic */
#define ORBIOCGPRIORITY _ORBIOC(14)
#endif /* _DRV_UORB_H */

18
src/drivers/drv_sensor.h
View File

@ -43,6 +43,24 @@
#include <stdint.h>
#include <sys/ioctl.h>
#include "drv_device.h"
/**
* Sensor type definitions.
*
* Used to create a unique device id for redundant and combo sensors
*/
#define DRV_MAG_DEVTYPE_HMC5883 0x01
#define DRV_MAG_DEVTYPE_LSM303D 0x02
#define DRV_ACC_DEVTYPE_LSM303D 0x11
#define DRV_ACC_DEVTYPE_BMA180 0x12
#define DRV_ACC_DEVTYPE_MPU6000 0x13
#define DRV_GYR_DEVTYPE_MPU6000 0x21
#define DRV_GYR_DEVTYPE_L3GD20 0x22
#define DRV_RNG_DEVTYPE_MB12XX 0x31
#define DRV_RNG_DEVTYPE_LL40LS 0x32
/*
* ioctl() definitions
*

7
src/drivers/hil/hil.cpp
View File

@ -332,8 +332,9 @@ HIL::task_main()
actuator_outputs_s outputs;
memset(&outputs, 0, sizeof(outputs));
/* advertise the mixed control outputs */
_t_outputs = orb_advertise(_primary_pwm_device ? ORB_ID_VEHICLE_CONTROLS : ORB_ID(actuator_outputs_1),
&outputs);
int dummy;
_t_outputs = orb_advertise_multi(ORB_ID(actuator_outputs),
&outputs, &dummy, ORB_PRIO_LOW);
pollfd fds[2];
fds[0].fd = _t_actuators;
@ -425,7 +426,7 @@ HIL::task_main()
}
/* and publish for anyone that cares to see */
orb_publish(ORB_ID_VEHICLE_CONTROLS, _t_outputs, &outputs);
orb_publish(ORB_ID(actuator_outputs), _t_outputs, &outputs);
}
}

35
src/drivers/hmc5883/hmc5883.cpp
View File

@ -69,7 +69,6 @@
#include <drivers/drv_device.h>
#include <uORB/uORB.h>
#include <uORB/topics/subsystem_info.h>
#include <float.h>
#include <getopt.h>
@ -157,10 +156,9 @@ private:
float _range_ga;
bool _collect_phase;
int _class_instance;
int _orb_class_instance;
orb_advert_t _mag_topic;
orb_advert_t _subsystem_pub;
orb_id_t _mag_orb_id;
perf_counter_t _sample_perf;
perf_counter_t _comms_errors;
@ -348,9 +346,8 @@ HMC5883::HMC5883(device::Device *interface, const char *path, enum Rotation rota
_range_ga(1.3f),
_collect_phase(false),
_class_instance(-1),
_orb_class_instance(-1),
_mag_topic(-1),
_subsystem_pub(-1),
_mag_orb_id(nullptr),
_sample_perf(perf_alloc(PC_ELAPSED, "hmc5883_read")),
_comms_errors(perf_alloc(PC_COUNT, "hmc5883_comms_errors")),
_buffer_overflows(perf_alloc(PC_COUNT, "hmc5883_buffer_overflows")),
@ -419,7 +416,6 @@ HMC5883::init()
reset();
_class_instance = register_class_devname(MAG_DEVICE_PATH);
_mag_orb_id = ORB_ID_TRIPLE(sensor_mag, _class_instance);
ret = OK;
/* sensor is ok, but not calibrated */
@ -850,6 +846,7 @@ HMC5883::collect()
perf_begin(_sample_perf);
struct mag_report new_report;
bool sensor_is_onboard = false;
/* this should be fairly close to the end of the measurement, so the best approximation of the time */
new_report.timestamp = hrt_absolute_time();
@ -902,7 +899,8 @@ HMC5883::collect()
// XXX revisit for SPI part, might require a bus type IOCTL
unsigned dummy;
if (!_interface->ioctl(MAGIOCGEXTERNAL, dummy)) {
sensor_is_onboard = !_interface->ioctl(MAGIOCGEXTERNAL, dummy);
if (sensor_is_onboard) {
// convert onboard so it matches offboard for the
// scaling below
report.y = -report.y;
@ -925,9 +923,10 @@ HMC5883::collect()
if (_mag_topic != -1) {
/* publish it */
orb_publish(_mag_orb_id, _mag_topic, &new_report);
orb_publish(ORB_ID(sensor_mag), _mag_topic, &new_report);
} else {
_mag_topic = orb_advertise(_mag_orb_id, &new_report);
_mag_topic = orb_advertise_multi(ORB_ID(sensor_mag), &new_report,
&_orb_class_instance, (sensor_is_onboard) ? ORB_PRIO_HIGH : ORB_PRIO_MAX);
if (_mag_topic < 0)
debug("ADVERT FAIL");
@ -1185,24 +1184,6 @@ int HMC5883::check_calibration()
warnx("mag cal status changed %s%s", (scale_valid) ? "" : "scale invalid ",
(offset_valid) ? "" : "offset invalid");
_calibrated = (offset_valid && scale_valid);
// XXX Change advertisement
/* notify about state change */
struct subsystem_info_s info = {
true,
true,
_calibrated,
SUBSYSTEM_TYPE_MAG};
if (!(_pub_blocked)) {
if (_subsystem_pub > 0) {
orb_publish(ORB_ID(subsystem_info), _subsystem_pub, &info);
} else {
_subsystem_pub = orb_advertise(ORB_ID(subsystem_info), &info);
}
}
}
/* return 0 if calibrated, 1 else */

56
src/drivers/l3gd20/l3gd20.cpp
View File

@ -1,6 +1,6 @@
/****************************************************************************
*
* Copyright (c) 2012-2014 PX4 Development Team. All rights reserved.
* Copyright (c) 2012-2015 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
@ -33,7 +33,7 @@
/**
* @file l3gd20.cpp
* Driver for the ST L3GD20 MEMS gyro connected via SPI.
* Driver for the ST L3GD20 MEMS and L3GD20H mems gyros connected via SPI.
*
* Note: With the exception of the self-test feature, the ST L3G4200D is
* also supported by this driver.
@ -179,6 +179,12 @@ static const int ERROR = -1;
#define L3GD20_DEFAULT_FILTER_FREQ 30
#define L3GD20_TEMP_OFFSET_CELSIUS 40
#ifdef PX4_SPI_BUS_EXT
#define EXTERNAL_BUS PX4_SPI_BUS_EXT
#else
#define EXTERNAL_BUS 0
#endif
#ifndef SENSOR_BOARD_ROTATION_DEFAULT
#define SENSOR_BOARD_ROTATION_DEFAULT SENSOR_BOARD_ROTATION_270_DEG
#endif
@ -214,14 +220,14 @@ private:
struct hrt_call _call;
unsigned _call_interval;
RingBuffer *_reports;
struct gyro_scale _gyro_scale;
float _gyro_range_scale;
float _gyro_range_rad_s;
orb_advert_t _gyro_topic;
orb_id_t _orb_id;
int _orb_class_instance;
int _class_instance;
unsigned _current_rate;
@ -273,6 +279,13 @@ private:
*/
void disable_i2c();
/**
* Get the internal / external state
*
* @return true if the sensor is not on the main MCU board
*/
bool is_external() { return (_bus == EXTERNAL_BUS); }
/**
* Static trampoline from the hrt_call context; because we don't have a
* generic hrt wrapper yet.
@ -391,7 +404,7 @@ L3GD20::L3GD20(int bus, const char* path, spi_dev_e device, enum Rotation rotati
_gyro_range_scale(0.0f),
_gyro_range_rad_s(0.0f),
_gyro_topic(-1),
_orb_id(nullptr),
_orb_class_instance(-1),
_class_instance(-1),
_current_rate(0),
_orientation(SENSOR_BOARD_ROTATION_DEFAULT),
@ -411,6 +424,8 @@ L3GD20::L3GD20(int bus, const char* path, spi_dev_e device, enum Rotation rotati
// enable debug() calls
_debug_enabled = true;
_device_id.devid_s.devtype = DRV_GYR_DEVTYPE_L3GD20;
// default scale factors
_gyro_scale.x_offset = 0;
_gyro_scale.x_scale = 1.0f;
@ -456,20 +471,6 @@ L3GD20::init()
_class_instance = register_class_devname(GYRO_DEVICE_PATH);
switch (_class_instance) {
case CLASS_DEVICE_PRIMARY:
_orb_id = ORB_ID(sensor_gyro0);
break;
case CLASS_DEVICE_SECONDARY:
_orb_id = ORB_ID(sensor_gyro1);
break;
case CLASS_DEVICE_TERTIARY:
_orb_id = ORB_ID(sensor_gyro2);
break;
}
reset();
measure();
@ -478,7 +479,8 @@ L3GD20::init()
struct gyro_report grp;
_reports->get(&grp);
_gyro_topic = orb_advertise(_orb_id, &grp);
_gyro_topic = orb_advertise_multi(ORB_ID(sensor_gyro), &grp,
&_orb_class_instance, (is_external()) ? ORB_PRIO_VERY_HIGH : ORB_PRIO_DEFAULT);
if (_gyro_topic < 0) {
debug("failed to create sensor_gyro publication");
@ -639,7 +641,7 @@ L3GD20::ioctl(struct file *filp, int cmd, unsigned long arg)
return -ENOMEM;
}
irqrestore(flags);
return OK;
}
@ -867,7 +869,7 @@ L3GD20::reset()
disable_i2c();
/* set default configuration */
write_checked_reg(ADDR_CTRL_REG1,
write_checked_reg(ADDR_CTRL_REG1,
REG1_POWER_NORMAL | REG1_Z_ENABLE | REG1_Y_ENABLE | REG1_X_ENABLE);
write_checked_reg(ADDR_CTRL_REG2, 0); /* disable high-pass filters */
write_checked_reg(ADDR_CTRL_REG3, 0x08); /* DRDY enable */
@ -911,7 +913,7 @@ L3GD20::check_registers(void)
if we get the wrong value then we know the SPI bus
or sensor is very sick. We set _register_wait to 20
and wait until we have seen 20 good values in a row
before we consider the sensor to be OK again.
before we consider the sensor to be OK again.
*/
perf_count(_bad_registers);
@ -974,7 +976,7 @@ L3GD20::measure()
we waited for DRDY, but did not see DRDY on all axes
when we captured. That means a transfer error of some sort
*/
perf_count(_errors);
perf_count(_errors);
return;
}
#endif
@ -994,7 +996,7 @@ L3GD20::measure()
*/
report.timestamp = hrt_absolute_time();
report.error_count = perf_event_count(_bad_registers);
switch (_orientation) {
case SENSOR_BOARD_ROTATION_000_DEG:
@ -1050,7 +1052,7 @@ L3GD20::measure()
/* publish for subscribers */
if (!(_pub_blocked)) {
/* publish it */
orb_publish(_orb_id, _gyro_topic, &report);
orb_publish(ORB_ID(sensor_gyro), _gyro_topic, &report);
}
_read++;
@ -1072,7 +1074,7 @@ L3GD20::print_info()
for (uint8_t i=0; i<L3GD20_NUM_CHECKED_REGISTERS; i++) {
uint8_t v = read_reg(_checked_registers[i]);
if (v != _checked_values[i]) {
::printf("reg %02x:%02x should be %02x\n",
::printf("reg %02x:%02x should be %02x\n",
(unsigned)_checked_registers[i],
(unsigned)v,
(unsigned)_checked_values[i]);

94
src/drivers/lsm303d/lsm303d.cpp
View File

@ -1,6 +1,6 @@
/****************************************************************************
*
* Copyright (c) 2013, 2014 PX4 Development Team. All rights reserved.
* Copyright (c) 2013-2015 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
@ -211,6 +211,12 @@ static const int ERROR = -1;
#define LSM303D_ONE_G 9.80665f
#ifdef PX4_SPI_BUS_EXT
#define EXTERNAL_BUS PX4_SPI_BUS_EXT
#else
#define EXTERNAL_BUS 0
#endif
extern "C" { __EXPORT int lsm303d_main(int argc, char *argv[]); }
@ -275,7 +281,7 @@ private:
unsigned _mag_samplerate;
orb_advert_t _accel_topic;
orb_id_t _accel_orb_id;
int _accel_orb_class_instance;
int _accel_class_instance;
unsigned _accel_read;
@ -295,7 +301,7 @@ private:
enum Rotation _rotation;
// values used to
// values used to
float _last_accel[3];
uint8_t _constant_accel_count;
@ -329,6 +335,13 @@ private:
*/
void disable_i2c();
/**
* Get the internal / external state
*
* @return true if the sensor is not on the main MCU board
*/
bool is_external() { return (_bus == EXTERNAL_BUS); }
/**
* Static trampoline from the hrt_call context; because we don't have a
* generic hrt wrapper yet.
@ -507,7 +520,7 @@ private:
LSM303D *_parent;
orb_advert_t _mag_topic;
orb_id_t _mag_orb_id;
int _mag_orb_class_instance;
int _mag_class_instance;
void measure();
@ -539,7 +552,7 @@ LSM303D::LSM303D(int bus, const char* path, spi_dev_e device, enum Rotation rota
_mag_range_scale(0.0f),
_mag_samplerate(0),
_accel_topic(-1),
_accel_orb_id(nullptr),
_accel_orb_class_instance(-1),
_accel_class_instance(-1),
_accel_read(0),
_mag_read(0),
@ -556,11 +569,18 @@ LSM303D::LSM303D(int bus, const char* path, spi_dev_e device, enum Rotation rota
_constant_accel_count(0),
_checked_next(0)
{
_device_id.devid_s.devtype = DRV_MAG_DEVTYPE_LSM303D;
// enable debug() calls
_debug_enabled = true;
_device_id.devid_s.devtype = DRV_ACC_DEVTYPE_LSM303D;
/* Prime _mag with parents devid. */
_mag->_device_id.devid = _device_id.devid;
_mag->_device_id.devid_s.devtype = DRV_MAG_DEVTYPE_LSM303D;
// default scale factors
_accel_scale.x_offset = 0.0f;
_accel_scale.x_scale = 1.0f;
@ -618,7 +638,6 @@ LSM303D::init()
if (_accel_reports == nullptr)
goto out;
/* advertise accel topic */
_mag_reports = new RingBuffer(2, sizeof(mag_report));
if (_mag_reports == nullptr)
@ -641,26 +660,14 @@ LSM303D::init()
_mag_reports->get(&mrp);
/* measurement will have generated a report, publish */
switch (_mag->_mag_class_instance) {
case CLASS_DEVICE_PRIMARY:
_mag->_mag_orb_id = ORB_ID(sensor_mag0);
break;
case CLASS_DEVICE_SECONDARY:
_mag->_mag_orb_id = ORB_ID(sensor_mag1);
break;
case CLASS_DEVICE_TERTIARY:
_mag->_mag_orb_id = ORB_ID(sensor_mag2);
break;
}
_mag->_mag_topic = orb_advertise(_mag->_mag_orb_id, &mrp);
_mag->_mag_topic = orb_advertise_multi(ORB_ID(sensor_mag), &mrp,
&_mag->_mag_orb_class_instance, ORB_PRIO_LOW);
if (_mag->_mag_topic < 0) {
warnx("ADVERT ERR");
}
_accel_class_instance = register_class_devname(ACCEL_DEVICE_PATH);
/* advertise sensor topic, measure manually to initialize valid report */
@ -668,21 +675,8 @@ LSM303D::init()
_accel_reports->get(&arp);
/* measurement will have generated a report, publish */
switch (_accel_class_instance) {
case CLASS_DEVICE_PRIMARY:
_accel_orb_id = ORB_ID(sensor_accel0);
break;
case CLASS_DEVICE_SECONDARY:
_accel_orb_id = ORB_ID(sensor_accel1);
break;
case CLASS_DEVICE_TERTIARY:
_accel_orb_id = ORB_ID(sensor_accel2);
break;
}
_accel_topic = orb_advertise(_accel_orb_id, &arp);
_accel_topic = orb_advertise_multi(ORB_ID(sensor_accel), &arp,
&_accel_orb_class_instance, (is_external()) ? ORB_PRIO_VERY_HIGH : ORB_PRIO_DEFAULT);
if (_accel_topic < 0) {
warnx("ADVERT ERR");
@ -712,7 +706,7 @@ LSM303D::reset()
disable_i2c();
/* enable accel*/
write_checked_reg(ADDR_CTRL_REG1,
write_checked_reg(ADDR_CTRL_REG1,
REG1_X_ENABLE_A | REG1_Y_ENABLE_A | REG1_Z_ENABLE_A | REG1_BDU_UPDATE | REG1_RATE_800HZ_A);
/* enable mag */
@ -746,7 +740,7 @@ LSM303D::probe()
/* verify that the device is attached and functioning */
bool success = (read_reg(ADDR_WHO_AM_I) == WHO_I_AM);
if (success) {
_checked_values[0] = WHO_I_AM;
return OK;
@ -1019,7 +1013,7 @@ LSM303D::mag_ioctl(struct file *filp, int cmd, unsigned long arg)
return SENSOR_POLLRATE_MANUAL;
return 1000000 / _call_mag_interval;
case SENSORIOCSQUEUEDEPTH: {
/* lower bound is mandatory, upper bound is a sanity check */
if ((arg < 1) || (arg > 100))
@ -1424,7 +1418,7 @@ LSM303D::check_registers(void)
if we get the wrong value then we know the SPI bus
or sensor is very sick. We set _register_wait to 20
and wait until we have seen 20 good values in a row
before we consider the sensor to be OK again.
before we consider the sensor to be OK again.
*/
perf_count(_bad_registers);
@ -1548,7 +1542,7 @@ LSM303D::measure()
perf_count(_bad_values);
_constant_accel_count = 0;
}
_last_accel[0] = x_in_new;
_last_accel[1] = y_in_new;
_last_accel[2] = z_in_new;
@ -1570,7 +1564,7 @@ LSM303D::measure()
if (!(_pub_blocked)) {
/* publish it */
orb_publish(_accel_orb_id, _accel_topic, &accel_report);
orb_publish(ORB_ID(sensor_accel), _accel_topic, &accel_report);
}
_accel_read++;
@ -1641,7 +1635,7 @@ LSM303D::mag_measure()
if (!(_pub_blocked)) {
/* publish it */
orb_publish(_mag->_mag_orb_id, _mag->_mag_topic, &mag_report);
orb_publish(ORB_ID(sensor_mag), _mag->_mag_topic, &mag_report);
}
_mag_read++;
@ -1666,7 +1660,7 @@ LSM303D::print_info()
for (uint8_t i=0; i<LSM303D_NUM_CHECKED_REGISTERS; i++) {
uint8_t v = read_reg(_checked_registers[i]);
if (v != _checked_values[i]) {
::printf("reg %02x:%02x should be %02x\n",
::printf("reg %02x:%02x should be %02x\n",
(unsigned)_checked_registers[i],
(unsigned)v,
(unsigned)_checked_values[i]);
@ -1742,7 +1736,7 @@ LSM303D_mag::LSM303D_mag(LSM303D *parent) :
CDev("LSM303D_mag", LSM303D_DEVICE_PATH_MAG),
_parent(parent),
_mag_topic(-1),
_mag_orb_id(nullptr),
_mag_orb_class_instance(-1),
_mag_class_instance(-1)
{
}
@ -1783,7 +1777,13 @@ LSM303D_mag::read(struct file *filp, char *buffer, size_t buflen)
int
LSM303D_mag::ioctl(struct file *filp, int cmd, unsigned long arg)
{
return _parent->mag_ioctl(filp, cmd, arg);
switch (cmd) {
case DEVIOCGDEVICEID:
return (int)CDev::ioctl(filp, cmd, arg);
break;
default:
return _parent->mag_ioctl(filp, cmd, arg);
}
}
void

5
src/drivers/mkblctrl/mkblctrl.cpp
View File

@ -457,8 +457,9 @@ MK::task_main()
actuator_outputs_s outputs;
memset(&outputs, 0, sizeof(outputs));
/* advertise the mixed control outputs */
_t_outputs = orb_advertise(_primary_pwm_device ? ORB_ID_VEHICLE_CONTROLS : ORB_ID(actuator_outputs_1),
&outputs);
int dummy;
_t_outputs = orb_advertise_multi(ORB_ID(actuator_outputs),
&outputs, &dummy, ORB_PRIO_HIGH);
/* advertise the blctrl status */
esc_status_s esc;

119
src/drivers/mpu6000/mpu6000.cpp
View File

@ -1,6 +1,6 @@
/****************************************************************************
*
* Copyright (c) 2012-2014 PX4 Development Team. All rights reserved.
* Copyright (c) 2012-2015 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
@ -175,6 +175,12 @@
#define MPU6000_ONE_G 9.80665f
#ifdef PX4_SPI_BUS_EXT
#define EXTERNAL_BUS PX4_SPI_BUS_EXT
#else
#define EXTERNAL_BUS 0
#endif
/*
the MPU6000 can only handle high SPI bus speeds on the sensor and
interrupt status registers. All other registers have a maximum 1MHz
@ -234,7 +240,7 @@ private:
float _accel_range_scale;
float _accel_range_m_s2;
orb_advert_t _accel_topic;
orb_id_t _accel_orb_id;
int _accel_orb_class_instance;
int _accel_class_instance;
RingBuffer *_gyro_reports;
@ -360,6 +366,13 @@ private:
*/
uint16_t swap16(uint16_t val) { return (val >> 8) | (val << 8); }
/**
* Get the internal / external state
*
* @return true if the sensor is not on the main MCU board
*/
bool is_external() { return (_bus == EXTERNAL_BUS); }
/**
* Measurement self test
*
@ -433,7 +446,7 @@ const uint8_t MPU6000::_checked_registers[MPU6000_NUM_CHECKED_REGISTERS] = { MPU
MPUREG_INT_ENABLE,
MPUREG_INT_PIN_CFG };
/**
* Helper class implementing the gyro driver node.
@ -457,7 +470,7 @@ protected:
private:
MPU6000 *_parent;
orb_advert_t _gyro_topic;
orb_id_t _gyro_orb_id;
int _gyro_orb_class_instance;
int _gyro_class_instance;
/* do not allow to copy this class due to pointer data members */
@ -479,7 +492,7 @@ MPU6000::MPU6000(int bus, const char *path_accel, const char *path_gyro, spi_dev
_accel_range_scale(0.0f),
_accel_range_m_s2(0.0f),
_accel_topic(-1),
_accel_orb_id(nullptr),
_accel_orb_class_instance(-1),
_accel_class_instance(-1),
_gyro_reports(nullptr),
_gyro_scale{},
@ -510,6 +523,12 @@ MPU6000::MPU6000(int bus, const char *path_accel, const char *path_gyro, spi_dev
// disable debug() calls
_debug_enabled = false;
_device_id.devid_s.devtype = DRV_ACC_DEVTYPE_MPU6000;
/* Prime _gyro with parents devid. */
_gyro->_device_id.devid = _device_id.devid;
_gyro->_device_id.devid_s.devtype = DRV_GYR_DEVTYPE_MPU6000;
// default accel scale factors
_accel_scale.x_offset = 0;
_accel_scale.x_scale = 1.0f;
@ -596,6 +615,7 @@ MPU6000::init()
_gyro_scale.z_offset = 0;
_gyro_scale.z_scale = 1.0f;
/* do CDev init for the gyro device node, keep it optional */
ret = _gyro->init();
/* if probe/setup failed, bail now */
@ -613,22 +633,8 @@ MPU6000::init()
_accel_reports->get(&arp);
/* measurement will have generated a report, publish */
switch (_accel_class_instance) {
case CLASS_DEVICE_PRIMARY:
_accel_orb_id = ORB_ID(sensor_accel0);
break;
case CLASS_DEVICE_SECONDARY:
_accel_orb_id = ORB_ID(sensor_accel1);
break;
case CLASS_DEVICE_TERTIARY:
_accel_orb_id = ORB_ID(sensor_accel2);
break;
}
_accel_topic = orb_advertise(_accel_orb_id, &arp);
_accel_topic = orb_advertise_multi(ORB_ID(sensor_accel), &arp,
&_accel_orb_class_instance, (is_external()) ? ORB_PRIO_MAX : ORB_PRIO_HIGH);
if (_accel_topic < 0) {
warnx("ADVERT FAIL");
@ -639,22 +645,8 @@ MPU6000::init()
struct gyro_report grp;
_gyro_reports->get(&grp);
switch (_gyro->_gyro_class_instance) {
case CLASS_DEVICE_PRIMARY:
_gyro->_gyro_orb_id = ORB_ID(sensor_gyro0);
break;
case CLASS_DEVICE_SECONDARY:
_gyro->_gyro_orb_id = ORB_ID(sensor_gyro1);
break;
case CLASS_DEVICE_TERTIARY:
_gyro->_gyro_orb_id = ORB_ID(sensor_gyro2);
break;
}
_gyro->_gyro_topic = orb_advertise(_gyro->_gyro_orb_id, &grp);
_gyro->_gyro_topic = orb_advertise_multi(ORB_ID(sensor_gyro), &grp,
&_gyro->_gyro_orb_class_instance, (is_external()) ? ORB_PRIO_MAX : ORB_PRIO_HIGH);
if (_gyro->_gyro_topic < 0) {
warnx("ADVERT FAIL");
@ -683,7 +675,7 @@ int MPU6000::reset()
// for it to come out of sleep
write_checked_reg(MPUREG_PWR_MGMT_1, MPU_CLK_SEL_PLLGYROZ);
up_udelay(1000);
// Disable I2C bus (recommended on datasheet)
write_checked_reg(MPUREG_USER_CTRL, BIT_I2C_IF_DIS);
irqrestore(state);
@ -741,7 +733,7 @@ int MPU6000::reset()
case MPU6000_REV_D9:
case MPU6000_REV_D10:
// default case to cope with new chip revisions, which
// presumably won't have the accel scaling bug
// presumably won't have the accel scaling bug
default:
// Accel scale 8g (4096 LSB/g)
write_checked_reg(MPUREG_ACCEL_CONFIG, 2 << 3);
@ -819,7 +811,7 @@ MPU6000::_set_dlpf_filter(uint16_t frequency_hz)
{
uint8_t filter;
/*
/*
choose next highest filter frequency available
*/
if (frequency_hz == 0) {
@ -921,7 +913,7 @@ MPU6000::gyro_self_test()
if (self_test())
return 1;
/*
/*
* Maximum deviation of 20 degrees, according to
* http://www.invensense.com/mems/gyro/documents/PS-MPU-6000A-00v3.4.pdf
* Section 6.1, initial ZRO tolerance
@ -982,7 +974,7 @@ MPU6000::factory_self_test()
// gyro self test has to be done at 250DPS
write_reg(MPUREG_GYRO_CONFIG, BITS_FS_250DPS);
struct MPUReport mpu_report;
struct MPUReport mpu_report;
float accel_baseline[3];
float gyro_baseline[3];
float accel[3];
@ -1012,10 +1004,10 @@ MPU6000::factory_self_test()
}
#if 1
write_reg(MPUREG_GYRO_CONFIG,
BITS_FS_250DPS |
BITS_GYRO_ST_X |
BITS_GYRO_ST_Y |
write_reg(MPUREG_GYRO_CONFIG,
BITS_FS_250DPS |
BITS_GYRO_ST_X |
BITS_GYRO_ST_Y |
BITS_GYRO_ST_Z);
// accel 8g, self-test enabled all axes
@ -1085,7 +1077,7 @@ MPU6000::factory_self_test()
::printf("FAIL\n");
ret = -EIO;
}
}
}
for (uint8_t i=0; i<3; i++) {
float diff = gyro[i]-gyro_baseline[i];
float err = 100*(diff - gyro_ftrim[i]) / gyro_ftrim[i];
@ -1100,7 +1092,7 @@ MPU6000::factory_self_test()
::printf("FAIL\n");
ret = -EIO;
}
}
}
write_reg(MPUREG_GYRO_CONFIG, saved_gyro_config);
write_reg(MPUREG_ACCEL_CONFIG, saved_accel_config);
@ -1247,14 +1239,14 @@ MPU6000::ioctl(struct file *filp, int cmd, unsigned long arg)
/* lower bound is mandatory, upper bound is a sanity check */
if ((arg < 1) || (arg > 100))
return -EINVAL;
irqstate_t flags = irqsave();
if (!_accel_reports->resize(arg)) {
irqrestore(flags);
return -ENOMEM;
}
irqrestore(flags);
return OK;
}
@ -1536,13 +1528,13 @@ MPU6000::check_registers(void)
the data registers.
*/
uint8_t v;
if ((v=read_reg(_checked_registers[_checked_next], MPU6000_HIGH_BUS_SPEED)) !=
if ((v=read_reg(_checked_registers[_checked_next], MPU6000_HIGH_BUS_SPEED)) !=
_checked_values[_checked_next]) {
/*
if we get the wrong value then we know the SPI bus
or sensor is very sick. We set _register_wait to 20
and wait until we have seen 20 good values in a row
before we consider the sensor to be OK again.
before we consider the sensor to be OK again.
*/
perf_count(_bad_registers);
@ -1655,7 +1647,7 @@ MPU6000::measure()
_register_wait--;
return;
}
/*
* Swap axes and negate y
@ -1716,7 +1708,7 @@ MPU6000::measure()
float x_in_new = ((report.accel_x * _accel_range_scale) - _accel_scale.x_offset) * _accel_scale.x_scale;
float y_in_new = ((report.accel_y * _accel_range_scale) - _accel_scale.y_offset) * _accel_scale.y_scale;
float z_in_new = ((report.accel_z * _accel_range_scale) - _accel_scale.z_offset) * _accel_scale.z_scale;
arb.x = _accel_filter_x.apply(x_in_new);
arb.y = _accel_filter_y.apply(y_in_new);
arb.z = _accel_filter_z.apply(z_in_new);
@ -1737,7 +1729,7 @@ MPU6000::measure()
float x_gyro_in_new = ((report.gyro_x * _gyro_range_scale) - _gyro_scale.x_offset) * _gyro_scale.x_scale;
float y_gyro_in_new = ((report.gyro_y * _gyro_range_scale) - _gyro_scale.y_offset) * _gyro_scale.y_scale;
float z_gyro_in_new = ((report.gyro_z * _gyro_range_scale) - _gyro_scale.z_offset) * _gyro_scale.z_scale;
grb.x = _gyro_filter_x.apply(x_gyro_in_new);
grb.y = _gyro_filter_y.apply(y_gyro_in_new);
grb.z = _gyro_filter_z.apply(z_gyro_in_new);
@ -1763,12 +1755,12 @@ MPU6000::measure()
perf_begin(_controller_latency_perf);
perf_begin(_system_latency_perf);
/* publish it */
orb_publish(_accel_orb_id, _accel_topic, &arb);
orb_publish(ORB_ID(sensor_accel), _accel_topic, &arb);
}
if (!(_pub_blocked)) {
/* publish it */
orb_publish(_gyro->_gyro_orb_id, _gyro->_gyro_topic, &grb);
orb_publish(ORB_ID(sensor_gyro), _gyro->_gyro_topic, &grb);
}
/* stop measuring */
@ -1791,7 +1783,7 @@ MPU6000::print_info()
for (uint8_t i=0; i<MPU6000_NUM_CHECKED_REGISTERS; i++) {
uint8_t v = read_reg(_checked_registers[i], MPU6000_HIGH_BUS_SPEED);
if (v != _checked_values[i]) {
::printf("reg %02x:%02x should be %02x\n",
::printf("reg %02x:%02x should be %02x\n",
(unsigned)_checked_registers[i],
(unsigned)v,
(unsigned)_checked_values[i]);
@ -1818,7 +1810,7 @@ MPU6000_gyro::MPU6000_gyro(MPU6000 *parent, const char *path) :
CDev("MPU6000_gyro", path),
_parent(parent),
_gyro_topic(-1),
_gyro_orb_id(nullptr),
_gyro_orb_class_instance(-1),
_gyro_class_instance(-1)
{
}
@ -1863,7 +1855,14 @@ MPU6000_gyro::read(struct file *filp, char *buffer, size_t buflen)
int
MPU6000_gyro::ioctl(struct file *filp, int cmd, unsigned long arg)
{
return _parent->gyro_ioctl(filp, cmd, arg);
switch (cmd) {
case DEVIOCGDEVICEID:
return (int)CDev::ioctl(filp, cmd, arg);
break;
default:
return _parent->gyro_ioctl(filp, cmd, arg);
}
}
/**

21
src/drivers/ms5611/ms5611.cpp
View File

@ -1,6 +1,6 @@
/****************************************************************************
*
* Copyright (c) 2012-2014 PX4 Development Team. All rights reserved.
* Copyright (c) 2012-2015 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
@ -57,6 +57,7 @@
#include <nuttx/clock.h>
#include <arch/board/board.h>
#include <board_config.h>
#include <drivers/device/device.h>
#include <drivers/drv_baro.h>
@ -134,8 +135,7 @@ protected:
unsigned _msl_pressure; /* in Pa */
orb_advert_t _baro_topic;
orb_id_t _orb_id;
int _orb_class_instance;
int _class_instance;
perf_counter_t _sample_perf;
@ -171,6 +171,13 @@ protected:
*/
void cycle();
/**
* Get the internal / external state
*
* @return true if the sensor is not on the main MCU board
*/
bool is_external() { return (_orb_class_instance == 0); /* XXX put this into the interface class */ }
/**
* Static trampoline from the workq context; because we don't have a
* generic workq wrapper yet.
@ -210,6 +217,7 @@ MS5611::MS5611(device::Device *interface, ms5611::prom_u &prom_buf, const char*
_SENS(0),
_msl_pressure(101325),
_baro_topic(-1),
_orb_class_instance(-1),
_class_instance(-1),
_sample_perf(perf_alloc(PC_ELAPSED, "ms5611_read")),
_measure_perf(perf_alloc(PC_ELAPSED, "ms5611_measure")),
@ -263,7 +271,6 @@ MS5611::init()
/* register alternate interfaces if we have to */
_class_instance = register_class_devname(BARO_DEVICE_PATH);
_orb_id = ORB_ID_TRIPLE(sensor_baro, _class_instance);
struct baro_report brp;
/* do a first measurement cycle to populate reports with valid data */
@ -303,7 +310,9 @@ MS5611::init()
ret = OK;
_baro_topic = orb_advertise(_orb_id, &brp);
_baro_topic = orb_advertise_multi(ORB_ID(sensor_baro), &brp,
&_orb_class_instance, (is_external()) ? ORB_PRIO_HIGH : ORB_PRIO_DEFAULT);
if (_baro_topic < 0) {
warnx("failed to create sensor_baro publication");
@ -725,7 +734,7 @@ MS5611::collect()
/* publish it */
if (!(_pub_blocked)) {
/* publish it */
orb_publish(_orb_id, _baro_topic, &report);
orb_publish(ORB_ID(sensor_baro), _baro_topic, &report);
}
if (_reports->force(&report)) {

10
src/drivers/px4fmu/fmu.cpp
View File

@ -145,7 +145,7 @@ private:
int _control_subs[actuator_controls_s::NUM_ACTUATOR_CONTROL_GROUPS];
actuator_controls_s _controls[actuator_controls_s::NUM_ACTUATOR_CONTROL_GROUPS];
orb_id_t _control_topics[actuator_controls_s::NUM_ACTUATOR_CONTROL_GROUPS];
orb_id_t _actuator_output_topic;
int _actuator_output_topic_instance;
pollfd _poll_fds[actuator_controls_s::NUM_ACTUATOR_CONTROL_GROUPS];
unsigned _poll_fds_num;
@ -260,7 +260,7 @@ PX4FMU::PX4FMU() :
_groups_required(0),
_groups_subscribed(0),
_control_subs{-1},
_actuator_output_topic(nullptr),
_actuator_output_topic_instance(-1),
_poll_fds_num(0),
_pwm_limit{},
_failsafe_pwm{0},
@ -331,8 +331,6 @@ PX4FMU::init()
log("default PWM output device");
}
_actuator_output_topic = ORB_ID_DOUBLE(actuator_outputs_, _class_instance);
/* reset GPIOs */
gpio_reset();
@ -683,10 +681,10 @@ PX4FMU::task_main()
/* publish mixed control outputs */
if (_outputs_pub < 0) {
_outputs_pub = orb_advertise(_actuator_output_topic, &outputs);
_outputs_pub = orb_advertise_multi(ORB_ID(actuator_outputs), &outputs, &_actuator_output_topic_instance, ORB_PRIO_DEFAULT);
} else {
orb_publish(_actuator_output_topic, _outputs_pub, &outputs);
orb_publish(ORB_ID(actuator_outputs), _outputs_pub, &outputs);
}
}
}

13
src/drivers/px4io/px4io.cpp
View File

@ -1685,17 +1685,12 @@ PX4IO::io_publish_pwm_outputs()
/* lazily advertise on first publication */
if (_to_outputs == 0) {
_to_outputs = orb_advertise((_primary_pwm_device ?
ORB_ID_VEHICLE_CONTROLS :
ORB_ID(actuator_outputs_1)),
&outputs);
int instance;
_to_outputs = orb_advertise_multi(ORB_ID(actuator_outputs),
&outputs, &instance, ORB_PRIO_MAX);
} else {
orb_publish((_primary_pwm_device ?
ORB_ID_VEHICLE_CONTROLS :
ORB_ID(actuator_outputs_1)),
_to_outputs,
&outputs);
orb_publish(ORB_ID(actuator_outputs), _to_outputs, &outputs);
}
return OK;

18
src/examples/matlab_csv_serial/matlab_csv_serial.c
View File

@ -1,6 +1,6 @@
/****************************************************************************
*
* Copyright (c) 2014 PX4 Development Team. All rights reserved.
* Copyright (c) 2014-2015 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
@ -190,10 +190,10 @@ int matlab_csv_serial_thread_main(int argc, char *argv[])
struct gyro_report gyro1;
/* subscribe to parameter changes */
int accel0_sub = orb_subscribe(ORB_ID(sensor_accel0));
int accel1_sub = orb_subscribe(ORB_ID(sensor_accel1));
int gyro0_sub = orb_subscribe(ORB_ID(sensor_gyro0));
int gyro1_sub = orb_subscribe(ORB_ID(sensor_gyro1));
int accel0_sub = orb_subscribe_multi(ORB_ID(sensor_accel), 0);
int accel1_sub = orb_subscribe_multi(ORB_ID(sensor_accel), 1);
int gyro0_sub = orb_subscribe_multi(ORB_ID(sensor_gyro), 0);
int gyro1_sub = orb_subscribe_multi(ORB_ID(sensor_gyro), 1);
thread_running = true;
@ -224,10 +224,10 @@ int matlab_csv_serial_thread_main(int argc, char *argv[])
/* accel0 update available? */
if (fds[0].revents & POLLIN)
{
orb_copy(ORB_ID(sensor_accel0), accel0_sub, &accel0);
orb_copy(ORB_ID(sensor_accel1), accel1_sub, &accel1);
orb_copy(ORB_ID(sensor_gyro0), gyro0_sub, &gyro0);
orb_copy(ORB_ID(sensor_gyro1), gyro1_sub, &gyro1);
orb_copy(ORB_ID(sensor_accel), accel0_sub, &accel0);
orb_copy(ORB_ID(sensor_accel), accel1_sub, &accel1);
orb_copy(ORB_ID(sensor_gyro), gyro0_sub, &gyro0);
orb_copy(ORB_ID(sensor_gyro), gyro1_sub, &gyro1);
// write out on accel 0, but collect for all other sensors as they have updates
dprintf(serial_fd, "%llu,%d,%d,%d,%d,%d,%d\n", accel0.timestamp, (int)accel0.x_raw, (int)accel0.y_raw, (int)accel0.z_raw,

8
src/modules/commander/accelerometer_calibration.cpp
View File

@ -159,6 +159,7 @@ int calculate_calibration_values(float accel_ref[6][3], float accel_T[3][3], flo
int do_accel_calibration(int mavlink_fd)
{
int fd;
int32_t device_id;
mavlink_log_info(mavlink_fd, CAL_STARTED_MSG, sensor_name);
@ -180,6 +181,9 @@ int do_accel_calibration(int mavlink_fd)
/* reset all offsets to zero and all scales to one */
fd = open(ACCEL_DEVICE_PATH, 0);
device_id = ioctl(fd, DEVIOCGDEVICEID, 0);
res = ioctl(fd, ACCELIOCSSCALE, (long unsigned int)&accel_scale);
close(fd);
@ -226,6 +230,10 @@ int do_accel_calibration(int mavlink_fd)
mavlink_log_critical(mavlink_fd, CAL_FAILED_SET_PARAMS_MSG);
res = ERROR;
}
if (param_set(param_find("SENS_ACC_ID"), &(device_id))) {
res = ERROR;
}
}
if (res == OK) {

11
src/modules/commander/gyro_calibration.cpp
View File

@ -62,6 +62,7 @@ static const char *sensor_name = "gyro";
int do_gyro_calibration(int mavlink_fd)
{
int32_t device_id;
mavlink_log_info(mavlink_fd, CAL_STARTED_MSG, sensor_name);
mavlink_log_info(mavlink_fd, "HOLD STILL");
@ -81,6 +82,9 @@ int do_gyro_calibration(int mavlink_fd)
/* reset all offsets to zero and all scales to one */
int fd = open(GYRO_DEVICE_PATH, 0);
device_id = ioctl(fd, DEVIOCGDEVICEID, 0);
res = ioctl(fd, GYROIOCSSCALE, (long unsigned int)&gyro_scale);
close(fd);
@ -95,7 +99,7 @@ int do_gyro_calibration(int mavlink_fd)
unsigned poll_errcount = 0;
/* subscribe to gyro sensor topic */
int sub_sensor_gyro = orb_subscribe(ORB_ID(sensor_gyro0));
int sub_sensor_gyro = orb_subscribe_multi(ORB_ID(sensor_gyro), 0);
struct gyro_report gyro_report;
while (calibration_counter < calibration_count) {
@ -107,7 +111,7 @@ int do_gyro_calibration(int mavlink_fd)
int poll_ret = poll(fds, 1, 1000);
if (poll_ret > 0) {
orb_copy(ORB_ID(sensor_gyro0), sub_sensor_gyro, &gyro_report);
orb_copy(ORB_ID(sensor_gyro), sub_sensor_gyro, &gyro_report);
gyro_scale.x_offset += gyro_report.x;
gyro_scale.y_offset += gyro_report.y;
gyro_scale.z_offset += gyro_report.z;
@ -277,6 +281,9 @@ int do_gyro_calibration(int mavlink_fd)
mavlink_log_critical(mavlink_fd, "ERROR: failed to set scale params");
res = ERROR;
}
if (param_set(param_find("SENS_GYRO_ID"), &(device_id))) {
res = ERROR;
}
}
if (res == OK) {

4
src/modules/commander/mag_calibration.cpp
View File

@ -149,7 +149,7 @@ int do_mag_calibration(int mavlink_fd)
}
if (res == OK) {
int sub_mag = orb_subscribe(ORB_ID(sensor_mag0));
int sub_mag = orb_subscribe_multi(ORB_ID(sensor_mag), 0);
if (sub_mag < 0) {
mavlink_log_critical(mavlink_fd, "No mag found, abort");
@ -179,7 +179,7 @@ int do_mag_calibration(int mavlink_fd)
int poll_ret = poll(fds, 1, 1000);
if (poll_ret > 0) {
orb_copy(ORB_ID(sensor_mag0), sub_mag, &mag);
orb_copy(ORB_ID(sensor_mag), sub_mag, &mag);
x[calibration_counter] = mag.x;
y[calibration_counter] = mag.y;

6
src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp
View File

@ -720,7 +720,7 @@ FixedwingEstimator::task_main()
* do subscriptions
*/
_distance_sub = orb_subscribe(ORB_ID(sensor_range_finder));
_baro_sub = orb_subscribe(ORB_ID(sensor_baro0));
_baro_sub = orb_subscribe_multi(ORB_ID(sensor_baro), 0);
_airspeed_sub = orb_subscribe(ORB_ID(airspeed));
_gps_sub = orb_subscribe(ORB_ID(vehicle_gps_position));
_vstatus_sub = orb_subscribe(ORB_ID(vehicle_status));
@ -1088,7 +1088,7 @@ FixedwingEstimator::task_main()
if (baro_updated) {
orb_copy(ORB_ID(sensor_baro0), _baro_sub, &_baro);
orb_copy(ORB_ID(sensor_baro), _baro_sub, &_baro);
float baro_elapsed = (_baro.timestamp - baro_last) / 1e6f;
baro_last = _baro.timestamp;
@ -1217,7 +1217,7 @@ FixedwingEstimator::task_main()
initVelNED[2] = _gps.vel_d_m_s;
// Set up height correctly
orb_copy(ORB_ID(sensor_baro0), _baro_sub, &_baro);
orb_copy(ORB_ID(sensor_baro), _baro_sub, &_baro);
_baro_ref_offset = _ekf->states[9]; // this should become zero in the local frame
// init filtered gps and baro altitudes

4
src/modules/fw_att_control/fw_att_control_main.cpp
View File

@ -554,7 +554,7 @@ FixedwingAttitudeControl::vehicle_accel_poll()
orb_check(_accel_sub, &accel_updated);
if (accel_updated) {
orb_copy(ORB_ID(sensor_accel0), _accel_sub, &_accel);
orb_copy(ORB_ID(sensor_accel), _accel_sub, &_accel);
}
}
@ -624,7 +624,7 @@ FixedwingAttitudeControl::task_main()
*/
_att_sp_sub = orb_subscribe(ORB_ID(vehicle_attitude_setpoint));
_att_sub = orb_subscribe(ORB_ID(vehicle_attitude));
_accel_sub = orb_subscribe(ORB_ID(sensor_accel0));
_accel_sub = orb_subscribe_multi(ORB_ID(sensor_accel), 0);
_airspeed_sub = orb_subscribe(ORB_ID(airspeed));
_vcontrol_mode_sub = orb_subscribe(ORB_ID(vehicle_control_mode));
_params_sub = orb_subscribe(ORB_ID(parameter_update));

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

@ -904,20 +904,20 @@ Mavlink::send_statustext(unsigned char severity, const char *string)
mavlink_logbuffer_write(&_logbuffer, &logmsg);
}
MavlinkOrbSubscription *Mavlink::add_orb_subscription(const orb_id_t topic)
MavlinkOrbSubscription *Mavlink::add_orb_subscription(const orb_id_t topic, int instance)
{
/* check if already subscribed to this topic */
MavlinkOrbSubscription *sub;
LL_FOREACH(_subscriptions, sub) {
if (sub->get_topic() == topic) {
if (sub->get_topic() == topic && sub->get_instance() == instance) {
/* already subscribed */
return sub;
}
}
/* add new subscription */
MavlinkOrbSubscription *sub_new = new MavlinkOrbSubscription(topic);
MavlinkOrbSubscription *sub_new = new MavlinkOrbSubscription(topic, instance);
LL_APPEND(_subscriptions, sub_new);

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

@ -171,7 +171,7 @@ public:
void handle_message(const mavlink_message_t *msg);
MavlinkOrbSubscription *add_orb_subscription(const orb_id_t topic);
MavlinkOrbSubscription *add_orb_subscription(const orb_id_t topic, int instance=0);
int get_instance_id();

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

@ -1342,14 +1342,7 @@ protected:
_act_sub(nullptr),
_act_time(0)
{
orb_id_t act_topics[] = {
ORB_ID(actuator_outputs_0),
ORB_ID(actuator_outputs_1),
ORB_ID(actuator_outputs_2),
ORB_ID(actuator_outputs_3)
};
_act_sub = _mavlink->add_orb_subscription(act_topics[N]);
_act_sub = _mavlink->add_orb_subscription(ORB_ID(actuator_outputs), N);
}
void send(const hrt_abstime t)
@ -1424,7 +1417,7 @@ protected:
_status_time(0),
_pos_sp_triplet_sub(_mavlink->add_orb_subscription(ORB_ID(position_setpoint_triplet))),
_pos_sp_triplet_time(0),
_act_sub(_mavlink->add_orb_subscription(ORB_ID(actuator_outputs_0))),
_act_sub(_mavlink->add_orb_subscription(ORB_ID(actuator_outputs))),
_act_time(0)
{}

11
src/modules/mavlink/mavlink_orb_subscription.cpp
View File

@ -46,10 +46,11 @@
#include "mavlink_orb_subscription.h"
MavlinkOrbSubscription::MavlinkOrbSubscription(const orb_id_t topic) :
MavlinkOrbSubscription::MavlinkOrbSubscription(const orb_id_t topic, int instance) :
next(nullptr),
_topic(topic),
_fd(orb_subscribe(_topic)),
_instance(instance),
_fd(orb_subscribe_multi(_topic, instance)),
_published(false)
{
}
@ -65,6 +66,12 @@ MavlinkOrbSubscription::get_topic() const
return _topic;
}
int
MavlinkOrbSubscription::get_instance() const
{
return _instance;
}
bool
MavlinkOrbSubscription::update(uint64_t *time, void* data)
{

4
src/modules/mavlink/mavlink_orb_subscription.h
View File

@ -50,7 +50,7 @@ class MavlinkOrbSubscription
public:
MavlinkOrbSubscription *next; ///< pointer to next subscription in list
MavlinkOrbSubscription(const orb_id_t topic);
MavlinkOrbSubscription(const orb_id_t topic, int instance);
~MavlinkOrbSubscription();
/**
@ -77,9 +77,11 @@ public:
*/
bool is_published();
orb_id_t get_topic() const;
int get_instance() const;
private:
const orb_id_t _topic; ///< topic metadata
const int _instance; ///< get topic instance
int _fd; ///< subscription handle
bool _published; ///< topic was ever published

21
src/modules/mavlink/mavlink_receiver.cpp
View File

@ -1043,10 +1043,10 @@ MavlinkReceiver::handle_message_hil_sensor(mavlink_message_t *msg)
gyro.temperature = imu.temperature;
if (_gyro_pub < 0) {
_gyro_pub = orb_advertise(ORB_ID(sensor_gyro0), &gyro);
_gyro_pub = orb_advertise(ORB_ID(sensor_gyro), &gyro);
} else {
orb_publish(ORB_ID(sensor_gyro0), _gyro_pub, &gyro);
orb_publish(ORB_ID(sensor_gyro), _gyro_pub, &gyro);
}
}
@ -1065,10 +1065,10 @@ MavlinkReceiver::handle_message_hil_sensor(mavlink_message_t *msg)
accel.temperature = imu.temperature;
if (_accel_pub < 0) {
_accel_pub = orb_advertise(ORB_ID(sensor_accel0), &accel);
_accel_pub = orb_advertise(ORB_ID(sensor_accel), &accel);
} else {
orb_publish(ORB_ID(sensor_accel0), _accel_pub, &accel);
orb_publish(ORB_ID(sensor_accel), _accel_pub, &accel);
}
}
@ -1086,10 +1086,11 @@ MavlinkReceiver::handle_message_hil_sensor(mavlink_message_t *msg)
mag.z = imu.zmag;
if (_mag_pub < 0) {
_mag_pub = orb_advertise(ORB_ID(sensor_mag0), &mag);
/* publish to the first mag topic */
_mag_pub = orb_advertise(ORB_ID(sensor_mag), &mag);
} else {
orb_publish(ORB_ID(sensor_mag0), _mag_pub, &mag);
orb_publish(ORB_ID(sensor_mag), _mag_pub, &mag);
}
}
@ -1104,10 +1105,10 @@ MavlinkReceiver::handle_message_hil_sensor(mavlink_message_t *msg)
baro.temperature = imu.temperature;
if (_baro_pub < 0) {
_baro_pub = orb_advertise(ORB_ID(sensor_baro0), &baro);
_baro_pub = orb_advertise(ORB_ID(sensor_baro), &baro);
} else {
orb_publish(ORB_ID(sensor_baro0), _baro_pub, &baro);
orb_publish(ORB_ID(sensor_baro), _baro_pub, &baro);
}
}
@ -1394,10 +1395,10 @@ MavlinkReceiver::handle_message_hil_state_quaternion(mavlink_message_t *msg)
accel.temperature = 25.0f;
if (_accel_pub < 0) {
_accel_pub = orb_advertise(ORB_ID(sensor_accel0), &accel);
_accel_pub = orb_advertise(ORB_ID(sensor_accel), &accel);
} else {
orb_publish(ORB_ID(sensor_accel0), _accel_pub, &accel);
orb_publish(ORB_ID(sensor_accel), _accel_pub, &accel);
}
}

72
src/modules/navigator/mission.cpp
View File

@ -39,6 +39,7 @@
* @author Thomas Gubler <thomasgubler@gmail.com>
* @author Anton Babushkin <anton.babushkin@me.com>
* @author Ban Siesta <bansiesta@gmail.com>
* @author Simon Wilks <simon@uaventure.com>
*/
#include <sys/types.h>
@ -68,6 +69,7 @@ Mission::Mission(Navigator *navigator, const char *name) :
_param_takeoff_alt(this, "MIS_TAKEOFF_ALT", false),
_param_dist_1wp(this, "MIS_DIST_1WP", false),
_param_altmode(this, "MIS_ALTMODE", false),
_param_yawmode(this, "MIS_YAWMODE", false),
_onboard_mission({0}),
_offboard_mission({0}),
_current_onboard_mission_index(-1),
@ -80,6 +82,7 @@ Mission::Mission(Navigator *navigator, const char *name) :
_missionFeasiblityChecker(),
_min_current_sp_distance_xy(FLT_MAX),
_mission_item_previous_alt(NAN),
_on_arrival_yaw(NAN),
_distance_current_previous(0.0f)
{
/* load initial params */
@ -166,6 +169,13 @@ Mission::on_active()
_navigator->set_can_loiter_at_sp(true);
}
}
/* see if we need to update the current yaw heading for rotary wing types */
if (_navigator->get_vstatus()->is_rotary_wing
&& _param_yawmode.get() != MISSION_YAWMODE_NONE
&& _mission_type != MISSION_TYPE_NONE) {
heading_sp_update();
}
}
void
@ -275,7 +285,7 @@ Mission::check_dist_1wp()
&mission_item, sizeof(mission_item_s)) == sizeof(mission_item_s)) {
/* check only items with valid lat/lon */
if ( mission_item.nav_cmd == NAV_CMD_WAYPOINT ||
if ( mission_item.nav_cmd == NAV_CMD_WAYPOINT ||
mission_item.nav_cmd == NAV_CMD_LOITER_TIME_LIMIT ||
mission_item.nav_cmd == NAV_CMD_LOITER_TURN_COUNT ||
mission_item.nav_cmd == NAV_CMD_LOITER_UNLIMITED ||
@ -362,7 +372,6 @@ Mission::set_mission_items()
mavlink_log_critical(_navigator->get_mavlink_fd(), "offboard mission now running");
}
_mission_type = MISSION_TYPE_OFFBOARD;
} else {
/* no mission available or mission finished, switch to loiter */
if (_mission_type != MISSION_TYPE_NONE) {
@ -396,6 +405,10 @@ Mission::set_mission_items()
return;
}
if (pos_sp_triplet->current.valid) {
_on_arrival_yaw = _mission_item.yaw;
}
/* do takeoff on first waypoint for rotary wing vehicles */
if (_navigator->get_vstatus()->is_rotary_wing) {
/* force takeoff if landed (additional protection) */
@ -442,6 +455,7 @@ Mission::set_mission_items()
_mission_item.nav_cmd = NAV_CMD_TAKEOFF;
_mission_item.lat = _navigator->get_global_position()->lat;
_mission_item.lon = _navigator->get_global_position()->lon;
_mission_item.yaw = NAN;
_mission_item.altitude = takeoff_alt;
_mission_item.altitude_is_relative = false;
_mission_item.autocontinue = true;
@ -481,7 +495,6 @@ Mission::set_mission_items()
if (read_mission_item(_mission_type == MISSION_TYPE_ONBOARD, false, &mission_item_next)) {
/* got next mission item, update setpoint triplet */
mission_item_to_position_setpoint(&mission_item_next, &pos_sp_triplet->next);
} else {
/* next mission item is not available */
pos_sp_triplet->next.valid = false;
@ -503,6 +516,59 @@ Mission::set_mission_items()
_navigator->set_position_setpoint_triplet_updated();
}
void
Mission::heading_sp_update()
{
if (_takeoff) {
/* we don't want to be yawing during takeoff */
return;
}
struct position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet();
/* Don't change setpoint if last and current waypoint are not valid */
if (!pos_sp_triplet->previous.valid || !pos_sp_triplet->current.valid ||
!isfinite(_on_arrival_yaw)) {
return;
}
/* Don't do FOH for landing and takeoff waypoints, the ground may be near
* and the FW controller has a custom landing logic */
if (_mission_item.nav_cmd == NAV_CMD_LAND || _mission_item.nav_cmd == NAV_CMD_TAKEOFF) {
return;
}
/* set yaw angle for the waypoint iff a loiter time has been specified */
if (_waypoint_position_reached && _mission_item.time_inside > 0.0f) {
_mission_item.yaw = _on_arrival_yaw;
/* always keep the front of the rotary wing pointing to the next waypoint */
} else if (_param_yawmode.get() == MISSION_YAWMODE_FRONT_TO_WAYPOINT) {
_mission_item.yaw = get_bearing_to_next_waypoint(
_navigator->get_global_position()->lat,
_navigator->get_global_position()->lon,
_mission_item.lat,
_mission_item.lon);
/* always keep the back of the rotary wing pointing towards home */
} else if (_param_yawmode.get() == MISSION_YAWMODE_FRONT_TO_HOME) {
_mission_item.yaw = get_bearing_to_next_waypoint(
_navigator->get_global_position()->lat,
_navigator->get_global_position()->lon,
_navigator->get_home_position()->lat,
_navigator->get_home_position()->lon);
/* always keep the back of the rotary wing pointing towards home */
} else if (_param_yawmode.get() == MISSION_YAWMODE_BACK_TO_HOME) {
_mission_item.yaw = _wrap_pi(get_bearing_to_next_waypoint(
_navigator->get_global_position()->lat,
_navigator->get_global_position()->lon,
_navigator->get_home_position()->lat,
_navigator->get_home_position()->lon) + M_PI_F);
}
mission_item_to_position_setpoint(&_mission_item, &pos_sp_triplet->current);
_navigator->set_position_setpoint_triplet_updated();
}
void
Mission::altitude_sp_foh_update()
{

16
src/modules/navigator/mission.h
View File

@ -83,6 +83,13 @@ public:
MISSION_ALTMODE_FOH = 1
};
enum mission_yaw_mode {
MISSION_YAWMODE_NONE = 0,
MISSION_YAWMODE_FRONT_TO_WAYPOINT = 1,
MISSION_YAWMODE_FRONT_TO_HOME = 2,
MISSION_YAWMODE_BACK_TO_HOME = 3
};
private:
/**
* Update onboard mission topic
@ -110,6 +117,11 @@ private:
*/
void set_mission_items();
/**
* Updates the heading of the vehicle. Rotary wings only.
*/
void heading_sp_update();
/**
* Updates the altitude sp to follow a foh
*/
@ -155,6 +167,7 @@ private:
control::BlockParamFloat _param_takeoff_alt;
control::BlockParamFloat _param_dist_1wp;
control::BlockParamInt _param_altmode;
control::BlockParamInt _param_yawmode;
struct mission_s _onboard_mission;
struct mission_s _offboard_mission;
@ -177,7 +190,8 @@ private:
float _min_current_sp_distance_xy; /**< minimum distance which was achieved to the current waypoint */
float _mission_item_previous_alt; /**< holds the altitude of the previous mission item,
can be replaced by a full copy of the previous mission item if needed*/
can be replaced by a full copy of the previous mission item if needed */
float _on_arrival_yaw; /**< holds the yaw value that should be applied when the current waypoint is reached */
float _distance_current_previous; /**< distance from previous to current sp in pos_sp_triplet,
only use if current and previous are valid */
};

3
src/modules/navigator/mission_block.cpp
View File

@ -134,6 +134,7 @@ MissionBlock::is_mission_item_reached()
}
}
/* Check if the waypoint and the requested yaw setpoint. */
if (_waypoint_position_reached && !_waypoint_yaw_reached) {
/* TODO: removed takeoff, why? */
@ -151,7 +152,7 @@ MissionBlock::is_mission_item_reached()
}
}
/* check if the current waypoint was reached */
/* Once the waypoint and yaw setpoint have been reached we can start the loiter time countdown */
if (_waypoint_position_reached && _waypoint_yaw_reached) {
if (_time_first_inside_orbit == 0) {

16
src/modules/navigator/mission_params.c
View File

@ -95,3 +95,19 @@ PARAM_DEFINE_FLOAT(MIS_DIST_1WP, 500);
* @group Mission
*/
PARAM_DEFINE_INT32(MIS_ALTMODE, 0);
/**
* Multirotor only. Yaw setpoint mode.
*
* 0: Set the yaw heading to the yaw value specified for the destination waypoint.
* 1: Maintain a yaw heading pointing towards the next waypoint.
* 2: Maintain a yaw heading that always points to the home location.
* 3: Maintain a yaw heading that always points away from the home location (ie: back always faces home).
*
* The values are defined in the enum mission_altitude_mode
*
* @min 0
* @max 3
* @group Mission
*/
PARAM_DEFINE_INT32(MIS_YAWMODE, 0);

4
src/modules/sdlog2/sdlog2.c
View File

@ -1100,7 +1100,7 @@ int sdlog2_thread_main(int argc, char *argv[])
subs.att_sub = orb_subscribe(ORB_ID(vehicle_attitude));
subs.att_sp_sub = orb_subscribe(ORB_ID(vehicle_attitude_setpoint));
subs.rates_sp_sub = orb_subscribe(ORB_ID(vehicle_rates_setpoint));
subs.act_outputs_sub = orb_subscribe(ORB_ID_VEHICLE_CONTROLS);
subs.act_outputs_sub = orb_subscribe(ORB_ID(actuator_outputs));
subs.act_controls_sub = orb_subscribe(ORB_ID_VEHICLE_ATTITUDE_CONTROLS);
subs.act_controls_1_sub = orb_subscribe(ORB_ID(actuator_controls_1));
subs.local_pos_sub = orb_subscribe(ORB_ID(vehicle_local_position));
@ -1477,7 +1477,7 @@ int sdlog2_thread_main(int argc, char *argv[])
}
/* --- ACTUATOR OUTPUTS --- */
if (copy_if_updated(ORB_ID(actuator_outputs_0), subs.act_outputs_sub, &buf.act_outputs)) {
if (copy_if_updated(ORB_ID(actuator_outputs), subs.act_outputs_sub, &buf.act_outputs)) {
log_msg.msg_type = LOG_OUT0_MSG;
memcpy(log_msg.body.log_OUT0.output, buf.act_outputs.output, sizeof(log_msg.body.log_OUT0.output));
LOGBUFFER_WRITE_AND_COUNT(OUT0);

15
src/modules/sensors/sensor_params.c
View File

@ -44,6 +44,13 @@
#include <nuttx/config.h>
#include <systemlib/param/param.h>
/**
* ID of the Gyro that the calibration is for.
*
* @group Sensor Calibration
*/
PARAM_DEFINE_INT32(SENS_GYRO_ID, 0);
/**
* Gyro X-axis offset
*
@ -153,6 +160,12 @@ PARAM_DEFINE_FLOAT(SENS_MAG_YSCALE, 1.0f);
*/
PARAM_DEFINE_FLOAT(SENS_MAG_ZSCALE, 1.0f);
/**
* ID of the Accelerometer that the calibration is for.
*
* @group Sensor Calibration
*/
PARAM_DEFINE_INT32(SENS_ACC_ID, 0);
/**
* Accelerometer X-axis offset
@ -270,7 +283,7 @@ PARAM_DEFINE_INT32(SENS_BOARD_ROT, 0);
/**
* PX4Flow board rotation
*
* This parameter defines the rotation of the PX4FLOW board relative to the platform.
* This parameter defines the rotation of the PX4FLOW board relative to the platform.
* Zero rotation is defined as Y on flow board pointing towards front of vehicle
* Possible values are:
* 0 = No rotation

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

@ -1113,7 +1113,7 @@ Sensors::accel_poll(struct sensor_combined_s &raw)
if (accel_updated) {
struct accel_report accel_report;
orb_copy(ORB_ID(sensor_accel0), _accel_sub, &accel_report);
orb_copy(ORB_ID(sensor_accel), _accel_sub, &accel_report);
math::Vector<3> vect(accel_report.x, accel_report.y, accel_report.z);
vect = _board_rotation * vect;
@ -1134,7 +1134,7 @@ Sensors::accel_poll(struct sensor_combined_s &raw)
if (accel_updated) {
struct accel_report accel_report;
orb_copy(ORB_ID(sensor_accel1), _accel_sub, &accel_report);
orb_copy(ORB_ID(sensor_accel), _accel1_sub, &accel_report);
math::Vector<3> vect(accel_report.x, accel_report.y, accel_report.z);
vect = _board_rotation * vect;
@ -1155,7 +1155,7 @@ Sensors::accel_poll(struct sensor_combined_s &raw)
if (accel_updated) {
struct accel_report accel_report;
orb_copy(ORB_ID(sensor_accel2), _accel_sub, &accel_report);
orb_copy(ORB_ID(sensor_accel), _accel2_sub, &accel_report);
math::Vector<3> vect(accel_report.x, accel_report.y, accel_report.z);
vect = _board_rotation * vect;
@ -1181,7 +1181,7 @@ Sensors::gyro_poll(struct sensor_combined_s &raw)
if (gyro_updated) {
struct gyro_report gyro_report;
orb_copy(ORB_ID(sensor_gyro0), _gyro_sub, &gyro_report);
orb_copy(ORB_ID(sensor_gyro), _gyro_sub, &gyro_report);
math::Vector<3> vect(gyro_report.x, gyro_report.y, gyro_report.z);
vect = _board_rotation * vect;
@ -1202,7 +1202,7 @@ Sensors::gyro_poll(struct sensor_combined_s &raw)
if (gyro_updated) {
struct gyro_report gyro_report;
orb_copy(ORB_ID(sensor_gyro1), _gyro1_sub, &gyro_report);
orb_copy(ORB_ID(sensor_gyro), _gyro1_sub, &gyro_report);
math::Vector<3> vect(gyro_report.x, gyro_report.y, gyro_report.z);
vect = _board_rotation * vect;
@ -1223,7 +1223,7 @@ Sensors::gyro_poll(struct sensor_combined_s &raw)
if (gyro_updated) {
struct gyro_report gyro_report;
orb_copy(ORB_ID(sensor_gyro2), _gyro_sub, &gyro_report);
orb_copy(ORB_ID(sensor_gyro), _gyro2_sub, &gyro_report);
math::Vector<3> vect(gyro_report.x, gyro_report.y, gyro_report.z);
vect = _board_rotation * vect;
@ -1249,7 +1249,7 @@ Sensors::mag_poll(struct sensor_combined_s &raw)
if (mag_updated) {
struct mag_report mag_report;
orb_copy(ORB_ID(sensor_mag0), _mag_sub, &mag_report);
orb_copy(ORB_ID(sensor_mag), _mag_sub, &mag_report);
math::Vector<3> vect(mag_report.x, mag_report.y, mag_report.z);
@ -1278,7 +1278,7 @@ Sensors::mag_poll(struct sensor_combined_s &raw)
if (mag_updated) {
struct mag_report mag_report;
orb_copy(ORB_ID(sensor_mag1), _mag1_sub, &mag_report);
orb_copy(ORB_ID(sensor_mag), _mag1_sub, &mag_report);
raw.magnetometer1_raw[0] = mag_report.x_raw;
raw.magnetometer1_raw[1] = mag_report.y_raw;
@ -1292,7 +1292,7 @@ Sensors::mag_poll(struct sensor_combined_s &raw)
if (mag_updated) {
struct mag_report mag_report;
orb_copy(ORB_ID(sensor_mag2), _mag2_sub, &mag_report);
orb_copy(ORB_ID(sensor_mag), _mag2_sub, &mag_report);
raw.magnetometer2_raw[0] = mag_report.x_raw;
raw.magnetometer2_raw[1] = mag_report.y_raw;
@ -1310,7 +1310,7 @@ Sensors::baro_poll(struct sensor_combined_s &raw)
if (baro_updated) {
orb_copy(ORB_ID(sensor_baro0), _baro_sub, &_barometer);
orb_copy(ORB_ID(sensor_baro), _baro_sub, &_barometer);
raw.baro_pres_mbar = _barometer.pressure; // Pressure in mbar
raw.baro_alt_meter = _barometer.altitude; // Altitude in meters
@ -1325,7 +1325,7 @@ Sensors::baro_poll(struct sensor_combined_s &raw)
struct baro_report baro_report;
orb_copy(ORB_ID(sensor_baro1), _baro1_sub, &baro_report);
orb_copy(ORB_ID(sensor_baro), _baro1_sub, &baro_report);
raw.baro1_pres_mbar = baro_report.pressure; // Pressure in mbar
raw.baro1_alt_meter = baro_report.altitude; // Altitude in meters
@ -1943,18 +1943,18 @@ Sensors::task_main()
/*
* do subscriptions
*/
_gyro_sub = orb_subscribe(ORB_ID(sensor_gyro0));
_accel_sub = orb_subscribe(ORB_ID(sensor_accel0));
_mag_sub = orb_subscribe(ORB_ID(sensor_mag0));
_gyro1_sub = orb_subscribe(ORB_ID(sensor_gyro1));
_accel1_sub = orb_subscribe(ORB_ID(sensor_accel1));
_mag1_sub = orb_subscribe(ORB_ID(sensor_mag1));
_gyro2_sub = orb_subscribe(ORB_ID(sensor_gyro2));
_accel2_sub = orb_subscribe(ORB_ID(sensor_accel2));
_mag2_sub = orb_subscribe(ORB_ID(sensor_mag2));
_gyro_sub = orb_subscribe_multi(ORB_ID(sensor_gyro), 0);
_accel_sub = orb_subscribe_multi(ORB_ID(sensor_accel), 0);
_mag_sub = orb_subscribe_multi(ORB_ID(sensor_mag), 0);
_gyro1_sub = orb_subscribe_multi(ORB_ID(sensor_gyro), 1);
_accel1_sub = orb_subscribe_multi(ORB_ID(sensor_accel), 1);
_mag1_sub = orb_subscribe_multi(ORB_ID(sensor_mag), 1);
_gyro2_sub = orb_subscribe_multi(ORB_ID(sensor_gyro), 2);
_accel2_sub = orb_subscribe_multi(ORB_ID(sensor_accel), 2);
_mag2_sub = orb_subscribe_multi(ORB_ID(sensor_mag), 2);
_rc_sub = orb_subscribe(ORB_ID(input_rc));
_baro_sub = orb_subscribe(ORB_ID(sensor_baro0));
_baro1_sub = orb_subscribe(ORB_ID(sensor_baro1));
_baro_sub = orb_subscribe_multi(ORB_ID(sensor_baro), 0);
_baro1_sub = orb_subscribe_multi(ORB_ID(sensor_baro), 1);
_diff_pres_sub = orb_subscribe(ORB_ID(differential_pressure));
_vcontrol_mode_sub = orb_subscribe(ORB_ID(vehicle_control_mode));
_params_sub = orb_subscribe(ORB_ID(parameter_update));

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

@ -1,6 +1,6 @@
/****************************************************************************
*
* Copyright (C) 2012 PX4 Development Team. All rights reserved.
* Copyright (c) 2012-2015 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

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

@ -1,6 +1,6 @@
/****************************************************************************
*
* Copyright (C) 2012 PX4 Development Team. All rights reserved.
* Copyright (c) 2012-2015 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

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

@ -1,6 +1,6 @@
/****************************************************************************
*
* Copyright (C) 2012 PX4 Development Team. All rights reserved.
* Copyright (c) 2012-2015 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

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

@ -1,6 +1,6 @@
/****************************************************************************
*
* Copyright (C) 2012 PX4 Development Team. All rights reserved.
* Copyright (c) 2012-2015 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

5
src/modules/uORB/module.mk
View File

@ -1,6 +1,6 @@
############################################################################
#
# Copyright (c) 2012, 2013 PX4 Development Team. All rights reserved.
# Copyright (c) 2012-2015 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
@ -37,8 +37,7 @@
MODULE_COMMAND = uorb
# XXX probably excessive, 2048 should be sufficient
MODULE_STACKSIZE = 4096
MODULE_STACKSIZE = 2048
SRCS = uORB.cpp \
objects_common.cpp \

23
src/modules/uORB/objects_common.cpp
View File

@ -1,6 +1,6 @@
/****************************************************************************
*
* Copyright (C) 2012-2014 PX4 Development Team. All rights reserved.
* Copyright (c) 2012-2015 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
@ -46,24 +46,16 @@
#include <drivers/drv_orb_dev.h>
#include <drivers/drv_mag.h>
ORB_DEFINE(sensor_mag0, struct mag_report);
ORB_DEFINE(sensor_mag1, struct mag_report);
ORB_DEFINE(sensor_mag2, struct mag_report);
ORB_DEFINE(sensor_mag, struct mag_report);
#include <drivers/drv_accel.h>
ORB_DEFINE(sensor_accel0, struct accel_report);
ORB_DEFINE(sensor_accel1, struct accel_report);
ORB_DEFINE(sensor_accel2, struct accel_report);
ORB_DEFINE(sensor_accel, struct accel_report);
#include <drivers/drv_gyro.h>
ORB_DEFINE(sensor_gyro0, struct gyro_report);
ORB_DEFINE(sensor_gyro1, struct gyro_report);
ORB_DEFINE(sensor_gyro2, struct gyro_report);
ORB_DEFINE(sensor_gyro, struct gyro_report);
#include <drivers/drv_baro.h>
ORB_DEFINE(sensor_baro0, struct baro_report);
ORB_DEFINE(sensor_baro1, struct baro_report);
ORB_DEFINE(sensor_baro2, struct baro_report);
ORB_DEFINE(sensor_baro, struct baro_report);
#include <drivers/drv_range_finder.h>
ORB_DEFINE(sensor_range_finder, struct range_finder_report);
@ -212,10 +204,7 @@ ORB_DEFINE(actuator_controls_virtual_fw, struct actuator_controls_virtual_fw_s);
ORB_DEFINE(actuator_armed, struct actuator_armed_s);
#include "topics/actuator_outputs.h"
ORB_DEFINE(actuator_outputs_0, struct actuator_outputs_s);
ORB_DEFINE(actuator_outputs_1, struct actuator_outputs_s);
ORB_DEFINE(actuator_outputs_2, struct actuator_outputs_s);
ORB_DEFINE(actuator_outputs_3, struct actuator_outputs_s);
ORB_DEFINE(actuator_outputs, struct actuator_outputs_s);
#include "topics/actuator_direct.h"
ORB_DEFINE(actuator_direct, struct actuator_direct_s);

10
src/modules/uORB/topics/actuator_outputs.h
View File

@ -68,12 +68,6 @@ struct actuator_outputs_s {
*/
/* actuator output sets; this list can be expanded as more drivers emerge */
ORB_DECLARE(actuator_outputs_0);
ORB_DECLARE(actuator_outputs_1);
ORB_DECLARE(actuator_outputs_2);
ORB_DECLARE(actuator_outputs_3);
ORB_DECLARE(actuator_outputs);
/* output sets with pre-defined applications */
#define ORB_ID_VEHICLE_CONTROLS ORB_ID(actuator_outputs_0)
#endif
#endif

298
src/modules/uORB/uORB.cpp
View File

@ -1,6 +1,6 @@
/****************************************************************************
*
* Copyright (Cc) 2012-2015 PX4 Development Team. All rights reserved.
* Copyright (c) 2012-2015 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
@ -51,6 +51,7 @@
#include <stdio.h>
#include <math.h>
#include <unistd.h>
#include <systemlib/err.h>
#include <nuttx/arch.h>
#include <nuttx/wqueue.h>
@ -68,6 +69,7 @@
namespace
{
/* internal use only */
static const unsigned orb_maxpath = 64;
/* oddly, ERROR is not defined for c++ */
@ -81,17 +83,30 @@ enum Flavor {
PARAM
};
struct orb_advertdata {
const struct orb_metadata *meta;
int *instance;
int priority;
};
int
node_mkpath(char *buf, Flavor f, const struct orb_metadata *meta)
node_mkpath(char *buf, Flavor f, const struct orb_metadata *meta, int *instance = nullptr)
{
unsigned len;
len = snprintf(buf, orb_maxpath, "/%s/%s",
(f == PUBSUB) ? "obj" : "param",
meta->o_name);
unsigned index = 0;
if (len >= orb_maxpath)
if (instance != nullptr) {
index = *instance;
}
len = snprintf(buf, orb_maxpath, "/%s/%s%d",
(f == PUBSUB) ? "obj" : "param",
meta->o_name, index);
if (len >= orb_maxpath) {
return -ENAMETOOLONG;
}
return OK;
}
@ -104,7 +119,7 @@ node_mkpath(char *buf, Flavor f, const struct orb_metadata *meta)
class ORBDevNode : public device::CDev
{
public:
ORBDevNode(const struct orb_metadata *meta, const char *name, const char *path);
ORBDevNode(const struct orb_metadata *meta, const char *name, const char *path, int priority);
~ORBDevNode();
virtual int open(struct file *filp);
@ -126,6 +141,7 @@ private:
struct hrt_call update_call; /**< deferred wakeup call if update_period is nonzero */
void *poll_priv; /**< saved copy of fds->f_priv while poll is active */
bool update_reported; /**< true if we have reported the update via poll/check */
int priority; /**< priority of publisher */
};
const struct orb_metadata *_meta; /**< object metadata information */
@ -133,6 +149,7 @@ private:
hrt_abstime _last_update; /**< time the object was last updated */
volatile unsigned _generation; /**< object generation count */
pid_t _publisher; /**< if nonzero, current publisher */
const int _priority; /**< priority of topic */
SubscriberData *filp_to_sd(struct file *filp) {
SubscriberData *sd = (SubscriberData *)(filp->f_priv);
@ -160,13 +177,14 @@ private:
bool appears_updated(SubscriberData *sd);
};
ORBDevNode::ORBDevNode(const struct orb_metadata *meta, const char *name, const char *path) :
ORBDevNode::ORBDevNode(const struct orb_metadata *meta, const char *name, const char *path, int priority) :
CDev(name, path),
_meta(meta),
_data(nullptr),
_last_update(0),
_generation(0),
_publisher(0)
_publisher(0),
_priority(priority)
{
// enable debug() calls
_debug_enabled = true;
@ -176,6 +194,7 @@ ORBDevNode::~ORBDevNode()
{
if (_data != nullptr)
delete[] _data;
}
int
@ -225,6 +244,9 @@ ORBDevNode::open(struct file *filp)
/* default to no pending update */
sd->generation = _generation;
/* set priority */
sd->priority = _priority;
filp->f_priv = (void *)sd;
ret = CDev::open(filp);
@ -283,6 +305,9 @@ ORBDevNode::read(struct file *filp, char *buffer, size_t buflen)
/* track the last generation that the file has seen */
sd->generation = _generation;
/* set priority */
sd->priority = _priority;
/*
* Clear the flag that indicates that an update has been reported, as
* we have just collected it.
@ -364,6 +389,10 @@ ORBDevNode::ioctl(struct file *filp, int cmd, unsigned long arg)
*(uintptr_t *)arg = (uintptr_t)this;
return OK;
case ORBIOCGPRIORITY:
*(int *)arg = sd->priority;
return OK;
default:
/* give it to the superclass */
return CDev::ioctl(filp, cmd, arg);
@ -556,40 +585,85 @@ ORBDevMaster::ioctl(struct file *filp, int cmd, unsigned long arg)
switch (cmd) {
case ORBIOCADVERTISE: {
const struct orb_metadata *meta = (const struct orb_metadata *)arg;
const struct orb_advertdata *adv = (const struct orb_advertdata *)arg;
const struct orb_metadata *meta = adv->meta;
const char *objname;
const char *devpath;
char nodepath[orb_maxpath];
ORBDevNode *node;
/* construct a path to the node - this also checks the node name */
ret = node_mkpath(nodepath, _flavor, meta);
if (ret != OK)
return ret;
/* driver wants a permanent copy of the node name, so make one here */
objname = strdup(meta->o_name);
if (objname == nullptr)
return -ENOMEM;
/* construct the new node */
node = new ORBDevNode(meta, objname, nodepath);
/* initialise the node - this may fail if e.g. a node with this name already exists */
if (node != nullptr)
ret = node->init();
/* if we didn't get a device, that's bad */
if (node == nullptr)
return -ENOMEM;
/* if init failed, discard the node and its name */
if (ret != OK) {
delete node;
free((void *)objname);
/* set instance to zero - we could allow selective multi-pubs later based on value */
if (adv->instance != nullptr) {
*(adv->instance) = 0;
}
/* construct a path to the node - this also checks the node name */
ret = node_mkpath(nodepath, _flavor, meta, adv->instance);
if (ret != OK) {
return ret;
}
/* ensure that only one advertiser runs through this critical section */
lock();
ret = ERROR;
/* try for topic groups */
const unsigned max_group_tries = (adv->instance != nullptr) ? ORB_MULTI_MAX_INSTANCES : 1;
unsigned group_tries = 0;
do {
/* if path is modifyable change try index */
if (adv->instance != nullptr) {
/* replace the number at the end of the string */
nodepath[strlen(nodepath) - 1] = '0' + group_tries;
*(adv->instance) = group_tries;
}
/* driver wants a permanent copy of the node name, so make one here */
objname = strdup(meta->o_name);
if (objname == nullptr) {
return -ENOMEM;
}
/* driver wants a permanent copy of the path, so make one here */
devpath = strdup(nodepath);
if (devpath == nullptr) {
return -ENOMEM;
}
/* construct the new node */
node = new ORBDevNode(meta, objname, devpath, adv->priority);
/* if we didn't get a device, that's bad */
if (node == nullptr) {
unlock();
return -ENOMEM;
}
/* initialise the node - this may fail if e.g. a node with this name already exists */
ret = node->init();
/* if init failed, discard the node and its name */
if (ret != OK) {
delete node;
free((void *)objname);
free((void *)devpath);
}
group_tries++;
} while (ret != OK && (group_tries < max_group_tries));
if (group_tries > max_group_tries) {
ret = -ENOMEM;
}
/* the file handle for the driver has been created, unlock */
unlock();
return ret;
}
@ -614,6 +688,7 @@ struct orb_test {
};
ORB_DEFINE(orb_test, struct orb_test);
ORB_DEFINE(orb_multitest, struct orb_test);
int
test_fail(const char *fmt, ...)
@ -643,8 +718,6 @@ test_note(const char *fmt, ...)
return OK;
}
ORB_DECLARE(sensor_combined);
int
test()
{
@ -700,48 +773,65 @@ test()
orb_unsubscribe(sfd);
close(pfd);
#if 0
/* this is a hacky test that exploits the sensors app to test rate-limiting */
/* this routine tests the multi-topic support */
test_note("try multi-topic support");
sfd = orb_subscribe(ORB_ID(sensor_combined));
int instance0;
int pfd0 = orb_advertise_multi(ORB_ID(orb_multitest), &t, &instance0, ORB_PRIO_MAX);
hrt_abstime start, end;
unsigned count;
test_note("advertised");
usleep(300000);
start = hrt_absolute_time();
count = 0;
int instance1;
int pfd1 = orb_advertise_multi(ORB_ID(orb_multitest), &t, &instance1, ORB_PRIO_MIN);
do {
orb_check(sfd, &updated);
if (instance0 != 0)
return test_fail("mult. id0: %d", instance0);
if (updated) {
orb_copy(ORB_ID(sensor_combined), sfd, nullptr);
count++;
}
} while (count < 100);
if (instance1 != 1)
return test_fail("mult. id1: %d", instance1);
end = hrt_absolute_time();
test_note("full-speed, 100 updates in %llu", end - start);
t.val = 103;
if (OK != orb_publish(ORB_ID(orb_multitest), pfd0, &t))
return test_fail("mult. pub0 fail");
orb_set_interval(sfd, 10);
test_note("published");
usleep(300000);
start = hrt_absolute_time();
count = 0;
t.val = 203;
if (OK != orb_publish(ORB_ID(orb_multitest), pfd1, &t))
return test_fail("mult. pub1 fail");
do {
orb_check(sfd, &updated);
/* subscribe to both topics and ensure valid data is received */
int sfd0 = orb_subscribe_multi(ORB_ID(orb_multitest), 0);
if (updated) {
orb_copy(ORB_ID(sensor_combined), sfd, nullptr);
count++;
}
} while (count < 100);
if (OK != orb_copy(ORB_ID(orb_multitest), sfd0, &t))
return test_fail("sub #0 copy failed: %d", errno);
end = hrt_absolute_time();
test_note("100Hz, 100 updates in %llu", end - start);
if (t.val != 103)
return test_fail("sub #0 val. mismatch: %d", t.val);
orb_unsubscribe(sfd);
#endif
int sfd1 = orb_subscribe_multi(ORB_ID(orb_multitest), 1);
if (OK != orb_copy(ORB_ID(orb_multitest), sfd1, &t))
return test_fail("sub #1 copy failed: %d", errno);
if (t.val != 203)
return test_fail("sub #1 val. mismatch: %d", t.val);
/* test priorities */
int prio;
if (OK != orb_priority(sfd0, &prio))
return test_fail("prio #0");
if (prio != ORB_PRIO_MAX)
return test_fail("prio: %d", prio);
if (OK != orb_priority(sfd1, &prio))
return test_fail("prio #1");
if (prio != ORB_PRIO_MIN)
return test_fail("prio: %d", prio);
return test_note("PASS");
}
@ -771,7 +861,7 @@ uorb_main(int argc, char *argv[])
if (!strcmp(argv[1], "start")) {
if (g_dev != nullptr) {
fprintf(stderr, "[uorb] already loaded\n");
warnx("already loaded");
/* user wanted to start uorb, its already running, no error */
return 0;
}
@ -780,18 +870,17 @@ uorb_main(int argc, char *argv[])
g_dev = new ORBDevMaster(PUBSUB);
if (g_dev == nullptr) {
fprintf(stderr, "[uorb] driver alloc failed\n");
warnx("driver alloc failed");
return -ENOMEM;
}
if (OK != g_dev->init()) {
fprintf(stderr, "[uorb] driver init failed\n");
warnx("driver init failed");
delete g_dev;
g_dev = nullptr;
return -EIO;
}
printf("[uorb] ready\n");
return OK;
}
@ -807,8 +896,7 @@ uorb_main(int argc, char *argv[])
if (!strcmp(argv[1], "status"))
return info();
fprintf(stderr, "unrecognised command, try 'start', 'test' or 'status'\n");
return -EINVAL;
errx(-EINVAL, "unrecognized command, try 'start', 'test' or 'status'");
}
/*
@ -825,11 +913,14 @@ namespace
* we tried to advertise.
*/
int
node_advertise(const struct orb_metadata *meta)
node_advertise(const struct orb_metadata *meta, int *instance = nullptr, int priority = ORB_PRIO_DEFAULT)
{
int fd = -1;
int ret = ERROR;
/* fill advertiser data */
const struct orb_advertdata adv = { meta, instance, priority };
/* open the control device */
fd = open(TOPIC_MASTER_DEVICE_PATH, 0);
@ -837,11 +928,12 @@ node_advertise(const struct orb_metadata *meta)
goto out;
/* advertise the object */
ret = ioctl(fd, ORBIOCADVERTISE, (unsigned long)(uintptr_t)meta);
ret = ioctl(fd, ORBIOCADVERTISE, (unsigned long)(uintptr_t)&adv);
/* it's OK if it already exists */
if ((OK != ret) && (EEXIST == errno))
if ((OK != ret) && (EEXIST == errno)) {
ret = OK;
}
out:
@ -858,7 +950,7 @@ out:
* advertisers.
*/
int
node_open(Flavor f, const struct orb_metadata *meta, const void *data, bool advertiser)
node_open(Flavor f, const struct orb_metadata *meta, const void *data, bool advertiser, int *instance = nullptr, int priority = ORB_PRIO_DEFAULT)
{
char path[orb_maxpath];
int fd, ret;
@ -883,7 +975,7 @@ node_open(Flavor f, const struct orb_metadata *meta, const void *data, bool adve
/*
* Generate the path to the node and try to open it.
*/
ret = node_mkpath(path, f, meta);
ret = node_mkpath(path, f, meta, instance);
if (ret != OK) {
errno = -ret;
@ -893,15 +985,34 @@ node_open(Flavor f, const struct orb_metadata *meta, const void *data, bool adve
/* open the path as either the advertiser or the subscriber */
fd = open(path, (advertiser) ? O_WRONLY : O_RDONLY);
/* if we want to advertise and the node existed, we have to re-try again */
if ((fd >= 0) && (instance != nullptr) && (advertiser)) {
/* close the fd, we want a new one */
close(fd);
/* the node_advertise call will automatically go for the next free entry */
fd = -1;
}
/* we may need to advertise the node... */
if (fd < 0) {
/* try to create the node */
ret = node_advertise(meta);
ret = node_advertise(meta, instance, priority);
if (ret == OK) {
/* update the path, as it might have been updated during the node_advertise call */
ret = node_mkpath(path, f, meta, instance);
if (ret != OK) {
errno = -ret;
return ERROR;
}
}
/* on success, try the open again */
if (ret == OK)
if (ret == OK) {
fd = open(path, (advertiser) ? O_WRONLY : O_RDONLY);
}
}
if (fd < 0) {
@ -917,12 +1028,18 @@ node_open(Flavor f, const struct orb_metadata *meta, const void *data, bool adve
orb_advert_t
orb_advertise(const struct orb_metadata *meta, const void *data)
{
return orb_advertise_multi(meta, data, nullptr, ORB_PRIO_DEFAULT);
}
orb_advert_t
orb_advertise_multi(const struct orb_metadata *meta, const void *data, int *instance, int priority)
{
int result, fd;
orb_advert_t advertiser;
/* open the node as an advertiser */
fd = node_open(PUBSUB, meta, data, true);
fd = node_open(PUBSUB, meta, data, true, instance, priority);
if (fd == ERROR)
return ERROR;
@ -933,7 +1050,7 @@ orb_advertise(const struct orb_metadata *meta, const void *data)
return ERROR;
/* the advertiser must perform an initial publish to initialise the object */
result= orb_publish(meta, advertiser, data);
result = orb_publish(meta, advertiser, data);
if (result == ERROR)
return ERROR;
@ -946,6 +1063,13 @@ orb_subscribe(const struct orb_metadata *meta)
return node_open(PUBSUB, meta, nullptr, false);
}
int
orb_subscribe_multi(const struct orb_metadata *meta, unsigned instance)
{
int inst = instance;
return node_open(PUBSUB, meta, nullptr, false, &inst);
}
int
orb_unsubscribe(int handle)
{
@ -988,6 +1112,12 @@ orb_stat(int handle, uint64_t *time)
return ioctl(handle, ORBIOCLASTUPDATE, (unsigned long)(uintptr_t)time);
}
int
orb_priority(int handle, int *priority)
{
return ioctl(handle, ORBIOCGPRIORITY, (unsigned long)(uintptr_t)priority);
}
int
orb_set_interval(int handle, unsigned interval)
{

119
src/modules/uORB/uORB.h
View File

@ -1,6 +1,6 @@
/****************************************************************************
*
* Copyright (C) 2012 PX4 Development Team. All rights reserved.
* Copyright (c) 2012-2015 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
@ -56,6 +56,25 @@ struct orb_metadata {
typedef const struct orb_metadata *orb_id_t;
/**
* Maximum number of multi topic instances
*/
#define ORB_MULTI_MAX_INSTANCES 3
/**
* Topic priority.
* Relevant for multi-topics / topic groups
*/
enum ORB_PRIO {
ORB_PRIO_MIN = 0,
ORB_PRIO_VERY_LOW = 25,
ORB_PRIO_LOW = 50,
ORB_PRIO_DEFAULT = 75,
ORB_PRIO_HIGH = 100,
ORB_PRIO_VERY_HIGH = 125,
ORB_PRIO_MAX = 255
};
/**
* Generates a pointer to the uORB metadata structure for
* a given topic.
@ -67,33 +86,6 @@ typedef const struct orb_metadata *orb_id_t;
*/
#define ORB_ID(_name) &__orb_##_name
/**
* Generates a pointer to the uORB metadata structure for
* a given topic.
*
* The topic must have been declared previously in scope
* with ORB_DECLARE().
*
* @param _name The name of the topic.
* @param _count The class ID of the topic
*/
#define ORB_ID_DOUBLE(_name, _count) ((_count == CLASS_DEVICE_PRIMARY) ? &__orb_##_name##0 : &__orb_##_name##1)
/**
* Generates a pointer to the uORB metadata structure for
* a given topic.
*
* The topic must have been declared previously in scope
* with ORB_DECLARE().
*
* @param _name The name of the topic.
* @param _count The class ID of the topic
*/
#define ORB_ID_TRIPLE(_name, _count) \
((_count == CLASS_DEVICE_PRIMARY) ? &__orb_##_name##0 : \
((_count == CLASS_DEVICE_SECONDARY) ? &__orb_##_name##1 : \
(((_count == CLASS_DEVICE_TERTIARY) ? &__orb_##_name##2 : 0))))
/**
* Declare (prototype) the uORB metadata for a topic.
*
@ -167,6 +159,34 @@ typedef intptr_t orb_advert_t;
*/
extern orb_advert_t orb_advertise(const struct orb_metadata *meta, const void *data) __EXPORT;
/**
* Advertise as the publisher of a topic.
*
* This performs the initial advertisement of a topic; it creates the topic
* node in /obj if required and publishes the initial data.
*
* Any number of advertisers may publish to a topic; publications are atomic
* but co-ordination between publishers is not provided by the ORB.
*
* @param meta The uORB metadata (usually from the ORB_ID() macro)
* for the topic.
* @param data A pointer to the initial data to be published.
* For topics updated by interrupt handlers, the advertisement
* must be performed from non-interrupt context.
* @param instance Pointer to an integer which will yield the instance ID (0-based)
* of the publication.
* @param priority The priority of the instance. If a subscriber subscribes multiple
* instances, the priority allows the subscriber to prioritize the best
* data source as long as its available.
* @return ERROR on error, otherwise returns a handle
* that can be used to publish to the topic.
* If the topic in question is not known (due to an
* ORB_DEFINE with no corresponding ORB_DECLARE)
* this function will return -1 and set errno to ENOENT.
*/
extern orb_advert_t orb_advertise_multi(const struct orb_metadata *meta, const void *data, int *instance, int priority) __EXPORT;
/**
* Publish new data to a topic.
*
@ -210,6 +230,37 @@ extern int orb_publish(const struct orb_metadata *meta, orb_advert_t handle, con
*/
extern int orb_subscribe(const struct orb_metadata *meta) __EXPORT;
/**
* Subscribe to a multi-instance of a topic.
*
* The returned value is a file descriptor that can be passed to poll()
* in order to wait for updates to a topic, as well as topic_read,
* orb_check and orb_stat.
*
* Subscription will succeed even if the topic has not been advertised;
* in this case the topic will have a timestamp of zero, it will never
* signal a poll() event, checking will always return false and it cannot
* be copied. When the topic is subsequently advertised, poll, check,
* stat and copy calls will react to the initial publication that is
* performed as part of the advertisement.
*
* Subscription will fail if the topic is not known to the system, i.e.
* there is nothing in the system that has declared the topic and thus it
* can never be published.
*
* @param meta The uORB metadata (usually from the ORB_ID() macro)
* for the topic.
* @param instance The instance of the topic. Instance 0 matches the
* topic of the orb_subscribe() call, higher indices
* are for topics created with orb_publish_multi().
* @return ERROR on error, otherwise returns a handle
* that can be used to read and update the topic.
* If the topic in question is not known (due to an
* ORB_DEFINE_OPTIONAL with no corresponding ORB_DECLARE)
* this function will return -1 and set errno to ENOENT.
*/
extern int orb_subscribe_multi(const struct orb_metadata *meta, unsigned instance) __EXPORT;
/**
* Unsubscribe from a topic.
*
@ -266,6 +317,18 @@ extern int orb_check(int handle, bool *updated) __EXPORT;
*/
extern int orb_stat(int handle, uint64_t *time) __EXPORT;
/**
* Return the priority of the topic
*
* @param handle A handle returned from orb_subscribe.
* @param priority Returns the priority of this topic. This is only relevant for
* topics which are published by multiple publishers (e.g. mag0, mag1, etc.)
* and allows a subscriber to automatically pick the topic with the highest
* priority, independent of the startup order of the associated publishers.
* @return OK on success, ERROR otherwise with errno set accordingly.
*/
extern int orb_priority(int handle, int *priority) __EXPORT;
/**
* Set the minimum interval between which updates are seen for a subscription.
*

2
src/modules/uavcan/module.mk
View File

@ -1,6 +1,6 @@
############################################################################
#
# Copyright (C) 2013 PX4 Development Team. All rights reserved.
# Copyright (c) 2013-2015 PX4 Development Team. All rights reserved.
# Author: Pavel Kirienko <pavel.kirienko@gmail.com>
#
# Redistribution and use in source and binary forms, with or without

9
src/modules/uavcan/sensors/baro.cpp
View File

@ -1,6 +1,6 @@
/****************************************************************************
*
* Copyright (C) 2014 PX4 Development Team. All rights reserved.
* Copyright (c) 2014, 2015 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
@ -38,15 +38,10 @@
#include "baro.hpp"
#include <cmath>
static const orb_id_t BARO_TOPICS[2] = {
ORB_ID(sensor_baro0),
ORB_ID(sensor_baro1)
};
const char *const UavcanBarometerBridge::NAME = "baro";
UavcanBarometerBridge::UavcanBarometerBridge(uavcan::INode& node) :
UavcanCDevSensorBridgeBase("uavcan_baro", "/dev/uavcan/baro", BARO_DEVICE_PATH, BARO_TOPICS),
UavcanCDevSensorBridgeBase("uavcan_baro", "/dev/uavcan/baro", BARO_DEVICE_PATH, ORB_ID(sensor_baro)),
_sub_air_data(node)
{
}

2
src/modules/uavcan/sensors/baro.hpp
View File

@ -1,6 +1,6 @@
/****************************************************************************
*
* Copyright (C) 2014 PX4 Development Team. All rights reserved.
* Copyright (c) 2014, 2015 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

2
src/modules/uavcan/sensors/gnss.cpp
View File

@ -1,6 +1,6 @@
/****************************************************************************
*
* Copyright (C) 2014 PX4 Development Team. All rights reserved.
* Copyright (c) 2014, 2015 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

2
src/modules/uavcan/sensors/gnss.hpp
View File

@ -1,6 +1,6 @@
/****************************************************************************
*
* Copyright (C) 2014 PX4 Development Team. All rights reserved.
* Copyright (c) 2014, 2015 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

10
src/modules/uavcan/sensors/mag.cpp
View File

@ -1,6 +1,6 @@
/****************************************************************************
*
* Copyright (C) 2014 PX4 Development Team. All rights reserved.
* Copyright (c) 2014, 2015 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
@ -39,16 +39,10 @@
#include <systemlib/err.h>
static const orb_id_t MAG_TOPICS[3] = {
ORB_ID(sensor_mag0),
ORB_ID(sensor_mag1),
ORB_ID(sensor_mag2)
};
const char *const UavcanMagnetometerBridge::NAME = "mag";
UavcanMagnetometerBridge::UavcanMagnetometerBridge(uavcan::INode& node) :
UavcanCDevSensorBridgeBase("uavcan_mag", "/dev/uavcan/mag", MAG_DEVICE_PATH, MAG_TOPICS),
UavcanCDevSensorBridgeBase("uavcan_mag", "/dev/uavcan/mag", MAG_DEVICE_PATH, ORB_ID(sensor_mag)),
_sub_mag(node)
{
_device_id.devid_s.devtype = DRV_MAG_DEVTYPE_HMC5883;

2
src/modules/uavcan/sensors/mag.hpp
View File

@ -1,6 +1,6 @@
/****************************************************************************
*
* Copyright (C) 2014 PX4 Development Team. All rights reserved.
* Copyright (c) 2014, 2015 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

6
src/modules/uavcan/sensors/sensor_bridge.cpp
View File

@ -62,7 +62,6 @@ UavcanCDevSensorBridgeBase::~UavcanCDevSensorBridgeBase()
(void)unregister_class_devname(_class_devname, _channels[i].class_instance);
}
}
delete [] _orb_topics;
delete [] _channels;
}
@ -116,11 +115,10 @@ void UavcanCDevSensorBridgeBase::publish(const int node_id, const void *report)
}
// Publish to the appropriate topic, abort on failure
channel->orb_id = _orb_topics[class_instance];
channel->node_id = node_id;
channel->class_instance = class_instance;
channel->orb_advert = orb_advertise(channel->orb_id, report);
channel->orb_advert = orb_advertise_multi(_orb_topic, report, &channel->orb_instance, ORB_PRIO_HIGH);
if (channel->orb_advert < 0) {
log("ADVERTISE FAILED");
(void)unregister_class_devname(_class_devname, class_instance);
@ -132,7 +130,7 @@ void UavcanCDevSensorBridgeBase::publish(const int node_id, const void *report)
}
assert(channel != nullptr);
(void)orb_publish(channel->orb_id, channel->orb_advert, report);
(void)orb_publish(_orb_topic, channel->orb_advert, report);
}
unsigned UavcanCDevSensorBridgeBase::get_num_redundant_channels() const

22
src/modules/uavcan/sensors/sensor_bridge.hpp
View File

@ -1,6 +1,6 @@
/****************************************************************************
*
* Copyright (C) 2014 PX4 Development Team. All rights reserved.
* Copyright (c) 2014, 2015 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
@ -75,8 +75,7 @@ public:
/**
* Sensor bridge factory.
* Creates a bridge object by its ASCII name, e.g. "gnss", "mag".
* @return nullptr if such bridge can't be created.
* Creates all known sensor bridges and puts them in the linked list.
*/
static void make_all(uavcan::INode &node, List<IUavcanSensorBridge*> &list);
};
@ -90,28 +89,29 @@ class UavcanCDevSensorBridgeBase : public IUavcanSensorBridge, public device::CD
struct Channel
{
int node_id = -1;
orb_id_t orb_id = nullptr;
orb_advert_t orb_advert = -1;
int class_instance = -1;
int orb_instance = -1;
};
const unsigned _max_channels;
const char *const _class_devname;
orb_id_t *const _orb_topics;
const orb_id_t _orb_topic;
Channel *const _channels;
bool _out_of_channels = false;
protected:
template <unsigned MaxChannels>
static constexpr unsigned DEFAULT_MAX_CHANNELS = 5; // 640 KB ought to be enough for anybody
UavcanCDevSensorBridgeBase(const char *name, const char *devname, const char *class_devname,
const orb_id_t (&orb_topics)[MaxChannels]) :
const orb_id_t orb_topic_sensor,
const unsigned max_channels = DEFAULT_MAX_CHANNELS) :
device::CDev(name, devname),
_max_channels(MaxChannels),
_max_channels(max_channels),
_class_devname(class_devname),
_orb_topics(new orb_id_t[MaxChannels]),
_channels(new Channel[MaxChannels])
_orb_topic(orb_topic_sensor),
_channels(new Channel[max_channels])
{
memcpy(_orb_topics, orb_topics, sizeof(orb_id_t) * MaxChannels);
_device_id.devid_s.bus_type = DeviceBusType_UAVCAN;
_device_id.devid_s.bus = 0;
}

17
src/systemcmds/config/config.c
View File

@ -81,7 +81,7 @@ config_main(int argc, char *argv[])
do_device(argc - 1, argv + 1);
}
}
errx(1, "expected a device, try '/dev/gyro', '/dev/accel', '/dev/mag'");
}
@ -192,8 +192,12 @@ do_gyro(int argc, char *argv[])
int srate = ioctl(fd, GYROIOCGSAMPLERATE, 0);
int prate = ioctl(fd, SENSORIOCGPOLLRATE, 0);
int range = ioctl(fd, GYROIOCGRANGE, 0);
int id = ioctl(fd, DEVIOCGDEVICEID,0);
int32_t calibration_id = 0;
warnx("gyro: \n\tsample rate:\t%d Hz\n\tread rate:\t%d Hz\n\trange:\t%d dps", srate, prate, range);
param_get(param_find("SENS_GYRO_ID"), &(calibration_id));
warnx("gyro: \n\tdevice id:\t0x%X\t(calibration is for device id 0x%X)\n\tsample rate:\t%d Hz\n\tread rate:\t%d Hz\n\trange:\t%d dps", id, calibration_id, srate, prate, range);
close(fd);
}
@ -267,9 +271,10 @@ do_mag(int argc, char *argv[])
int range = ioctl(fd, MAGIOCGRANGE, 0);
int id = ioctl(fd, DEVIOCGDEVICEID,0);
int32_t calibration_id = 0;
param_get(param_find("SENS_MAG_ID"), &(calibration_id));
warnx("mag: \n\tdevice id:\t%x\t(calibration is for device id %x)\n\tsample rate:\t%d Hz\n\tread rate:\t%d Hz\n\trange:\t%d Ga", id, calibration_id, srate, prate, range);
warnx("mag: \n\tdevice id:\t0x%X\t(calibration is for device id 0x%X)\n\tsample rate:\t%d Hz\n\tread rate:\t%d Hz\n\trange:\t%d Ga", id, calibration_id, srate, prate, range);
close(fd);
}
@ -341,8 +346,12 @@ do_accel(int argc, char *argv[])
int srate = ioctl(fd, ACCELIOCGSAMPLERATE, 0);
int prate = ioctl(fd, SENSORIOCGPOLLRATE, 0);
int range = ioctl(fd, ACCELIOCGRANGE, 0);
int id = ioctl(fd, DEVIOCGDEVICEID,0);
int32_t calibration_id = 0;
warnx("accel: \n\tsample rate:\t%d Hz\n\tread rate:\t%d Hz\n\trange:\t%d G", srate, prate, range);
param_get(param_find("SENS_ACC_ID"), &(calibration_id));
warnx("accel: \n\tdevice id:\t0x%X\t(calibration is for device id 0x%X)\n\tsample rate:\t%d Hz\n\tread rate:\t%d Hz\n\trange:\t%d G", id, calibration_id, srate, prate, range);
close(fd);
}

38
src/systemcmds/preflight_check/preflight_check.c
View File

@ -84,7 +84,7 @@ int preflight_check_main(int argc, char *argv[])
/* open text message output path */
int mavlink_fd = open(MAVLINK_LOG_DEVICE, 0);
int ret;
int32_t mag_devid,mag_calibration_devid;
int32_t devid, calibration_devid;
/* give the system some time to sample the sensors in the background */
usleep(150000);
@ -98,9 +98,9 @@ int preflight_check_main(int argc, char *argv[])
goto system_eval;
}
mag_devid = ioctl(fd, DEVIOCGDEVICEID,0);
param_get(param_find("SENS_MAG_ID"), &(mag_calibration_devid));
if (mag_devid != mag_calibration_devid){
devid = ioctl(fd, DEVIOCGDEVICEID,0);
param_get(param_find("SENS_MAG_ID"), &(calibration_devid));
if (devid != calibration_devid){
warnx("magnetometer calibration is for a different device - calibrate magnetometer first");
mavlink_log_critical(mavlink_fd, "SENSOR FAIL: MAG CAL ID");
system_ok = false;
@ -108,7 +108,7 @@ int preflight_check_main(int argc, char *argv[])
}
ret = ioctl(fd, MAGIOCSELFTEST, 0);
if (ret != OK) {
warnx("magnetometer calibration missing or bad - calibrate magnetometer first");
mavlink_log_critical(mavlink_fd, "SENSOR FAIL: MAG CHECK/CAL");
@ -120,8 +120,18 @@ int preflight_check_main(int argc, char *argv[])
close(fd);
fd = open(ACCEL_DEVICE_PATH, O_RDONLY);
devid = ioctl(fd, DEVIOCGDEVICEID,0);
param_get(param_find("SENS_ACC_ID"), &(calibration_devid));
if (devid != calibration_devid){
warnx("accelerometer calibration is for a different device - calibrate accelerometer first");
mavlink_log_critical(mavlink_fd, "SENSOR FAIL: ACC CAL ID");
system_ok = false;
goto system_eval;
}
ret = ioctl(fd, ACCELIOCSELFTEST, 0);
if (ret != OK) {
warnx("accel self test failed");
mavlink_log_critical(mavlink_fd, "SENSOR FAIL: ACCEL CHECK/CAL");
@ -156,8 +166,18 @@ int preflight_check_main(int argc, char *argv[])
close(fd);
fd = open(GYRO_DEVICE_PATH, 0);
devid = ioctl(fd, DEVIOCGDEVICEID,0);
param_get(param_find("SENS_GYRO_ID"), &(calibration_devid));
if (devid != calibration_devid){
warnx("gyro calibration is for a different device - calibrate gyro first");
mavlink_log_critical(mavlink_fd, "SENSOR FAIL: GYRO CAL ID");
system_ok = false;
goto system_eval;
}
ret = ioctl(fd, GYROIOCSELFTEST, 0);
if (ret != OK) {
warnx("gyro self test failed");
mavlink_log_critical(mavlink_fd, "SENSOR FAIL: GYRO CHECK/CAL");
@ -183,10 +203,10 @@ int preflight_check_main(int argc, char *argv[])
system_ok &= rc_ok;
system_eval:
if (system_ok) {
/* all good, exit silently */
exit(0);