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

Merge pull request #1186 from PX4/logging 

Multi-instance handling for sensors
This commit is contained in:
Lorenz Meier 2014-07-17 07:33:50 +02:00
parent 65c952e134 7705a24f72
commit f89062fe3b
27 changed files with 757 additions and 114 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

39
ROMFS/px4fmu_common/init.d/rc.sensors
View File

@ -6,28 +6,51 @@
ms5611 start
adc start
# Mag might be external
if hmc5883 start
if mpu6000 -X start
then
echo "[init] Using HMC5883"
fi
if mpu6000 start
then
echo "[init] Using MPU6000"
fi
if l3gd20 -X start
then
fi
if l3gd20 start
then
echo "[init] Using L3GD20(H)"
fi
# MAG selection
if param compare SENS_EXT_MAG 2
then
if hmc5883 -I start
then
fi
else
# Use only external as primary
if param compare SENS_EXT_MAG 1
then
if hmc5883 -X start
then
fi
else
# auto-detect the primary, prefer external
if hmc5883 start
then
fi
fi
fi
if ver hwcmp PX4FMU_V2
then
# IMPORTANT: EXTERNAL BUSES SHOULD BE SCANNED FIRST
if lsm303d -X start
then
fi
if lsm303d start
then
echo "[init] Using LSM303D"
fi
fi
@ -38,11 +61,9 @@ then
else
if ets_airspeed start
then
echo "[init] Using ETS airspeed sensor (bus 3)"
else
if ets_airspeed start -b 1
then
echo "[init] Using ETS airspeed sensor (bus 1)"
fi
fi
fi

5
makefiles/config_px4fmu-v2_test.mk
View File

@ -33,6 +33,11 @@ MODULES += systemcmds/nshterm
MODULES += systemcmds/mtd
MODULES += systemcmds/ver
#
# Testing modules
#
MODULES += examples/matlab_csv_serial
#
# Library modules
#

6
src/drivers/device/device.h
View File

@ -543,6 +543,10 @@ private:
} // namespace device
// class instance for primary driver of each class
#define CLASS_DEVICE_PRIMARY 0
enum CLASS_DEVICE {
CLASS_DEVICE_PRIMARY=0,
CLASS_DEVICE_SECONDARY=1,
CLASS_DEVICE_TERTIARY=2
};
#endif /* _DEVICE_DEVICE_H */

4
src/drivers/drv_accel.h
View File

@ -81,7 +81,9 @@ struct accel_scale {
/*
* ObjDev tag for raw accelerometer data.
*/
ORB_DECLARE(sensor_accel);
ORB_DECLARE(sensor_accel0);
ORB_DECLARE(sensor_accel1);
ORB_DECLARE(sensor_accel2);
/*
* ioctl() definitions

3
src/drivers/drv_baro.h
View File

@ -63,7 +63,8 @@ struct baro_report {
/*
* ObjDev tag for raw barometer data.
*/
ORB_DECLARE(sensor_baro);
ORB_DECLARE(sensor_baro0);
ORB_DECLARE(sensor_baro1);
/*
* ioctl() definitions

4
src/drivers/drv_gyro.h
View File

@ -81,7 +81,9 @@ struct gyro_scale {
/*
* ObjDev tag for raw gyro data.
*/
ORB_DECLARE(sensor_gyro);
ORB_DECLARE(sensor_gyro0);
ORB_DECLARE(sensor_gyro1);
ORB_DECLARE(sensor_gyro2);
/*
* ioctl() definitions

5
src/drivers/drv_mag.h
View File

@ -79,8 +79,9 @@ struct mag_scale {
/*
* ObjDev tag for raw magnetometer data.
*/
ORB_DECLARE(sensor_mag);
ORB_DECLARE(sensor_mag0);
ORB_DECLARE(sensor_mag1);
ORB_DECLARE(sensor_mag2);
/*
* mag device types, for _device_id

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

@ -162,6 +162,7 @@ private:
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;
@ -360,6 +361,7 @@ HMC5883::HMC5883(int bus, const char *path, enum Rotation rotation) :
_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")),
@ -428,6 +430,20 @@ HMC5883::init()
_class_instance = register_class_devname(MAG_DEVICE_PATH);
switch (_class_instance) {
case CLASS_DEVICE_PRIMARY:
_mag_orb_id = ORB_ID(sensor_mag0);
break;
case CLASS_DEVICE_SECONDARY:
_mag_orb_id = ORB_ID(sensor_mag1);
break;
case CLASS_DEVICE_TERTIARY:
_mag_orb_id = ORB_ID(sensor_mag2);
break;
}
ret = OK;
/* sensor is ok, but not calibrated */
_sensor_ok = true;
@ -872,6 +888,7 @@ HMC5883::collect()
struct {
int16_t x, y, z;
} report;
int ret;
uint8_t cmd;
uint8_t check_counter;
@ -950,16 +967,16 @@ HMC5883::collect()
// apply user specified rotation
rotate_3f(_rotation, new_report.x, new_report.y, new_report.z);
if (_class_instance == CLASS_DEVICE_PRIMARY && !(_pub_blocked)) {
if (!(_pub_blocked)) {
if (_mag_topic != -1) {
/* publish it */
orb_publish(ORB_ID(sensor_mag), _mag_topic, &new_report);
orb_publish(_mag_orb_id, _mag_topic, &new_report);
} else {
_mag_topic = orb_advertise(ORB_ID(sensor_mag), &new_report);
_mag_topic = orb_advertise(_mag_orb_id, &new_report);
if (_mag_topic < 0)
debug("failed to create sensor_mag publication");
debug("ADVERT FAIL");
}
}

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

@ -213,6 +213,7 @@ private:
float _gyro_range_scale;
float _gyro_range_rad_s;
orb_advert_t _gyro_topic;
orb_id_t _orb_id;
int _class_instance;
unsigned _current_rate;
@ -345,6 +346,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),
_class_instance(-1),
_current_rate(0),
_orientation(SENSOR_BOARD_ROTATION_DEFAULT),
@ -405,21 +407,32 @@ 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();
if (_class_instance == CLASS_DEVICE_PRIMARY) {
/* advertise sensor topic, measure manually to initialize valid report */
struct gyro_report grp;
_reports->get(&grp);
/* advertise sensor topic, measure manually to initialize valid report */
struct gyro_report grp;
_reports->get(&grp);
_gyro_topic = orb_advertise(ORB_ID(sensor_gyro), &grp);
if (_gyro_topic < 0)
debug("failed to create sensor_gyro publication");
_gyro_topic = orb_advertise(_orb_id, &grp);
if (_gyro_topic < 0) {
debug("failed to create sensor_gyro publication");
}
ret = OK;
@ -937,9 +950,9 @@ L3GD20::measure()
poll_notify(POLLIN);
/* publish for subscribers */
if (_gyro_topic > 0 && !(_pub_blocked)) {
if (!(_pub_blocked)) {
/* publish it */
orb_publish(ORB_ID(sensor_gyro), _gyro_topic, &report);
orb_publish(_orb_id, _gyro_topic, &report);
}
_read++;

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

@ -284,6 +284,7 @@ private:
unsigned _mag_samplerate;
orb_advert_t _accel_topic;
orb_id_t _accel_orb_id;
int _accel_class_instance;
unsigned _accel_read;
@ -489,6 +490,7 @@ private:
LSM303D *_parent;
orb_advert_t _mag_topic;
orb_id_t _mag_orb_id;
int _mag_class_instance;
void measure();
@ -520,6 +522,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_class_instance(-1),
_accel_read(0),
_mag_read(0),
@ -624,34 +627,56 @@ LSM303D::init()
/* fill report structures */
measure();
if (_mag->_mag_class_instance == CLASS_DEVICE_PRIMARY) {
/* advertise sensor topic, measure manually to initialize valid report */
struct mag_report mrp;
_mag_reports->get(&mrp);
/* advertise sensor topic, measure manually to initialize valid report */
struct mag_report mrp;
_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;
/* measurement will have generated a report, publish */
_mag->_mag_topic = orb_advertise(ORB_ID(sensor_mag), &mrp);
case CLASS_DEVICE_SECONDARY:
_mag->_mag_orb_id = ORB_ID(sensor_mag1);
break;
if (_mag->_mag_topic < 0)
debug("failed to create sensor_mag publication");
case CLASS_DEVICE_TERTIARY:
_mag->_mag_orb_id = ORB_ID(sensor_mag2);
break;
}
_mag->_mag_topic = orb_advertise(_mag->_mag_orb_id, &mrp);
if (_mag->_mag_topic < 0) {
warnx("ADVERT ERR");
}
_accel_class_instance = register_class_devname(ACCEL_DEVICE_PATH);
if (_accel_class_instance == CLASS_DEVICE_PRIMARY) {
/* advertise sensor topic, measure manually to initialize valid report */
struct accel_report arp;
_accel_reports->get(&arp);
/* advertise sensor topic, measure manually to initialize valid report */
struct accel_report arp;
_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;
/* measurement will have generated a report, publish */
_accel_topic = orb_advertise(ORB_ID(sensor_accel), &arp);
case CLASS_DEVICE_SECONDARY:
_accel_orb_id = ORB_ID(sensor_accel1);
break;
if (_accel_topic < 0)
debug("failed to create sensor_accel publication");
case CLASS_DEVICE_TERTIARY:
_accel_orb_id = ORB_ID(sensor_accel2);
break;
}
_accel_topic = orb_advertise(_accel_orb_id, &arp);
if (_accel_topic < 0) {
warnx("ADVERT ERR");
}
out:
@ -1570,9 +1595,9 @@ LSM303D::measure()
/* notify anyone waiting for data */
poll_notify(POLLIN);
if (_accel_topic > 0 && !(_pub_blocked)) {
if (!(_pub_blocked)) {
/* publish it */
orb_publish(ORB_ID(sensor_accel), _accel_topic, &accel_report);
orb_publish(_accel_orb_id, _accel_topic, &accel_report);
}
_accel_read++;
@ -1647,9 +1672,9 @@ LSM303D::mag_measure()
/* notify anyone waiting for data */
poll_notify(POLLIN);
if (_mag->_mag_topic > 0 && !(_pub_blocked)) {
if (!(_pub_blocked)) {
/* publish it */
orb_publish(ORB_ID(sensor_mag), _mag->_mag_topic, &mag_report);
orb_publish(_mag->_mag_orb_id, _mag->_mag_topic, &mag_report);
}
_mag_read++;
@ -1743,6 +1768,7 @@ LSM303D_mag::LSM303D_mag(LSM303D *parent) :
CDev("LSM303D_mag", LSM303D_DEVICE_PATH_MAG),
_parent(parent),
_mag_topic(-1),
_mag_orb_id(nullptr),
_mag_class_instance(-1)
{
}

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

@ -215,6 +215,7 @@ private:
float _accel_range_scale;
float _accel_range_m_s2;
orb_advert_t _accel_topic;
orb_id_t _accel_orb_id;
int _accel_class_instance;
RingBuffer *_gyro_reports;
@ -370,6 +371,7 @@ protected:
private:
MPU6000 *_parent;
orb_advert_t _gyro_topic;
orb_id_t _gyro_orb_id;
int _gyro_class_instance;
/* do not allow to copy this class due to pointer data members */
@ -391,6 +393,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_class_instance(-1),
_gyro_reports(nullptr),
_gyro_scale{},
@ -507,31 +510,56 @@ MPU6000::init()
measure();
if (_accel_class_instance == CLASS_DEVICE_PRIMARY) {
/* advertise sensor topic, measure manually to initialize valid report */
struct accel_report arp;
_accel_reports->get(&arp);
/* advertise sensor topic, measure manually to initialize valid report */
struct accel_report arp;
_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;
/* measurement will have generated a report, publish */
_accel_topic = orb_advertise(ORB_ID(sensor_accel), &arp);
if (_accel_topic < 0)
debug("failed to create sensor_accel publication");
case CLASS_DEVICE_TERTIARY:
_accel_orb_id = ORB_ID(sensor_accel2);
break;
}
if (_gyro->_gyro_class_instance == CLASS_DEVICE_PRIMARY) {
_accel_topic = orb_advertise(_accel_orb_id, &arp);
/* advertise sensor topic, measure manually to initialize valid report */
struct gyro_report grp;
_gyro_reports->get(&grp);
if (_accel_topic < 0) {
warnx("ADVERT FAIL");
}
_gyro->_gyro_topic = orb_advertise(ORB_ID(sensor_gyro), &grp);
if (_gyro->_gyro_topic < 0)
debug("failed to create sensor_gyro publication");
/* advertise sensor topic, measure manually to initialize valid report */
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);
if (_gyro->_gyro_topic < 0) {
warnx("ADVERT FAIL");
}
out:
@ -1354,14 +1382,14 @@ MPU6000::measure()
poll_notify(POLLIN);
_gyro->parent_poll_notify();
if (_accel_topic > 0 && !(_pub_blocked)) {
if (!(_pub_blocked)) {
/* publish it */
orb_publish(ORB_ID(sensor_accel), _accel_topic, &arb);
orb_publish(_accel_orb_id, _accel_topic, &arb);
}
if (_gyro->_gyro_topic > 0 && !(_pub_blocked)) {
if (!(_pub_blocked)) {
/* publish it */
orb_publish(ORB_ID(sensor_gyro), _gyro->_gyro_topic, &grb);
orb_publish(_gyro->_gyro_orb_id, _gyro->_gyro_topic, &grb);
}
/* stop measuring */
@ -1382,6 +1410,7 @@ MPU6000_gyro::MPU6000_gyro(MPU6000 *parent, const char *path) :
CDev("MPU6000_gyro", path),
_parent(parent),
_gyro_topic(-1),
_gyro_orb_id(nullptr),
_gyro_class_instance(-1)
{
}

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

@ -300,12 +300,17 @@ MS5611::init()
ret = OK;
if (_class_instance == CLASS_DEVICE_PRIMARY) {
switch (_class_instance) {
case CLASS_DEVICE_PRIMARY:
_baro_topic = orb_advertise(ORB_ID(sensor_baro0), &brp);
break;
case CLASS_DEVICE_SECONDARY:
_baro_topic = orb_advertise(ORB_ID(sensor_baro1), &brp);
break;
}
_baro_topic = orb_advertise(ORB_ID(sensor_baro), &brp);
if (_baro_topic < 0)
debug("failed to create sensor_baro publication");
if (_baro_topic < 0) {
warnx("failed to create sensor_baro publication");
}
} while (0);
@ -722,9 +727,17 @@ MS5611::collect()
report.altitude = (((pow((p / p1), (-(a * R) / g))) * T1) - T1) / a;
/* publish it */
if (_baro_topic > 0 && !(_pub_blocked)) {
if (!(_pub_blocked)) {
/* publish it */
orb_publish(ORB_ID(sensor_baro), _baro_topic, &report);
switch (_class_instance) {
case CLASS_DEVICE_PRIMARY:
orb_publish(ORB_ID(sensor_baro0), _baro_topic, &report);
break;
case CLASS_DEVICE_SECONDARY:
orb_publish(ORB_ID(sensor_baro1), _baro_topic, &report);
break;
}
}
if (_reports->force(&report)) {

2
src/drivers/pca8574/module.mk
View File

@ -4,3 +4,5 @@
MODULE_COMMAND = pca8574
SRCS = pca8574.cpp
MAXOPTIMIZATION = -Os

2
src/drivers/rgbled/module.mk
View File

@ -4,3 +4,5 @@
MODULE_COMMAND = rgbled
SRCS = rgbled.cpp
MAXOPTIMIZATION = -Os

247
src/examples/matlab_csv_serial/matlab_csv_serial.c Normal file
View File

@ -0,0 +1,247 @@
/****************************************************************************
*
* Copyright (c) 2014 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name PX4 nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
/**
* @file matlab_csv_serial_main.c
*
* Matlab CSV / ASCII format interface at 921600 baud, 8 data bits,
* 1 stop bit, no parity
*
* @author Lorenz Meier <lm@inf.ethz.ch>
*/
#include <nuttx/config.h>
#include <unistd.h>
#include <stdlib.h>
#include <stdio.h>
#include <string.h>
#include <stdbool.h>
#include <fcntl.h>
#include <float.h>
#include <nuttx/sched.h>
#include <sys/prctl.h>
#include <drivers/drv_hrt.h>
#include <termios.h>
#include <errno.h>
#include <limits.h>
#include <math.h>
#include <uORB/uORB.h>
#include <drivers/drv_accel.h>
#include <drivers/drv_gyro.h>
#include <systemlib/perf_counter.h>
#include <systemlib/systemlib.h>
#include <systemlib/err.h>
#include <poll.h>
__EXPORT int matlab_csv_serial_main(int argc, char *argv[]);
static bool thread_should_exit = false; /**< Daemon exit flag */
static bool thread_running = false; /**< Daemon status flag */
static int daemon_task; /**< Handle of daemon task / thread */
int matlab_csv_serial_thread_main(int argc, char *argv[]);
static void usage(const char *reason);
static void usage(const char *reason)
{
if (reason)
fprintf(stderr, "%s\n", reason);
fprintf(stderr, "usage: daemon {start|stop|status} [-p <additional params>]\n\n");
exit(1);
}
/**
* The daemon app only briefly exists to start
* the background job. The stack size assigned in the
* Makefile does only apply to this management task.
*
* The actual stack size should be set in the call
* to task_spawn_cmd().
*/
int matlab_csv_serial_main(int argc, char *argv[])
{
if (argc < 1)
usage("missing command");
if (!strcmp(argv[1], "start"))
{
if (thread_running)
{
warnx("already running\n");
/* this is not an error */
exit(0);
}
thread_should_exit = false;
daemon_task = task_spawn_cmd("matlab_csv_serial",
SCHED_DEFAULT,
SCHED_PRIORITY_MAX - 5,
2000,
matlab_csv_serial_thread_main,
(argv) ? (const char **)&argv[2] : (const char **)NULL);
exit(0);
}
if (!strcmp(argv[1], "stop"))
{
thread_should_exit = true;
exit(0);
}
if (!strcmp(argv[1], "status"))
{
if (thread_running) {
warnx("running");
} else {
warnx("stopped");
}
exit(0);
}
usage("unrecognized command");
exit(1);
}
int matlab_csv_serial_thread_main(int argc, char *argv[])
{
if (argc < 2) {
errx(1, "need a serial port name as argument");
}
const char* uart_name = argv[1];
warnx("opening port %s", uart_name);
int serial_fd = open(uart_name, O_RDWR | O_NOCTTY);
unsigned speed = 921600;
if (serial_fd < 0) {
err(1, "failed to open port: %s", uart_name);
}
/* Try to set baud rate */
struct termios uart_config;
int termios_state;
/* Back up the original uart configuration to restore it after exit */
if ((termios_state = tcgetattr(serial_fd, &uart_config)) < 0) {
warnx("ERR GET CONF %s: %d\n", uart_name, termios_state);
close(serial_fd);
return -1;
}
/* Clear ONLCR flag (which appends a CR for every LF) */
uart_config.c_oflag &= ~ONLCR;
/* USB serial is indicated by /dev/ttyACM0*/
if (strcmp(uart_name, "/dev/ttyACM0") != OK && strcmp(uart_name, "/dev/ttyACM1") != OK) {
/* Set baud rate */
if (cfsetispeed(&uart_config, speed) < 0 || cfsetospeed(&uart_config, speed) < 0) {
warnx("ERR SET BAUD %s: %d\n", uart_name, termios_state);
close(serial_fd);
return -1;
}
}
if ((termios_state = tcsetattr(serial_fd, TCSANOW, &uart_config)) < 0) {
warnx("ERR SET CONF %s\n", uart_name);
close(serial_fd);
return -1;
}
/* subscribe to vehicle status, attitude, sensors and flow*/
struct accel_report accel0;
struct accel_report accel1;
struct gyro_report gyro0;
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));
thread_running = true;
while (!thread_should_exit)
{
/*This runs at the rate of the sensors */
struct pollfd fds[] = {
{ .fd = accel0_sub, .events = POLLIN }
};
/* wait for a sensor update, check for exit condition every 500 ms */
int ret = poll(fds, sizeof(fds) / sizeof(fds[0]), 500);
if (ret < 0)
{
/* poll error, ignore */
}
else if (ret == 0)
{
/* no return value, ignore */
warnx("no sensor data");
}
else
{
/* 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);
// 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,
(int)accel1.x_raw, (int)accel1.y_raw, (int)accel1.z_raw);
}
}
}
warnx("exiting");
thread_running = false;
fflush(stdout);
return 0;
}

40
src/examples/matlab_csv_serial/module.mk Normal file
View File

@ -0,0 +1,40 @@
############################################################################
#
# Copyright (c) 2014 PX4 Development Team. All rights reserved.
#
# Redistribution and use in source and binary forms, with or without
# modification, are permitted provided that the following conditions
# are met:
#
# 1. Redistributions of source code must retain the above copyright
# notice, this list of conditions and the following disclaimer.
# 2. Redistributions in binary form must reproduce the above copyright
# notice, this list of conditions and the following disclaimer in
# the documentation and/or other materials provided with the
# distribution.
# 3. Neither the name PX4 nor the names of its contributors may be
# used to endorse or promote products derived from this software
# without specific prior written permission.
#
# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
# POSSIBILITY OF SUCH DAMAGE.
#
############################################################################
#
# Build position estimator
#
MODULE_COMMAND = matlab_csv_serial
SRCS = matlab_csv_serial.c

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

@ -92,7 +92,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_gyro));
int sub_sensor_gyro = orb_subscribe(ORB_ID(sensor_gyro0));
struct gyro_report gyro_report;
while (calibration_counter < calibration_count) {
@ -104,7 +104,7 @@ int do_gyro_calibration(int mavlink_fd)
int poll_ret = poll(fds, 1, 1000);
if (poll_ret > 0) {
orb_copy(ORB_ID(sensor_gyro), sub_sensor_gyro, &gyro_report);
orb_copy(ORB_ID(sensor_gyro0), 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;

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

@ -145,7 +145,7 @@ int do_mag_calibration(int mavlink_fd)
}
if (res == OK) {
int sub_mag = orb_subscribe(ORB_ID(sensor_mag));
int sub_mag = orb_subscribe(ORB_ID(sensor_mag0));
struct mag_report mag;
/* limit update rate to get equally spaced measurements over time (in ms) */
@ -170,7 +170,7 @@ int do_mag_calibration(int mavlink_fd)
int poll_ret = poll(fds, 1, 1000);
if (poll_ret > 0) {
orb_copy(ORB_ID(sensor_mag), sub_mag, &mag);
orb_copy(ORB_ID(sensor_mag0), 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

@ -704,7 +704,7 @@ FixedwingEstimator::task_main()
/*
* do subscriptions
*/
_baro_sub = orb_subscribe(ORB_ID(sensor_baro));
_baro_sub = orb_subscribe(ORB_ID(sensor_baro0));
_airspeed_sub = orb_subscribe(ORB_ID(airspeed));
_gps_sub = orb_subscribe(ORB_ID(vehicle_gps_position));
_vstatus_sub = orb_subscribe(ORB_ID(vehicle_status));
@ -1052,7 +1052,7 @@ FixedwingEstimator::task_main()
orb_check(_baro_sub, &baro_updated);
if (baro_updated) {
orb_copy(ORB_ID(sensor_baro), _baro_sub, &_baro);
orb_copy(ORB_ID(sensor_baro0), _baro_sub, &_baro);
_ekf->baroHgt = _baro.altitude;
@ -1144,7 +1144,7 @@ FixedwingEstimator::task_main()
initVelNED[2] = _gps.vel_d_m_s;
// Set up height correctly
orb_copy(ORB_ID(sensor_baro), _baro_sub, &_baro);
orb_copy(ORB_ID(sensor_baro0), _baro_sub, &_baro);
_baro_ref_offset = _ekf->states[9]; // this should become zero in the local frame
_baro_gps_offset = _baro.altitude - gps_alt;
_ekf->baroHgt = _baro.altitude;

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

@ -529,7 +529,7 @@ FixedwingAttitudeControl::vehicle_accel_poll()
orb_check(_accel_sub, &accel_updated);
if (accel_updated) {
orb_copy(ORB_ID(sensor_accel), _accel_sub, &_accel);
orb_copy(ORB_ID(sensor_accel0), _accel_sub, &_accel);
}
}
@ -577,7 +577,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_accel));
_accel_sub = orb_subscribe(ORB_ID(sensor_accel0));
_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));

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

@ -546,10 +546,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_gyro), &gyro);
_gyro_pub = orb_advertise(ORB_ID(sensor_gyro0), &gyro);
} else {
orb_publish(ORB_ID(sensor_gyro), _gyro_pub, &gyro);
orb_publish(ORB_ID(sensor_gyro0), _gyro_pub, &gyro);
}
}
@ -568,10 +568,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_accel), &accel);
_accel_pub = orb_advertise(ORB_ID(sensor_accel0), &accel);
} else {
orb_publish(ORB_ID(sensor_accel), _accel_pub, &accel);
orb_publish(ORB_ID(sensor_accel0), _accel_pub, &accel);
}
}
@ -589,10 +589,10 @@ MavlinkReceiver::handle_message_hil_sensor(mavlink_message_t *msg)
mag.z = imu.zmag;
if (_mag_pub < 0) {
_mag_pub = orb_advertise(ORB_ID(sensor_mag), &mag);
_mag_pub = orb_advertise(ORB_ID(sensor_mag0), &mag);
} else {
orb_publish(ORB_ID(sensor_mag), _mag_pub, &mag);
orb_publish(ORB_ID(sensor_mag0), _mag_pub, &mag);
}
}
@ -607,10 +607,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_baro), &baro);
_baro_pub = orb_advertise(ORB_ID(sensor_baro0), &baro);
} else {
orb_publish(ORB_ID(sensor_baro), _baro_pub, &baro);
orb_publish(ORB_ID(sensor_baro0), _baro_pub, &baro);
}
}
@ -884,10 +884,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_accel), &accel);
_accel_pub = orb_advertise(ORB_ID(sensor_accel0), &accel);
} else {
orb_publish(ORB_ID(sensor_accel), _accel_pub, &accel);
orb_publish(ORB_ID(sensor_accel0), _accel_pub, &accel);
}
}

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

@ -1073,6 +1073,12 @@ int sdlog2_thread_main(int argc, char *argv[])
hrt_abstime magnetometer_timestamp = 0;
hrt_abstime barometer_timestamp = 0;
hrt_abstime differential_pressure_timestamp = 0;
hrt_abstime gyro1_timestamp = 0;
hrt_abstime accelerometer1_timestamp = 0;
hrt_abstime magnetometer1_timestamp = 0;
hrt_abstime gyro2_timestamp = 0;
hrt_abstime accelerometer2_timestamp = 0;
hrt_abstime magnetometer2_timestamp = 0;
/* initialize calculated mean SNR */
float snr_mean = 0.0f;
@ -1209,6 +1215,8 @@ int sdlog2_thread_main(int argc, char *argv[])
/* --- SENSOR COMBINED --- */
if (copy_if_updated(ORB_ID(sensor_combined), subs.sensor_sub, &buf.sensor)) {
bool write_IMU = false;
bool write_IMU1 = false;
bool write_IMU2 = false;
bool write_SENS = false;
if (buf.sensor.timestamp != gyro_timestamp) {
@ -1260,6 +1268,64 @@ int sdlog2_thread_main(int argc, char *argv[])
LOGBUFFER_WRITE_AND_COUNT(SENS);
}
if (buf.sensor.accelerometer1_timestamp != accelerometer1_timestamp) {
accelerometer1_timestamp = buf.sensor.accelerometer1_timestamp;
write_IMU1 = true;
}
if (buf.sensor.gyro1_timestamp != gyro1_timestamp) {
gyro1_timestamp = buf.sensor.gyro1_timestamp;
write_IMU1 = true;
}
if (buf.sensor.magnetometer1_timestamp != magnetometer1_timestamp) {
magnetometer1_timestamp = buf.sensor.magnetometer1_timestamp;
write_IMU1 = true;
}
if (write_IMU1) {
log_msg.msg_type = LOG_IMU1_MSG;
log_msg.body.log_IMU.gyro_x = buf.sensor.gyro1_rad_s[0];
log_msg.body.log_IMU.gyro_y = buf.sensor.gyro1_rad_s[1];
log_msg.body.log_IMU.gyro_z = buf.sensor.gyro1_rad_s[2];
log_msg.body.log_IMU.acc_x = buf.sensor.accelerometer1_m_s2[0];
log_msg.body.log_IMU.acc_y = buf.sensor.accelerometer1_m_s2[1];
log_msg.body.log_IMU.acc_z = buf.sensor.accelerometer1_m_s2[2];
log_msg.body.log_IMU.mag_x = buf.sensor.magnetometer1_ga[0];
log_msg.body.log_IMU.mag_y = buf.sensor.magnetometer1_ga[1];
log_msg.body.log_IMU.mag_z = buf.sensor.magnetometer1_ga[2];
LOGBUFFER_WRITE_AND_COUNT(IMU);
}
if (buf.sensor.accelerometer2_timestamp != accelerometer2_timestamp) {
accelerometer2_timestamp = buf.sensor.accelerometer2_timestamp;
write_IMU2 = true;
}
if (buf.sensor.gyro2_timestamp != gyro2_timestamp) {
gyro2_timestamp = buf.sensor.gyro2_timestamp;
write_IMU2 = true;
}
if (buf.sensor.magnetometer2_timestamp != magnetometer2_timestamp) {
magnetometer2_timestamp = buf.sensor.magnetometer2_timestamp;
write_IMU2 = true;
}
if (write_IMU2) {
log_msg.msg_type = LOG_IMU2_MSG;
log_msg.body.log_IMU.gyro_x = buf.sensor.gyro2_rad_s[0];
log_msg.body.log_IMU.gyro_y = buf.sensor.gyro2_rad_s[1];
log_msg.body.log_IMU.gyro_z = buf.sensor.gyro2_rad_s[2];
log_msg.body.log_IMU.acc_x = buf.sensor.accelerometer2_m_s2[0];
log_msg.body.log_IMU.acc_y = buf.sensor.accelerometer2_m_s2[1];
log_msg.body.log_IMU.acc_z = buf.sensor.accelerometer2_m_s2[2];
log_msg.body.log_IMU.mag_x = buf.sensor.magnetometer2_ga[0];
log_msg.body.log_IMU.mag_y = buf.sensor.magnetometer2_ga[1];
log_msg.body.log_IMU.mag_z = buf.sensor.magnetometer2_ga[2];
LOGBUFFER_WRITE_AND_COUNT(IMU);
}
}
/* --- ATTITUDE --- */

10
src/modules/sdlog2/sdlog2_messages.h
View File

@ -73,6 +73,8 @@ struct log_ATSP_s {
/* --- IMU - IMU SENSORS --- */
#define LOG_IMU_MSG 4
#define LOG_IMU1_MSG 22
#define LOG_IMU2_MSG 23
struct log_IMU_s {
float acc_x;
float acc_y;
@ -276,8 +278,8 @@ struct log_DIST_s {
uint8_t flags;
};
// ID 22 available
// ID 23 available
/* LOG IMU1 and IMU2 MSGs consume IDs 22 and 23 */
/* --- PWR - ONBOARD POWER SYSTEM --- */
#define LOG_PWR_MSG 24
@ -420,7 +422,9 @@ static const struct log_format_s log_formats[] = {
/* business-level messages, ID < 0x80 */
LOG_FORMAT(ATT, "fffffffff", "Roll,Pitch,Yaw,RollRate,PitchRate,YawRate,GX,GY,GZ"),
LOG_FORMAT(ATSP, "ffff", "RollSP,PitchSP,YawSP,ThrustSP"),
LOG_FORMAT(IMU, "fffffffff", "AccX,AccY,AccZ,GyroX,GyroY,GyroZ,MagX,MagY,MagZ"),
LOG_FORMAT_S(IMU, IMU, "fffffffff", "AccX,AccY,AccZ,GyroX,GyroY,GyroZ,MagX,MagY,MagZ"),
LOG_FORMAT_S(IMU1, IMU, "fffffffff", "AccX,AccY,AccZ,GyroX,GyroY,GyroZ,MagX,MagY,MagZ"),
LOG_FORMAT_S(IMU2, IMU, "fffffffff", "AccX,AccY,AccZ,GyroX,GyroY,GyroZ,MagX,MagY,MagZ"),
LOG_FORMAT(SENS, "fffff", "BaroPres,BaroAlt,BaroTemp,DiffPres,DiffPresFilt"),
LOG_FORMAT(LPOS, "ffffffffLLfBBBff", "X,Y,Z,Dist,DistR,VX,VY,VZ,RLat,RLon,RAlt,PFlg,LFlg,GFlg,EPH,EPV"),
LOG_FORMAT(LPSP, "ffff", "X,Y,Z,Yaw"),

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

@ -292,6 +292,19 @@ PARAM_DEFINE_FLOAT(SENS_BOARD_Z_OFF, 0.0f);
*/
PARAM_DEFINE_INT32(SENS_EXT_MAG_ROT, 0);
/**
* Set usage of external magnetometer
*
* * Set to 0 (default) to auto-detect (will try to get the external as primary)
* * Set to 1 to force the external magnetometer as primary
* * Set to 2 to force the internal magnetometer as primary
*
* @min 0
* @max 2
* @group Sensor Calibration
*/
PARAM_DEFINE_INT32(SENS_EXT_MAG, 0);
/**
* RC Channel 1 Minimum

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

@ -199,9 +199,15 @@ private:
bool _hil_enabled; /**< if true, HIL is active */
bool _publishing; /**< if true, we are publishing sensor data */
int _gyro_sub; /**< raw gyro data subscription */
int _accel_sub; /**< raw accel data subscription */
int _mag_sub; /**< raw mag data subscription */
int _gyro_sub; /**< raw gyro0 data subscription */
int _accel_sub; /**< raw accel0 data subscription */
int _mag_sub; /**< raw mag0 data subscription */
int _gyro1_sub; /**< raw gyro1 data subscription */
int _accel1_sub; /**< raw accel1 data subscription */
int _mag1_sub; /**< raw mag1 data subscription */
int _gyro2_sub; /**< raw gyro2 data subscription */
int _accel2_sub; /**< raw accel2 data subscription */
int _mag2_sub; /**< raw mag2 data subscription */
int _rc_sub; /**< raw rc channels data subscription */
int _baro_sub; /**< raw baro data subscription */
int _airspeed_sub; /**< airspeed subscription */
@ -478,6 +484,12 @@ Sensors::Sensors() :
_gyro_sub(-1),
_accel_sub(-1),
_mag_sub(-1),
_gyro1_sub(-1),
_accel1_sub(-1),
_mag1_sub(-1),
_gyro2_sub(-1),
_accel2_sub(-1),
_mag2_sub(-1),
_rc_sub(-1),
_baro_sub(-1),
_vcontrol_mode_sub(-1),
@ -1004,7 +1016,7 @@ Sensors::accel_poll(struct sensor_combined_s &raw)
if (accel_updated) {
struct accel_report accel_report;
orb_copy(ORB_ID(sensor_accel), _accel_sub, &accel_report);
orb_copy(ORB_ID(sensor_accel0), _accel_sub, &accel_report);
math::Vector<3> vect(accel_report.x, accel_report.y, accel_report.z);
vect = _board_rotation * vect;
@ -1019,6 +1031,48 @@ Sensors::accel_poll(struct sensor_combined_s &raw)
raw.accelerometer_timestamp = accel_report.timestamp;
}
orb_check(_accel1_sub, &accel_updated);
if (accel_updated) {
struct accel_report accel_report;
orb_copy(ORB_ID(sensor_accel1), _accel_sub, &accel_report);
math::Vector<3> vect(accel_report.x, accel_report.y, accel_report.z);
vect = _board_rotation * vect;
raw.accelerometer1_m_s2[0] = vect(0);
raw.accelerometer1_m_s2[1] = vect(1);
raw.accelerometer1_m_s2[2] = vect(2);
raw.accelerometer1_raw[0] = accel_report.x_raw;
raw.accelerometer1_raw[1] = accel_report.y_raw;
raw.accelerometer1_raw[2] = accel_report.z_raw;
raw.accelerometer1_timestamp = accel_report.timestamp;
}
orb_check(_accel2_sub, &accel_updated);
if (accel_updated) {
struct accel_report accel_report;
orb_copy(ORB_ID(sensor_accel2), _accel_sub, &accel_report);
math::Vector<3> vect(accel_report.x, accel_report.y, accel_report.z);
vect = _board_rotation * vect;
raw.accelerometer2_m_s2[0] = vect(0);
raw.accelerometer2_m_s2[1] = vect(1);
raw.accelerometer2_m_s2[2] = vect(2);
raw.accelerometer2_raw[0] = accel_report.x_raw;
raw.accelerometer2_raw[1] = accel_report.y_raw;
raw.accelerometer2_raw[2] = accel_report.z_raw;
raw.accelerometer2_timestamp = accel_report.timestamp;
}
}
void
@ -1030,7 +1084,7 @@ Sensors::gyro_poll(struct sensor_combined_s &raw)
if (gyro_updated) {
struct gyro_report gyro_report;
orb_copy(ORB_ID(sensor_gyro), _gyro_sub, &gyro_report);
orb_copy(ORB_ID(sensor_gyro0), _gyro_sub, &gyro_report);
math::Vector<3> vect(gyro_report.x, gyro_report.y, gyro_report.z);
vect = _board_rotation * vect;
@ -1045,6 +1099,48 @@ Sensors::gyro_poll(struct sensor_combined_s &raw)
raw.timestamp = gyro_report.timestamp;
}
orb_check(_gyro1_sub, &gyro_updated);
if (gyro_updated) {
struct gyro_report gyro_report;
orb_copy(ORB_ID(sensor_gyro1), _gyro1_sub, &gyro_report);
math::Vector<3> vect(gyro_report.x, gyro_report.y, gyro_report.z);
vect = _board_rotation * vect;
raw.gyro1_rad_s[0] = vect(0);
raw.gyro1_rad_s[1] = vect(1);
raw.gyro1_rad_s[2] = vect(2);
raw.gyro1_raw[0] = gyro_report.x_raw;
raw.gyro1_raw[1] = gyro_report.y_raw;
raw.gyro1_raw[2] = gyro_report.z_raw;
raw.gyro1_timestamp = gyro_report.timestamp;
}
orb_check(_gyro2_sub, &gyro_updated);
if (gyro_updated) {
struct gyro_report gyro_report;
orb_copy(ORB_ID(sensor_gyro2), _gyro_sub, &gyro_report);
math::Vector<3> vect(gyro_report.x, gyro_report.y, gyro_report.z);
vect = _board_rotation * vect;
raw.gyro2_rad_s[0] = vect(0);
raw.gyro2_rad_s[1] = vect(1);
raw.gyro2_rad_s[2] = vect(2);
raw.gyro2_raw[0] = gyro_report.x_raw;
raw.gyro2_raw[1] = gyro_report.y_raw;
raw.gyro2_raw[2] = gyro_report.z_raw;
raw.gyro2_timestamp = gyro_report.timestamp;
}
}
void
@ -1056,10 +1152,12 @@ Sensors::mag_poll(struct sensor_combined_s &raw)
if (mag_updated) {
struct mag_report mag_report;
orb_copy(ORB_ID(sensor_mag), _mag_sub, &mag_report);
orb_copy(ORB_ID(sensor_mag0), _mag_sub, &mag_report);
math::Vector<3> vect(mag_report.x, mag_report.y, mag_report.z);
// XXX we need device-id based handling here
if (_mag_is_external) {
vect = _external_mag_rotation * vect;
@ -1087,7 +1185,7 @@ Sensors::baro_poll(struct sensor_combined_s &raw)
if (baro_updated) {
orb_copy(ORB_ID(sensor_baro), _baro_sub, &_barometer);
orb_copy(ORB_ID(sensor_baro0), _baro_sub, &_barometer);
raw.baro_pres_mbar = _barometer.pressure; // Pressure in mbar
raw.baro_alt_meter = _barometer.altitude; // Altitude in meters
@ -1618,11 +1716,17 @@ Sensors::task_main()
/*
* do subscriptions
*/
_gyro_sub = orb_subscribe(ORB_ID(sensor_gyro));
_accel_sub = orb_subscribe(ORB_ID(sensor_accel));
_mag_sub = orb_subscribe(ORB_ID(sensor_mag));
_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));
_rc_sub = orb_subscribe(ORB_ID(input_rc));
_baro_sub = orb_subscribe(ORB_ID(sensor_baro));
_baro_sub = orb_subscribe(ORB_ID(sensor_baro0));
_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));

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

@ -46,16 +46,23 @@
#include <drivers/drv_orb_dev.h>
#include <drivers/drv_mag.h>
ORB_DEFINE(sensor_mag, struct mag_report);
ORB_DEFINE(sensor_mag0, struct mag_report);
ORB_DEFINE(sensor_mag1, struct mag_report);
ORB_DEFINE(sensor_mag2, struct mag_report);
#include <drivers/drv_accel.h>
ORB_DEFINE(sensor_accel, struct accel_report);
ORB_DEFINE(sensor_accel0, struct accel_report);
ORB_DEFINE(sensor_accel1, struct accel_report);
ORB_DEFINE(sensor_accel2, struct accel_report);
#include <drivers/drv_gyro.h>
ORB_DEFINE(sensor_gyro, struct gyro_report);
ORB_DEFINE(sensor_gyro0, struct gyro_report);
ORB_DEFINE(sensor_gyro1, struct gyro_report);
ORB_DEFINE(sensor_gyro2, struct gyro_report);
#include <drivers/drv_baro.h>
ORB_DEFINE(sensor_baro, struct baro_report);
ORB_DEFINE(sensor_baro0, struct baro_report);
ORB_DEFINE(sensor_baro1, struct baro_report);
#include <drivers/drv_range_finder.h>
ORB_DEFINE(sensor_range_finder, struct range_finder_report);

24
src/modules/uORB/topics/sensor_combined.h
View File

@ -95,6 +95,30 @@ struct sensor_combined_s {
float magnetometer_cuttoff_freq_hz; /**< Internal analog low pass frequency of sensor */
uint64_t magnetometer_timestamp; /**< Magnetometer timestamp */
int16_t gyro1_raw[3]; /**< Raw sensor values of angular velocity */
float gyro1_rad_s[3]; /**< Angular velocity in radian per seconds */
uint64_t gyro1_timestamp; /**< Gyro timestamp */
int16_t accelerometer1_raw[3]; /**< Raw acceleration in NED body frame */
float accelerometer1_m_s2[3]; /**< Acceleration in NED body frame, in m/s^2 */
uint64_t accelerometer1_timestamp; /**< Accelerometer timestamp */
int16_t magnetometer1_raw[3]; /**< Raw magnetic field in NED body frame */
float magnetometer1_ga[3]; /**< Magnetic field in NED body frame, in Gauss */
uint64_t magnetometer1_timestamp; /**< Magnetometer timestamp */
int16_t gyro2_raw[3]; /**< Raw sensor values of angular velocity */
float gyro2_rad_s[3]; /**< Angular velocity in radian per seconds */
uint64_t gyro2_timestamp; /**< Gyro timestamp */
int16_t accelerometer2_raw[3]; /**< Raw acceleration in NED body frame */
float accelerometer2_m_s2[3]; /**< Acceleration in NED body frame, in m/s^2 */
uint64_t accelerometer2_timestamp; /**< Accelerometer timestamp */
int16_t magnetometer2_raw[3]; /**< Raw magnetic field in NED body frame */
float magnetometer2_ga[3]; /**< Magnetic field in NED body frame, in Gauss */
uint64_t magnetometer2_timestamp; /**< Magnetometer timestamp */
float baro_pres_mbar; /**< Barometric pressure, already temp. comp. */
float baro_alt_meter; /**< Altitude, already temp. comp. */
float baro_temp_celcius; /**< Temperature in degrees celsius */