diff options
27 files changed, 756 insertions, 113 deletions
diff --git a/ROMFS/px4fmu_common/init.d/rc.sensors b/ROMFS/px4fmu_common/init.d/rc.sensors index be54ea98b..ecb408a54 100644 --- a/ROMFS/px4fmu_common/init.d/rc.sensors +++ b/ROMFS/px4fmu_common/init.d/rc.sensors @@ -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 diff --git a/makefiles/config_px4fmu-v2_test.mk b/makefiles/config_px4fmu-v2_test.mk index 395a8f2ac..932f92261 100644 --- a/makefiles/config_px4fmu-v2_test.mk +++ b/makefiles/config_px4fmu-v2_test.mk @@ -34,6 +34,11 @@ MODULES += systemcmds/mtd MODULES += systemcmds/ver # +# Testing modules +# +MODULES += examples/matlab_csv_serial + +# # Library modules # MODULES += modules/systemlib diff --git a/src/drivers/device/device.h b/src/drivers/device/device.h index 2d105bb79..9d684e394 100644 --- a/src/drivers/device/device.h +++ b/src/drivers/device/device.h @@ -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 */ diff --git a/src/drivers/drv_accel.h b/src/drivers/drv_accel.h index 7d93ed938..1f98d966b 100644 --- a/src/drivers/drv_accel.h +++ b/src/drivers/drv_accel.h @@ -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 diff --git a/src/drivers/drv_baro.h b/src/drivers/drv_baro.h index e2f0137ae..248b9a73d 100644 --- a/src/drivers/drv_baro.h +++ b/src/drivers/drv_baro.h @@ -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 diff --git a/src/drivers/drv_gyro.h b/src/drivers/drv_gyro.h index 2ae8c7058..41b013a44 100644 --- a/src/drivers/drv_gyro.h +++ b/src/drivers/drv_gyro.h @@ -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 diff --git a/src/drivers/drv_mag.h b/src/drivers/drv_mag.h index a259ac9c0..5ddf5d08e 100644 --- a/src/drivers/drv_mag.h +++ b/src/drivers/drv_mag.h @@ -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 diff --git a/src/drivers/hmc5883/hmc5883.cpp b/src/drivers/hmc5883/hmc5883.cpp index ca654e3c0..0e9a961ac 100644 --- a/src/drivers/hmc5883/hmc5883.cpp +++ b/src/drivers/hmc5883/hmc5883.cpp @@ -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"); } } diff --git a/src/drivers/l3gd20/l3gd20.cpp b/src/drivers/l3gd20/l3gd20.cpp index f72db82c0..cfae8761c 100644 --- a/src/drivers/l3gd20/l3gd20.cpp +++ b/src/drivers/l3gd20/l3gd20.cpp @@ -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); - reset(); + switch (_class_instance) { + case CLASS_DEVICE_PRIMARY: + _orb_id = ORB_ID(sensor_gyro0); + break; - measure(); + case CLASS_DEVICE_SECONDARY: + _orb_id = ORB_ID(sensor_gyro1); + break; - if (_class_instance == CLASS_DEVICE_PRIMARY) { + case CLASS_DEVICE_TERTIARY: + _orb_id = ORB_ID(sensor_gyro2); + break; + } + + reset(); - /* advertise sensor topic, measure manually to initialize valid report */ - struct gyro_report grp; - _reports->get(&grp); + measure(); - _gyro_topic = orb_advertise(ORB_ID(sensor_gyro), &grp); + /* advertise sensor topic, measure manually to initialize valid report */ + struct gyro_report grp; + _reports->get(&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++; diff --git a/src/drivers/lsm303d/lsm303d.cpp b/src/drivers/lsm303d/lsm303d.cpp index 84ec18f28..6880cf0f8 100644 --- a/src/drivers/lsm303d/lsm303d.cpp +++ b/src/drivers/lsm303d/lsm303d.cpp @@ -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; + + case CLASS_DEVICE_SECONDARY: + _mag->_mag_orb_id = ORB_ID(sensor_mag1); + break; - /* measurement will have generated a report, publish */ - _mag->_mag_topic = orb_advertise(ORB_ID(sensor_mag), &mrp); + case CLASS_DEVICE_TERTIARY: + _mag->_mag_orb_id = ORB_ID(sensor_mag2); + break; + } - if (_mag->_mag_topic < 0) - debug("failed to create sensor_mag publication"); + _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; + + 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); + case CLASS_DEVICE_TERTIARY: + _accel_orb_id = ORB_ID(sensor_accel2); + break; + } - if (_accel_topic < 0) - debug("failed to create sensor_accel publication"); + _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) { } diff --git a/src/drivers/mpu6000/mpu6000.cpp b/src/drivers/mpu6000/mpu6000.cpp index db9d4d7e1..6f5dae7ad 100644 --- a/src/drivers/mpu6000/mpu6000.cpp +++ b/src/drivers/mpu6000/mpu6000.cpp @@ -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); + case CLASS_DEVICE_TERTIARY: + _accel_orb_id = ORB_ID(sensor_accel2); + break; - if (_accel_topic < 0) - debug("failed to create sensor_accel publication"); + } + + _accel_topic = orb_advertise(_accel_orb_id, &arp); + if (_accel_topic < 0) { + warnx("ADVERT FAIL"); } - if (_gyro->_gyro_class_instance == CLASS_DEVICE_PRIMARY) { - /* advertise sensor topic, measure manually to initialize valid report */ - struct gyro_report grp; - _gyro_reports->get(&grp); + /* advertise sensor topic, measure manually to initialize valid report */ + struct gyro_report grp; + _gyro_reports->get(&grp); - _gyro->_gyro_topic = orb_advertise(ORB_ID(sensor_gyro), &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; + + } - if (_gyro->_gyro_topic < 0) - debug("failed to create sensor_gyro publication"); + _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) { } diff --git a/src/drivers/ms5611/ms5611.cpp b/src/drivers/ms5611/ms5611.cpp index fe669b5f5..873fa62c4 100644 --- a/src/drivers/ms5611/ms5611.cpp +++ b/src/drivers/ms5611/ms5611.cpp @@ -300,12 +300,17 @@ MS5611::init() ret = OK; - if (_class_instance == CLASS_DEVICE_PRIMARY) { - - _baro_topic = orb_advertise(ORB_ID(sensor_baro), &brp); + 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; + } - 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)) { diff --git a/src/drivers/pca8574/module.mk b/src/drivers/pca8574/module.mk index 825ee9bb7..c53ed9ab2 100644 --- a/src/drivers/pca8574/module.mk +++ b/src/drivers/pca8574/module.mk @@ -4,3 +4,5 @@ MODULE_COMMAND = pca8574 SRCS = pca8574.cpp + +MAXOPTIMIZATION = -Os diff --git a/src/drivers/rgbled/module.mk b/src/drivers/rgbled/module.mk index 39b53ec9e..c287e35f3 100644 --- a/src/drivers/rgbled/module.mk +++ b/src/drivers/rgbled/module.mk @@ -4,3 +4,5 @@ MODULE_COMMAND = rgbled SRCS = rgbled.cpp + +MAXOPTIMIZATION = -Os diff --git a/src/examples/matlab_csv_serial/matlab_csv_serial.c b/src/examples/matlab_csv_serial/matlab_csv_serial.c new file mode 100644 index 000000000..c66bebeec --- /dev/null +++ b/src/examples/matlab_csv_serial/matlab_csv_serial.c @@ -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; +} + + diff --git a/src/examples/matlab_csv_serial/module.mk b/src/examples/matlab_csv_serial/module.mk new file mode 100644 index 000000000..1629c2ce4 --- /dev/null +++ b/src/examples/matlab_csv_serial/module.mk @@ -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 diff --git a/src/modules/commander/gyro_calibration.cpp b/src/modules/commander/gyro_calibration.cpp index cbc2844c1..d89c67c2b 100644 --- a/src/modules/commander/gyro_calibration.cpp +++ b/src/modules/commander/gyro_calibration.cpp @@ -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; diff --git a/src/modules/commander/mag_calibration.cpp b/src/modules/commander/mag_calibration.cpp index 0ead22f77..23900f386 100644 --- a/src/modules/commander/mag_calibration.cpp +++ b/src/modules/commander/mag_calibration.cpp @@ -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; diff --git a/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp b/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp index b3eb816f9..ccc497323 100644 --- a/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp +++ b/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp @@ -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; diff --git a/src/modules/fw_att_control/fw_att_control_main.cpp b/src/modules/fw_att_control/fw_att_control_main.cpp index 4cdba735a..0cea13cc4 100644 --- a/src/modules/fw_att_control/fw_att_control_main.cpp +++ b/src/modules/fw_att_control/fw_att_control_main.cpp @@ -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)); diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index 4740f674d..3ee7ec34d 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -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); } } diff --git a/src/modules/sdlog2/sdlog2.c b/src/modules/sdlog2/sdlog2.c index 0d36fa2c6..f534c0f4c 100644 --- a/src/modules/sdlog2/sdlog2.c +++ b/src/modules/sdlog2/sdlog2.c @@ -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 --- */ diff --git a/src/modules/sdlog2/sdlog2_messages.h b/src/modules/sdlog2/sdlog2_messages.h index 87f7f718f..b14ef04cc 100644 --- a/src/modules/sdlog2/sdlog2_messages.h +++ b/src/modules/sdlog2/sdlog2_messages.h @@ -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"), diff --git a/src/modules/sensors/sensor_params.c b/src/modules/sensors/sensor_params.c index 38b190761..7ce6ef5ef 100644 --- a/src/modules/sensors/sensor_params.c +++ b/src/modules/sensors/sensor_params.c @@ -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 diff --git a/src/modules/sensors/sensors.cpp b/src/modules/sensors/sensors.cpp index 6bafb9ba6..4e8a8c01d 100644 --- a/src/modules/sensors/sensors.cpp +++ b/src/modules/sensors/sensors.cpp @@ -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)); diff --git a/src/modules/uORB/objects_common.cpp b/src/modules/uORB/objects_common.cpp index c5831462b..08c3ce1ac 100644 --- a/src/modules/uORB/objects_common.cpp +++ b/src/modules/uORB/objects_common.cpp @@ -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); diff --git a/src/modules/uORB/topics/sensor_combined.h b/src/modules/uORB/topics/sensor_combined.h index fa3367de9..06dfcdab3 100644 --- a/src/modules/uORB/topics/sensor_combined.h +++ b/src/modules/uORB/topics/sensor_combined.h @@ -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 */ |