aboutsummaryrefslogtreecommitdiff
diff options
context:
space:
mode:
Diffstat
-rw-r--r--makefiles/config_px4fmu-v2_test.mk5
-rw-r--r--src/drivers/device/device.h5
-rw-r--r--src/drivers/drv_accel.h3
-rw-r--r--src/drivers/drv_baro.h3
-rw-r--r--src/drivers/drv_gyro.h3
-rw-r--r--src/drivers/drv_mag.h3
-rw-r--r--src/drivers/hmc5883/hmc5883.cpp4
-rw-r--r--src/drivers/l3gd20/l3gd20.cpp30
-rw-r--r--src/drivers/lsm303d/lsm303d.cpp71
-rw-r--r--src/drivers/mpu6000/mpu6000.cpp71
-rw-r--r--src/drivers/ms5611/ms5611.cpp27
-rw-r--r--src/examples/matlab_csv_serial/matlab_csv_serial.c247
-rw-r--r--src/examples/matlab_csv_serial/module.mk40
-rw-r--r--src/modules/commander/gyro_calibration.cpp4
-rw-r--r--src/modules/commander/mag_calibration.cpp4
-rw-r--r--src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp6
-rw-r--r--src/modules/fw_att_control/fw_att_control_main.cpp4
-rw-r--r--src/modules/mavlink/mavlink_receiver.cpp20
-rw-r--r--src/modules/sensors/sensors.cpp16
-rw-r--r--src/modules/uORB/objects_common.cpp12
20 files changed, 480 insertions, 98 deletions
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 7df234cab..b65649d9d 100644
--- a/src/drivers/device/device.h
+++ b/src/drivers/device/device.h
@@ -538,6 +538,9 @@ 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
+};
#endif /* _DEVICE_DEVICE_H */
diff --git a/src/drivers/drv_accel.h b/src/drivers/drv_accel.h
index 7d93ed938..a6abaec70 100644
--- a/src/drivers/drv_accel.h
+++ b/src/drivers/drv_accel.h
@@ -81,7 +81,8 @@ struct accel_scale {
/*
* ObjDev tag for raw accelerometer data.
*/
-ORB_DECLARE(sensor_accel);
+ORB_DECLARE(sensor_accel0);
+ORB_DECLARE(sensor_accel1);
/*
* 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..3635cbce1 100644
--- a/src/drivers/drv_gyro.h
+++ b/src/drivers/drv_gyro.h
@@ -81,7 +81,8 @@ struct gyro_scale {
/*
* ObjDev tag for raw gyro data.
*/
-ORB_DECLARE(sensor_gyro);
+ORB_DECLARE(sensor_gyro0);
+ORB_DECLARE(sensor_gyro1);
/*
* ioctl() definitions
diff --git a/src/drivers/drv_mag.h b/src/drivers/drv_mag.h
index a259ac9c0..a6c509d31 100644
--- a/src/drivers/drv_mag.h
+++ b/src/drivers/drv_mag.h
@@ -79,7 +79,8 @@ struct mag_scale {
/*
* ObjDev tag for raw magnetometer data.
*/
-ORB_DECLARE(sensor_mag);
+ORB_DECLARE(sensor_mag0);
+ORB_DECLARE(sensor_mag1);
/*
diff --git a/src/drivers/hmc5883/hmc5883.cpp b/src/drivers/hmc5883/hmc5883.cpp
index 4aef43102..ad1770300 100644
--- a/src/drivers/hmc5883/hmc5883.cpp
+++ b/src/drivers/hmc5883/hmc5883.cpp
@@ -949,9 +949,9 @@ HMC5883::collect()
if (_mag_topic != -1) {
/* publish it */
- orb_publish(ORB_ID(sensor_mag), _mag_topic, &new_report);
+ orb_publish(ORB_ID(sensor_mag0), _mag_topic, &new_report);
} else {
- _mag_topic = orb_advertise(ORB_ID(sensor_mag), &new_report);
+ _mag_topic = orb_advertise(ORB_ID(sensor_mag0), &new_report);
if (_mag_topic < 0)
debug("failed to create sensor_mag publication");
diff --git a/src/drivers/l3gd20/l3gd20.cpp b/src/drivers/l3gd20/l3gd20.cpp
index 64d1a7e55..9e5eb00db 100644
--- a/src/drivers/l3gd20/l3gd20.cpp
+++ b/src/drivers/l3gd20/l3gd20.cpp
@@ -403,17 +403,19 @@ L3GD20::init()
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 (_class_instance == CLASS_DEVICE_PRIMARY) {
+ _gyro_topic = orb_advertise(ORB_ID(sensor_gyro0), &grp);
- if (_gyro_topic < 0)
- debug("failed to create sensor_gyro publication");
+ } else if (_class_instance == CLASS_DEVICE_SECONDARY) {
+ _gyro_topic = orb_advertise(ORB_ID(sensor_gyro1), &grp);
+ }
+ if (_gyro_topic < 0) {
+ debug("failed to create sensor_gyro publication");
}
ret = OK;
@@ -931,9 +933,17 @@ 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);
+ switch (_class_instance) {
+ case CLASS_DEVICE_PRIMARY:
+ orb_publish(ORB_ID(sensor_gyro0), _gyro_topic, &report);
+ break;
+
+ case CLASS_DEVICE_SECONDARY:
+ orb_publish(ORB_ID(sensor_gyro1), _gyro_topic, &report);
+ break;
+ }
}
_read++;
diff --git a/src/drivers/lsm303d/lsm303d.cpp b/src/drivers/lsm303d/lsm303d.cpp
index 45e775055..ff0eda60b 100644
--- a/src/drivers/lsm303d/lsm303d.cpp
+++ b/src/drivers/lsm303d/lsm303d.cpp
@@ -611,34 +611,45 @@ 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);
-
- /* measurement will have generated a report, publish */
- _mag->_mag_topic = orb_advertise(ORB_ID(sensor_mag), &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_topic = orb_advertise(ORB_ID(sensor_mag0), &mrp);
+ break;
- if (_mag->_mag_topic < 0)
- debug("failed to create sensor_mag publication");
+ case CLASS_DEVICE_SECONDARY:
+ _mag->_mag_topic = orb_advertise(ORB_ID(sensor_mag1), &mrp);
+ break;
+ }
+ if (_mag->_mag_topic < 0) {
+ warnx("failed to create sensor_mag publication");
}
_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_topic = orb_advertise(ORB_ID(sensor_accel0), &arp);
+ break;
- /* measurement will have generated a report, publish */
- _accel_topic = orb_advertise(ORB_ID(sensor_accel), &arp);
+ case CLASS_DEVICE_SECONDARY:
+ _accel_topic = orb_advertise(ORB_ID(sensor_accel1), &arp);
+ break;
- if (_accel_topic < 0)
- debug("failed to create sensor_accel publication");
+ }
+ if (_accel_topic < 0) {
+ warnx("failed to create sensor_accel publication");
}
out:
@@ -1557,9 +1568,17 @@ 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);
+ switch (_accel_class_instance) {
+ case CLASS_DEVICE_PRIMARY:
+ orb_publish(ORB_ID(sensor_accel0), _accel_topic, &accel_report);
+ break;
+
+ case CLASS_DEVICE_SECONDARY:
+ orb_publish(ORB_ID(sensor_accel1), _accel_topic, &accel_report);
+ break;
+ }
}
_accel_read++;
@@ -1634,9 +1653,17 @@ 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);
+ switch (_mag->_mag_class_instance) {
+ case CLASS_DEVICE_PRIMARY:
+ orb_publish(ORB_ID(sensor_mag0), _mag->_mag_topic, &mag_report);
+ break;
+
+ case CLASS_DEVICE_SECONDARY:
+ orb_publish(ORB_ID(sensor_mag1), _mag->_mag_topic, &mag_report);
+ break;
+ }
}
_mag_read++;
diff --git a/src/drivers/mpu6000/mpu6000.cpp b/src/drivers/mpu6000/mpu6000.cpp
index 1b3a96a0d..25fd7d8a8 100644
--- a/src/drivers/mpu6000/mpu6000.cpp
+++ b/src/drivers/mpu6000/mpu6000.cpp
@@ -498,31 +498,44 @@ 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);
-
- /* measurement will have generated a report, publish */
- _accel_topic = orb_advertise(ORB_ID(sensor_accel), &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_topic = orb_advertise(ORB_ID(sensor_accel0), &arp);
+ break;
+
+ case CLASS_DEVICE_SECONDARY:
+ _accel_topic = orb_advertise(ORB_ID(sensor_accel1), &arp);
+ break;
- if (_accel_topic < 0)
- debug("failed to create sensor_accel publication");
+ }
+ if (_accel_topic < 0) {
+ warnx("failed to create sensor_accel publication");
}
- 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_topic = orb_advertise(ORB_ID(sensor_gyro0), &grp);
+ break;
+
+ case CLASS_DEVICE_SECONDARY:
+ _gyro->_gyro_topic = orb_advertise(ORB_ID(sensor_gyro1), &grp);
+ break;
- if (_gyro->_gyro_topic < 0)
- debug("failed to create sensor_gyro publication");
+ }
+ if (_gyro->_gyro_topic < 0) {
+ warnx("failed to create sensor_gyro publication");
}
out:
@@ -1345,14 +1358,30 @@ 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);
+ switch (_accel_class_instance) {
+ case CLASS_DEVICE_PRIMARY:
+ orb_publish(ORB_ID(sensor_accel0), _accel_topic, &arb);
+ break;
+
+ case CLASS_DEVICE_SECONDARY:
+ orb_publish(ORB_ID(sensor_accel1), _accel_topic, &arb);
+ break;
+ }
}
- if (_gyro->_gyro_topic > 0 && !(_pub_blocked)) {
+ if (!(_pub_blocked)) {
/* publish it */
- orb_publish(ORB_ID(sensor_gyro), _gyro->_gyro_topic, &grb);
+ switch (_gyro->_gyro_class_instance) {
+ case CLASS_DEVICE_PRIMARY:
+ orb_publish(ORB_ID(sensor_gyro0), _gyro->_gyro_topic, &grb);
+ break;
+
+ case CLASS_DEVICE_SECONDARY:
+ orb_publish(ORB_ID(sensor_gyro1), _gyro->_gyro_topic, &grb);
+ break;
+ }
}
/* stop measuring */
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/examples/matlab_csv_serial/matlab_csv_serial.c b/src/examples/matlab_csv_serial/matlab_csv_serial.c
new file mode 100644
index 000000000..1327d1a23
--- /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 <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;
+ bool is_usb = false;
+
+ /* 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), accel0_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 99ec98ee9..75d3b6ce4 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/sensors/sensors.cpp b/src/modules/sensors/sensors.cpp
index 6bafb9ba6..388cd2dcc 100644
--- a/src/modules/sensors/sensors.cpp
+++ b/src/modules/sensors/sensors.cpp
@@ -1004,7 +1004,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;
@@ -1030,7 +1030,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;
@@ -1056,7 +1056,7 @@ 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);
@@ -1087,7 +1087,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 +1618,11 @@ 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));
_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..679d6ea4f 100644
--- a/src/modules/uORB/objects_common.cpp
+++ b/src/modules/uORB/objects_common.cpp
@@ -46,16 +46,20 @@
#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);
#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);
#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);
#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);
generated by cgit v1.2.3 (git 2.39.1) at 2024-07-23 00:04:14 +0000