From 882076a0d48d465becff9c62db0096e76dd685b3 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 3 Jun 2014 16:38:45 +0200 Subject: [PATCH 01/59] Better defaults for min values for some of the multicopters --- ROMFS/px4fmu_common/init.d/10016_3dr_iris | 3 +++ ROMFS/px4fmu_common/init.d/4010_dji_f330 | 4 ++-- ROMFS/px4fmu_common/init.d/4011_dji_f450 | 4 ++-- 3 files changed, 7 insertions(+), 4 deletions(-) diff --git a/ROMFS/px4fmu_common/init.d/10016_3dr_iris b/ROMFS/px4fmu_common/init.d/10016_3dr_iris index 084dff140f..3f47390c15 100644 --- a/ROMFS/px4fmu_common/init.d/10016_3dr_iris +++ b/ROMFS/px4fmu_common/init.d/10016_3dr_iris @@ -30,3 +30,6 @@ fi set MIXER FMU_quad_w set PWM_OUTPUTS 1234 + +set PWM_MIN 1200 +set PWM_MAX 1950 diff --git a/ROMFS/px4fmu_common/init.d/4010_dji_f330 b/ROMFS/px4fmu_common/init.d/4010_dji_f330 index cd4480c3eb..aa43940155 100644 --- a/ROMFS/px4fmu_common/init.d/4010_dji_f330 +++ b/ROMFS/px4fmu_common/init.d/4010_dji_f330 @@ -23,5 +23,5 @@ then param set MC_YAWRATE_D 0.0 fi -set PWM_MIN 1175 -set PWM_MAX 1900 +set PWM_MIN 1200 +set PWM_MAX 1950 diff --git a/ROMFS/px4fmu_common/init.d/4011_dji_f450 b/ROMFS/px4fmu_common/init.d/4011_dji_f450 index ac2ecc70aa..4d055b4217 100644 --- a/ROMFS/px4fmu_common/init.d/4011_dji_f450 +++ b/ROMFS/px4fmu_common/init.d/4011_dji_f450 @@ -24,5 +24,5 @@ then param set MC_YAWRATE_D 0.0 fi -set PWM_MIN 1175 -set PWM_MAX 1900 +set PWM_MIN 1230 +set PWM_MAX 1950 From c4280a7cd83f7793d4773ffedd36e0fa220e2c07 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Fri, 6 Jun 2014 17:47:25 +0200 Subject: [PATCH 02/59] matlab logging: Initial CSV example --- .../matlab_csv_serial/matlab_csv_serial.c | 246 ++++++++++++++++++ src/examples/matlab_csv_serial/module.mk | 41 +++ 2 files changed, 287 insertions(+) create mode 100644 src/examples/matlab_csv_serial/matlab_csv_serial.c create mode 100644 src/examples/matlab_csv_serial/module.mk 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 0000000000..7c8bcc31e8 --- /dev/null +++ b/src/examples/matlab_csv_serial/matlab_csv_serial.c @@ -0,0 +1,246 @@ +/**************************************************************************** + * + * 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 + */ + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +__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 ]\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) + { + printf("flow position estimator 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[]) +{ + /* welcome user */ + thread_running = true; + + if (argc < 2) { + errx(1, "need a serial port name as argument"); + } + + int serial_fd = open(argv[1], O_RDWR | O_NOCTTY); + + unsigned speed = 921600; + + if (serial_fd < 0) { + err(1, "failed to open port: %s", argv[1]); + } + + /* 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(_uart_fd, uart_config_original)) < 0) { + warnx("ERR GET CONF %s: %d\n", uart_name, termios_state); + close(_uart_fd); + return -1; + } + + /* Fill the struct for the new configuration */ + tcgetattr(_uart_fd, &uart_config); + + /* 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", argv[1], termios_state); + close(serial_fd); + return -1; + } + + } + + if ((termios_state = tcsetattr(serial_fd, TCSANOW, &uart_config)) < 0) { + warnx("ERR SET CONF %s\n", argv[1]); + 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_accel)); + int accel1_sub; + int gyro0_sub; + int gyro1_sub; + + while (!thread_should_exit) + { + + /*This runs at the rate of the sensors */ + struct pollfd fds[] = { + { .fd = accel0_sub, .events = POLLIN }, + { .fd = accel1_sub, .events = POLLIN }, + { .fd = gyro0_sub, .events = POLLIN }, + { .fd = gyro1_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, count it in perf */ + perf_count(mc_err_perf); + + } + else if (ret == 0) + { + /* no return value, ignore */ + warnx("no sensor data"); + } + else + { + + /* parameter update available? */ + if (fds[0].revents & POLLIN) + { + orb_copy(ORB_ID(sensor_accel), accel0_sub, &accel0); + + // write out on accel 0, but collect for all other sensors as they have updates + vdprintf(serial_fd, "%llu,%d\n", accel0.timestamp, (int)accel0.x_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 0000000000..c83d1fefd6 --- /dev/null +++ b/src/examples/matlab_csv_serial/module.mk @@ -0,0 +1,41 @@ +############################################################################ +# +# 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 \ + matlab_csv_serial.c From 578680135e6813151a791dbcc3c31b1ba9c31a97 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 10 Jun 2014 15:03:58 +0200 Subject: [PATCH 03/59] introduce multi device support on uORB --- src/drivers/drv_accel.h | 3 ++- src/drivers/drv_baro.h | 3 ++- src/drivers/drv_gyro.h | 3 ++- src/drivers/drv_mag.h | 3 ++- src/modules/uORB/objects_common.cpp | 12 ++++++++---- 5 files changed, 16 insertions(+), 8 deletions(-) diff --git a/src/drivers/drv_accel.h b/src/drivers/drv_accel.h index 7d93ed9383..a6abaec70f 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 e2f0137ae2..248b9a73dc 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 2ae8c70582..3635cbce14 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 06107bd3d1..32e0e48b3b 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); /* * ioctl() definitions diff --git a/src/modules/uORB/objects_common.cpp b/src/modules/uORB/objects_common.cpp index 90675bb2e9..8340ca28a8 100644 --- a/src/modules/uORB/objects_common.cpp +++ b/src/modules/uORB/objects_common.cpp @@ -46,16 +46,20 @@ #include #include -ORB_DEFINE(sensor_mag, struct mag_report); +ORB_DEFINE(sensor_mag0, struct mag_report); +ORB_DEFINE(sensor_mag1, struct mag_report); #include -ORB_DEFINE(sensor_accel, struct accel_report); +ORB_DEFINE(sensor_accel0, struct accel_report); +ORB_DEFINE(sensor_accel1, struct accel_report); #include -ORB_DEFINE(sensor_gyro, struct gyro_report); +ORB_DEFINE(sensor_gyro0, struct gyro_report); +ORB_DEFINE(sensor_gyro1, struct gyro_report); #include -ORB_DEFINE(sensor_baro, struct baro_report); +ORB_DEFINE(sensor_baro0, struct baro_report); +ORB_DEFINE(sensor_baro1, struct baro_report); #include ORB_DEFINE(sensor_range_finder, struct range_finder_report); From 96accbf96cbc0262e4bb815d722282a318e8b8c5 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 10 Jun 2014 15:04:18 +0200 Subject: [PATCH 04/59] Make sensors app multi-device aware --- src/modules/sensors/sensors.cpp | 16 ++++++++-------- 1 file changed, 8 insertions(+), 8 deletions(-) diff --git a/src/modules/sensors/sensors.cpp b/src/modules/sensors/sensors.cpp index b268b1b36b..0ae93d7f01 100644 --- a/src/modules/sensors/sensors.cpp +++ b/src/modules/sensors/sensors.cpp @@ -967,7 +967,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; @@ -993,7 +993,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; @@ -1019,7 +1019,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); @@ -1050,7 +1050,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 @@ -1579,11 +1579,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)); From 07fb1e089d1d15d512a8a68b03020b1ffd877ed3 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 10 Jun 2014 15:04:56 +0200 Subject: [PATCH 05/59] Make commander multi-device aware --- src/modules/commander/gyro_calibration.cpp | 4 ++-- src/modules/commander/mag_calibration.cpp | 4 ++-- 2 files changed, 4 insertions(+), 4 deletions(-) diff --git a/src/modules/commander/gyro_calibration.cpp b/src/modules/commander/gyro_calibration.cpp index cbc2844c11..d89c67c2be 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 0ead22f773..23900f3868 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; From a2ef04146aa5eb233fd564f38e447e983e5985a7 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 10 Jun 2014 15:05:22 +0200 Subject: [PATCH 06/59] Introduce enum / define for multiple devices --- src/drivers/device/device.h | 5 ++++- 1 file changed, 4 insertions(+), 1 deletion(-) diff --git a/src/drivers/device/device.h b/src/drivers/device/device.h index d99f229220..be6f6f4ec2 100644 --- a/src/drivers/device/device.h +++ b/src/drivers/device/device.h @@ -510,6 +510,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 */ From dc2df309cba57a754b291b528f92d252fc271f57 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 10 Jun 2014 15:05:50 +0200 Subject: [PATCH 07/59] Introduce HMC5883 multi-device support --- src/drivers/hmc5883/hmc5883.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/drivers/hmc5883/hmc5883.cpp b/src/drivers/hmc5883/hmc5883.cpp index fddba806e3..be8e148078 100644 --- a/src/drivers/hmc5883/hmc5883.cpp +++ b/src/drivers/hmc5883/hmc5883.cpp @@ -866,9 +866,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"); From 506f900513c1ce319a0160f8ac82a399274ac66f Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 10 Jun 2014 15:06:05 +0200 Subject: [PATCH 08/59] Introduce MPU6K multi-device uORB support --- src/drivers/mpu6000/mpu6000.cpp | 71 +++++++++++++++++++++++---------- 1 file changed, 50 insertions(+), 21 deletions(-) diff --git a/src/drivers/mpu6000/mpu6000.cpp b/src/drivers/mpu6000/mpu6000.cpp index 321fdd1733..10966e2d0f 100644 --- a/src/drivers/mpu6000/mpu6000.cpp +++ b/src/drivers/mpu6000/mpu6000.cpp @@ -491,31 +491,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); - /* 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); - - if (_accel_topic < 0) - debug("failed to create sensor_accel publication"); + /* 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 (_gyro->_gyro_class_instance == CLASS_DEVICE_PRIMARY) { + if (_accel_topic < 0) { + warnx("failed to create sensor_accel publication"); + } - /* 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); + /* advertise sensor topic, measure manually to initialize valid report */ + struct gyro_report grp; + _gyro_reports->get(&grp); - if (_gyro->_gyro_topic < 0) - debug("failed to create sensor_gyro publication"); + 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) { + warnx("failed to create sensor_gyro publication"); } out: @@ -1326,14 +1339,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 */ From 795f3693f26f1671a417cb7b6e36d743cab72304 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 10 Jun 2014 15:07:42 +0200 Subject: [PATCH 09/59] LSM303D: Add support for multi-uORB devices --- src/drivers/lsm303d/lsm303d.cpp | 69 +++++++++++++++++++++++---------- 1 file changed, 48 insertions(+), 21 deletions(-) diff --git a/src/drivers/lsm303d/lsm303d.cpp b/src/drivers/lsm303d/lsm303d.cpp index 8bf76dcc33..8a14aca4bc 100644 --- a/src/drivers/lsm303d/lsm303d.cpp +++ b/src/drivers/lsm303d/lsm303d.cpp @@ -600,34 +600,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); - /* 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; - /* measurement will have generated a report, publish */ - _mag->_mag_topic = orb_advertise(ORB_ID(sensor_mag), &mrp); - - 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: @@ -1541,9 +1552,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++; @@ -1615,9 +1634,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++; From c35a25e70a57229b58fe065fda23003de856e9f5 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 10 Jun 2014 15:08:21 +0200 Subject: [PATCH 10/59] L3GD20: Add support for multi uORB topics --- src/drivers/l3gd20/l3gd20.cpp | 30 ++++++++++++++++++++---------- 1 file changed, 20 insertions(+), 10 deletions(-) diff --git a/src/drivers/l3gd20/l3gd20.cpp b/src/drivers/l3gd20/l3gd20.cpp index 37e72388be..2fcbdc0ade 100644 --- a/src/drivers/l3gd20/l3gd20.cpp +++ b/src/drivers/l3gd20/l3gd20.cpp @@ -398,17 +398,19 @@ L3GD20::init() measure(); + /* advertise sensor topic, measure manually to initialize valid report */ + struct gyro_report grp; + _reports->get(&grp); + if (_class_instance == CLASS_DEVICE_PRIMARY) { + _gyro_topic = orb_advertise(ORB_ID(sensor_gyro0), &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"); + } 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; @@ -923,9 +925,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++; From becfed9fbd93204134c44f6da09d954151dff10d Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 10 Jun 2014 15:08:47 +0200 Subject: [PATCH 11/59] MS5611: Add support for multi uORB devices --- src/drivers/ms5611/ms5611.cpp | 27 ++++++++++++++++++++------- 1 file changed, 20 insertions(+), 7 deletions(-) diff --git a/src/drivers/ms5611/ms5611.cpp b/src/drivers/ms5611/ms5611.cpp index 1ce93aeeaf..2d85a5e0dc 100644 --- a/src/drivers/ms5611/ms5611.cpp +++ b/src/drivers/ms5611/ms5611.cpp @@ -299,12 +299,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); @@ -721,9 +726,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)) { From 699ad1512cb55db004ab7d7143dd1ef8741fb9ad Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 10 Jun 2014 15:09:16 +0200 Subject: [PATCH 12/59] EKF estimator: Add support for multi uORB sensor topics --- .../ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) 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 0a6d3fa8fe..d806b02417 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 @@ -522,7 +522,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)); @@ -861,7 +861,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; @@ -1040,7 +1040,7 @@ FixedwingEstimator::task_main() float gps_alt = _gps.alt / 1e3f; // Set up height correctly - orb_copy(ORB_ID(sensor_baro), _baro_sub, &_baro); + orb_copy(ORB_ID(sensor_baro0), _baro_sub, &_baro); _baro_gps_offset = _baro_ref - _baro.altitude; _ekf->baroHgt = _baro.altitude; _ekf->hgtMea = 1.0f * (_ekf->baroHgt - (_baro_ref)); From e3e8bf25befa23edd6e2963760794abf4c8d6e2e Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 10 Jun 2014 15:09:41 +0200 Subject: [PATCH 13/59] FW att control: Add support for multi uORB topics --- src/modules/fw_att_control/fw_att_control_main.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) 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 178b590ae5..c026e29a72 100644 --- a/src/modules/fw_att_control/fw_att_control_main.cpp +++ b/src/modules/fw_att_control/fw_att_control_main.cpp @@ -520,7 +520,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); } } @@ -568,7 +568,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)); From 0031713004dc9dcc071e13ee388af8ad028d3978 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 10 Jun 2014 15:10:12 +0200 Subject: [PATCH 14/59] mavlink: Add support for multi uORB sensor interface --- src/modules/mavlink/mavlink_receiver.cpp | 20 ++++++++++---------- 1 file changed, 10 insertions(+), 10 deletions(-) diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index 53769e0cfe..98a7cef5a3 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -495,10 +495,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); } } @@ -517,10 +517,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); } } @@ -538,10 +538,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); } } @@ -556,10 +556,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); } } @@ -835,10 +835,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); } } From d0f4232ac6e2ff9d796df9d995e749734edc32ee Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 10 Jun 2014 15:12:11 +0200 Subject: [PATCH 15/59] Build and runtime fixes for matlab csv serial bridge --- .../matlab_csv_serial/matlab_csv_serial.c | 51 ++++++++++--------- src/examples/matlab_csv_serial/module.mk | 3 +- 2 files changed, 27 insertions(+), 27 deletions(-) diff --git a/src/examples/matlab_csv_serial/matlab_csv_serial.c b/src/examples/matlab_csv_serial/matlab_csv_serial.c index 7c8bcc31e8..1327d1a232 100644 --- a/src/examples/matlab_csv_serial/matlab_csv_serial.c +++ b/src/examples/matlab_csv_serial/matlab_csv_serial.c @@ -95,7 +95,7 @@ int matlab_csv_serial_main(int argc, char *argv[]) { if (thread_running) { - printf("flow position estimator already running\n"); + warnx("already running\n"); /* this is not an error */ exit(0); } @@ -133,19 +133,21 @@ int matlab_csv_serial_main(int argc, char *argv[]) int matlab_csv_serial_thread_main(int argc, char *argv[]) { - /* welcome user */ - thread_running = true; if (argc < 2) { errx(1, "need a serial port name as argument"); } - int serial_fd = open(argv[1], O_RDWR | O_NOCTTY); + 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", argv[1]); + err(1, "failed to open port: %s", uart_name); } /* Try to set baud rate */ @@ -154,15 +156,12 @@ int matlab_csv_serial_thread_main(int argc, char *argv[]) bool is_usb = false; /* Back up the original uart configuration to restore it after exit */ - if ((termios_state = tcgetattr(_uart_fd, uart_config_original)) < 0) { + if ((termios_state = tcgetattr(serial_fd, &uart_config)) < 0) { warnx("ERR GET CONF %s: %d\n", uart_name, termios_state); - close(_uart_fd); + close(serial_fd); return -1; } - /* Fill the struct for the new configuration */ - tcgetattr(_uart_fd, &uart_config); - /* Clear ONLCR flag (which appends a CR for every LF) */ uart_config.c_oflag &= ~ONLCR; @@ -171,7 +170,7 @@ int matlab_csv_serial_thread_main(int argc, char *argv[]) /* Set baud rate */ if (cfsetispeed(&uart_config, speed) < 0 || cfsetospeed(&uart_config, speed) < 0) { - warnx("ERR SET BAUD %s: %d\n", argv[1], termios_state); + warnx("ERR SET BAUD %s: %d\n", uart_name, termios_state); close(serial_fd); return -1; } @@ -179,7 +178,7 @@ int matlab_csv_serial_thread_main(int argc, char *argv[]) } if ((termios_state = tcsetattr(serial_fd, TCSANOW, &uart_config)) < 0) { - warnx("ERR SET CONF %s\n", argv[1]); + warnx("ERR SET CONF %s\n", uart_name); close(serial_fd); return -1; } @@ -191,20 +190,19 @@ int matlab_csv_serial_thread_main(int argc, char *argv[]) struct gyro_report gyro1; /* subscribe to parameter changes */ - int accel0_sub = orb_subscribe(ORB_ID(sensor_accel)); - int accel1_sub; - int gyro0_sub; - int gyro1_sub; + 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 }, - { .fd = accel1_sub, .events = POLLIN }, - { .fd = gyro0_sub, .events = POLLIN }, - { .fd = gyro1_sub, .events = POLLIN } + { .fd = accel0_sub, .events = POLLIN } }; /* wait for a sensor update, check for exit condition every 500 ms */ @@ -212,8 +210,7 @@ int matlab_csv_serial_thread_main(int argc, char *argv[]) if (ret < 0) { - /* poll error, count it in perf */ - perf_count(mc_err_perf); + /* poll error, ignore */ } else if (ret == 0) @@ -224,13 +221,17 @@ int matlab_csv_serial_thread_main(int argc, char *argv[]) else { - /* parameter update available? */ + /* accel0 update available? */ if (fds[0].revents & POLLIN) { - orb_copy(ORB_ID(sensor_accel), accel0_sub, &accel0); + 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 - vdprintf(serial_fd, "%llu,%d\n", accel0.timestamp, (int)accel0.x_raw); + 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); } } diff --git a/src/examples/matlab_csv_serial/module.mk b/src/examples/matlab_csv_serial/module.mk index c83d1fefd6..1629c2ce46 100644 --- a/src/examples/matlab_csv_serial/module.mk +++ b/src/examples/matlab_csv_serial/module.mk @@ -37,5 +37,4 @@ MODULE_COMMAND = matlab_csv_serial -SRCS = matlab_csv_serial.c \ - matlab_csv_serial.c +SRCS = matlab_csv_serial.c From 474de62ebb825de8d3757730fc3bdf8af250e173 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 10 Jun 2014 15:12:23 +0200 Subject: [PATCH 16/59] Enable matlab bridge in test setup --- makefiles/config_px4fmu-v2_test.mk | 5 +++++ 1 file changed, 5 insertions(+) diff --git a/makefiles/config_px4fmu-v2_test.mk b/makefiles/config_px4fmu-v2_test.mk index 66b2157edb..119056a399 100644 --- a/makefiles/config_px4fmu-v2_test.mk +++ b/makefiles/config_px4fmu-v2_test.mk @@ -32,6 +32,11 @@ MODULES += systemcmds/nshterm MODULES += systemcmds/mtd MODULES += systemcmds/ver +# +# Testing modules +# +MODULES += examples/matlab_csv_serial + # # Library modules # From e8c5b8230d6dbdfdfb5ac9749a1b713d10c4521e Mon Sep 17 00:00:00 2001 From: Jean Cyr Date: Thu, 10 Jul 2014 23:01:41 -0400 Subject: [PATCH 17/59] px4io_uploader cleanup and minor optimization Remove redundant code Cleanup error handling in program function --- src/drivers/px4io/px4io_uploader.cpp | 51 +++++++++++----------------- src/drivers/px4io/uploader.h | 1 - 2 files changed, 20 insertions(+), 32 deletions(-) diff --git a/src/drivers/px4io/px4io_uploader.cpp b/src/drivers/px4io/px4io_uploader.cpp index d134c0246e..bf6893a7e5 100644 --- a/src/drivers/px4io/px4io_uploader.cpp +++ b/src/drivers/px4io/px4io_uploader.cpp @@ -204,12 +204,8 @@ PX4IO_Uploader::upload(const char *filenames[]) if (bl_rev <= 2) { ret = verify_rev2(fw_size); - } else if(bl_rev == 3) { - ret = verify_rev3(fw_size); } else { - /* verify rev 4 and higher still uses the same approach and - * every version *needs* to be verified. - */ + /* verify rev 3 and higher. Every version *needs* to be verified. */ ret = verify_rev3(fw_size); } @@ -276,14 +272,14 @@ PX4IO_Uploader::recv(uint8_t &c, unsigned timeout) int PX4IO_Uploader::recv(uint8_t *p, unsigned count) { + int ret; while (count--) { - int ret = recv(*p++, 5000); + ret = recv(*p++, 5000); if (ret != OK) - return ret; + break; } - - return OK; + return ret; } void @@ -314,21 +310,19 @@ PX4IO_Uploader::send(uint8_t c) #endif if (write(_io_fd, &c, 1) != 1) return -errno; - return OK; } int PX4IO_Uploader::send(uint8_t *p, unsigned count) { + int ret; while (count--) { - int ret = send(*p++); - + ret = send(*p++); if (ret != OK) - return ret; + break; } - - return OK; + return ret; } int @@ -419,12 +413,15 @@ PX4IO_Uploader::program(size_t fw_size) int ret; size_t sent = 0; - file_buf = (uint8_t *)malloc(PROG_MULTI_MAX); + file_buf = new uint8_t[PROG_MULTI_MAX]; if (!file_buf) { log("Can't allocate program buffer"); return -ENOMEM; } + ASSERT((fw_size & 3) == 0); + ASSERT((PROG_MULTI_MAX & 3) == 0); + log("programming %u bytes...", (unsigned)fw_size); ret = lseek(_fw_fd, 0, SEEK_SET); @@ -443,34 +440,26 @@ PX4IO_Uploader::program(size_t fw_size) (unsigned)sent, (int)count, (int)errno); - } - - if (count == 0) { - free(file_buf); - return OK; + ret = -errno; + break; } sent += count; - if (count < 0) - return -errno; - - ASSERT((count % 4) == 0); - send(PROTO_PROG_MULTI); send(count); - send(&file_buf[0], count); + send(file_buf, count); send(PROTO_EOC); ret = get_sync(1000); if (ret != OK) { - free(file_buf); - return ret; + break; } } - free(file_buf); - return OK; + + delete [] file_buf; + return ret; } int diff --git a/src/drivers/px4io/uploader.h b/src/drivers/px4io/uploader.h index 55f63eef96..3e2142cf28 100644 --- a/src/drivers/px4io/uploader.h +++ b/src/drivers/px4io/uploader.h @@ -75,7 +75,6 @@ private: INFO_FLASH_SIZE = 4, /**< max firmware size in bytes */ PROG_MULTI_MAX = 60, /**< protocol max is 255, must be multiple of 4 */ - READ_MULTI_MAX = 60, /**< protocol max is 255, something overflows with >= 64 */ }; From a1c5f87deaf3e8626f838b191be5b57f33d3dc46 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 13 Jul 2014 16:26:47 +0200 Subject: [PATCH 18/59] Fix C++ warnings --- .../ekf_att_pos_estimator_main.cpp | 13 ++++++++++++- .../ekf_att_pos_estimator/estimator_utilities.cpp | 7 +++++-- 2 files changed, 17 insertions(+), 3 deletions(-) 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 a835599e79..bc40ce3c47 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 @@ -112,6 +112,10 @@ public: */ FixedwingEstimator(); + /* we do not want people ever copying this class */ + FixedwingEstimator(const FixedwingEstimator& that) = delete; + FixedwingEstimator operator=(const FixedwingEstimator&) = delete; + /** * Destructor, also kills the sensors task. */ @@ -362,9 +366,10 @@ FixedwingEstimator::FixedwingEstimator() : _mag_offsets({}), #ifdef SENSOR_COMBINED_SUB - _sensor_combined({}), + _sensor_combined{}, #endif + _pos_ref{}, _baro_ref(0.0f), _baro_ref_offset(0.0f), _baro_gps_offset(0.0f), @@ -381,12 +386,18 @@ FixedwingEstimator::FixedwingEstimator() : /* states */ _baro_init(false), _gps_initialized(false), + _gps_start_time(0), + _filter_start_time(0), + _last_sensor_timestamp(0), + _last_run(0), _gyro_valid(false), _accel_valid(false), _mag_valid(false), _ekf_logging(true), _debug(0), _mavlink_fd(-1), + _parameters{}, + _parameter_handles{}, _ekf(nullptr), _velocity_xy_filtered(0.0f), _velocity_z_filtered(0.0f), diff --git a/src/modules/ekf_att_pos_estimator/estimator_utilities.cpp b/src/modules/ekf_att_pos_estimator/estimator_utilities.cpp index 29a8c8d1e3..77cc1eeeb8 100644 --- a/src/modules/ekf_att_pos_estimator/estimator_utilities.cpp +++ b/src/modules/ekf_att_pos_estimator/estimator_utilities.cpp @@ -45,8 +45,11 @@ void Vector3f::zero(void) z = 0.0f; } -Mat3f::Mat3f() { - identity(); +Mat3f::Mat3f() : + x{1.0f, 0.0f, 0.0f}, + y{0.0f, 1.0f, 0.0f}, + z{0.0f, 0.0f, 1.0f} +{ } void Mat3f::identity() { From 17c5e925fb928d9c0926d8495a2db7a7b464c15f Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 13 Jul 2014 16:27:08 +0200 Subject: [PATCH 19/59] Enable C++ warnings for EKF --- src/modules/ekf_att_pos_estimator/module.mk | 2 ++ 1 file changed, 2 insertions(+) diff --git a/src/modules/ekf_att_pos_estimator/module.mk b/src/modules/ekf_att_pos_estimator/module.mk index dc5220bf0c..36d854dddb 100644 --- a/src/modules/ekf_att_pos_estimator/module.mk +++ b/src/modules/ekf_att_pos_estimator/module.mk @@ -41,3 +41,5 @@ SRCS = ekf_att_pos_estimator_main.cpp \ ekf_att_pos_estimator_params.c \ estimator_23states.cpp \ estimator_utilities.cpp + +EXTRACXXFLAGS = -Weffc++ From f219c05f0f53ee8b8f5dbe24318678be6c255dd9 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 13 Jul 2014 16:27:30 +0200 Subject: [PATCH 20/59] Fix a set of C++ warnings in mathlib --- src/lib/mathlib/math/Matrix.hpp | 23 ++++++++++++++--------- src/lib/mathlib/math/Vector.hpp | 9 ++++++--- 2 files changed, 20 insertions(+), 12 deletions(-) diff --git a/src/lib/mathlib/math/Matrix.hpp b/src/lib/mathlib/math/Matrix.hpp index ea0cf4ca1e..c2109b479d 100644 --- a/src/lib/mathlib/math/Matrix.hpp +++ b/src/lib/mathlib/math/Matrix.hpp @@ -69,27 +69,32 @@ public: /** * trivial ctor - * note that this ctor will not initialize elements + * Initializes the elements to zero. */ - MatrixBase() { - arm_mat = {M, N, &data[0][0]}; + MatrixBase() : + data{}, + arm_mat{M, N, &data[0][0]} + { } /** * copyt ctor */ - MatrixBase(const MatrixBase &m) { - arm_mat = {M, N, &data[0][0]}; + MatrixBase(const MatrixBase &m) : + arm_mat{M, N, &data[0][0]} + { memcpy(data, m.data, sizeof(data)); } - MatrixBase(const float *d) { - arm_mat = {M, N, &data[0][0]}; + MatrixBase(const float *d) : + arm_mat{M, N, &data[0][0]} + { memcpy(data, d, sizeof(data)); } - MatrixBase(const float d[M][N]) { - arm_mat = {M, N, &data[0][0]}; + MatrixBase(const float d[M][N]) : + arm_mat{M, N, &data[0][0]} + { memcpy(data, d, sizeof(data)); } diff --git a/src/lib/mathlib/math/Vector.hpp b/src/lib/mathlib/math/Vector.hpp index c7323c2153..5017b7c793 100644 --- a/src/lib/mathlib/math/Vector.hpp +++ b/src/lib/mathlib/math/Vector.hpp @@ -69,10 +69,13 @@ public: /** * trivial ctor - * note that this ctor will not initialize elements + * initializes elements to zero */ - VectorBase() { - arm_col = {N, 1, &data[0]}; + VectorBase() : + data{}, + arm_col{N, 1, &data[0]} + { + } /** From 61286451361c0e4547caf2385f56ee6cc8afffb1 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 15 Jul 2014 07:37:17 +0200 Subject: [PATCH 21/59] mathlib: More C++ fixes --- src/lib/mathlib/math/Matrix.hpp | 2 ++ src/lib/mathlib/math/Vector.hpp | 12 ++++++++---- 2 files changed, 10 insertions(+), 4 deletions(-) diff --git a/src/lib/mathlib/math/Matrix.hpp b/src/lib/mathlib/math/Matrix.hpp index c2109b479d..ca931e2daf 100644 --- a/src/lib/mathlib/math/Matrix.hpp +++ b/src/lib/mathlib/math/Matrix.hpp @@ -77,6 +77,8 @@ public: { } + virtual ~MatrixBase() {}; + /** * copyt ctor */ diff --git a/src/lib/mathlib/math/Vector.hpp b/src/lib/mathlib/math/Vector.hpp index 5017b7c793..0ddf776155 100644 --- a/src/lib/mathlib/math/Vector.hpp +++ b/src/lib/mathlib/math/Vector.hpp @@ -78,19 +78,23 @@ public: } + virtual ~VectorBase() {}; + /** * copy ctor */ - VectorBase(const VectorBase &v) { - arm_col = {N, 1, &data[0]}; + VectorBase(const VectorBase &v) : + arm_col{N, 1, &data[0]} + { memcpy(data, v.data, sizeof(data)); } /** * setting ctor */ - VectorBase(const float d[N]) { - arm_col = {N, 1, &data[0]}; + VectorBase(const float d[N]) : + arm_col{N, 1, &data[0]} + { memcpy(data, d, sizeof(data)); } From b0b6ee06448c266dc97d8efbd48be548f4a57ee6 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 15 Jul 2014 18:22:38 +0200 Subject: [PATCH 22/59] Forbid copy constructor in CDev --- src/drivers/device/device.h | 5 +++++ 1 file changed, 5 insertions(+) diff --git a/src/drivers/device/device.h b/src/drivers/device/device.h index 7df234cab0..2d105bb791 100644 --- a/src/drivers/device/device.h +++ b/src/drivers/device/device.h @@ -240,6 +240,7 @@ private: * @param context Pointer to the interrupted context. */ static void dev_interrupt(int irq, void *context); + }; /** @@ -469,6 +470,10 @@ private: * @return OK, or -errno on error. */ int remove_poll_waiter(struct pollfd *fds); + + /* do not allow copying this class */ + CDev(const CDev&); + CDev operator=(const CDev&); }; /** From 1ac2b307e4424d3b6555ab3ca21c43d8de19b81e Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 15 Jul 2014 18:23:17 +0200 Subject: [PATCH 23/59] Enable stricter compile mode and ensure the most relevant bits are initialized. Needs more work to avoid the remaining warnings --- src/modules/mavlink/mavlink_commands.cpp | 7 +++++-- src/modules/mavlink/mavlink_ftp.cpp | 16 ++++++++++------ src/modules/mavlink/mavlink_main.cpp | 10 +++++++++- src/modules/mavlink/mavlink_main.h | 4 ++++ src/modules/mavlink/mavlink_mission.h | 4 ++++ src/modules/mavlink/mavlink_orb_subscription.h | 4 ++++ src/modules/mavlink/mavlink_receiver.cpp | 12 ++++++------ src/modules/mavlink/mavlink_receiver.h | 4 ++++ src/modules/mavlink/mavlink_stream.h | 4 ++++ src/modules/mavlink/module.mk | 2 ++ 10 files changed, 52 insertions(+), 15 deletions(-) diff --git a/src/modules/mavlink/mavlink_commands.cpp b/src/modules/mavlink/mavlink_commands.cpp index fccd4d9a59..b502c3c863 100644 --- a/src/modules/mavlink/mavlink_commands.cpp +++ b/src/modules/mavlink/mavlink_commands.cpp @@ -40,9 +40,12 @@ #include "mavlink_commands.h" -MavlinkCommandsStream::MavlinkCommandsStream(Mavlink *mavlink, mavlink_channel_t channel) : _channel(channel), _cmd_time(0) +MavlinkCommandsStream::MavlinkCommandsStream(Mavlink *mavlink, mavlink_channel_t channel) : + _cmd_sub(mavlink->add_orb_subscription(ORB_ID(vehicle_command))), + _cmd{}, + _channel(channel), + _cmd_time(0) { - _cmd_sub = mavlink->add_orb_subscription(ORB_ID(vehicle_command)); } void diff --git a/src/modules/mavlink/mavlink_ftp.cpp b/src/modules/mavlink/mavlink_ftp.cpp index 675a6870e1..6a2c900afe 100644 --- a/src/modules/mavlink/mavlink_ftp.cpp +++ b/src/modules/mavlink/mavlink_ftp.cpp @@ -50,16 +50,20 @@ MavlinkFTP::getServer() return _server; } -MavlinkFTP::MavlinkFTP() +MavlinkFTP::MavlinkFTP() : + _session_fds{}, + _workBufs{}, + _workFree{}, + _lock{} { // initialise the request freelist dq_init(&_workFree); sem_init(&_lock, 0, 1); - - // initialize session list - for (size_t i=0; i Date: Tue, 15 Jul 2014 22:08:40 +0200 Subject: [PATCH 24/59] Do not initialize variable if value is never read --- src/drivers/hmc5883/hmc5883.cpp | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/src/drivers/hmc5883/hmc5883.cpp b/src/drivers/hmc5883/hmc5883.cpp index ad17703001..573b4fbba5 100644 --- a/src/drivers/hmc5883/hmc5883.cpp +++ b/src/drivers/hmc5883/hmc5883.cpp @@ -867,7 +867,8 @@ HMC5883::collect() struct { int16_t x, y, z; } report; - int ret = -EIO; + + int ret; uint8_t cmd; uint8_t check_counter; From 5ef4e08c580a4f15628ed16194a680508ea044bf Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 15 Jul 2014 23:14:30 +0200 Subject: [PATCH 25/59] hmc5883: Support for three sensors --- src/drivers/hmc5883/hmc5883.cpp | 24 ++++++++++++++++++++---- 1 file changed, 20 insertions(+), 4 deletions(-) diff --git a/src/drivers/hmc5883/hmc5883.cpp b/src/drivers/hmc5883/hmc5883.cpp index 573b4fbba5..26014c6d8b 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; @@ -355,6 +356,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")), @@ -423,6 +425,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; @@ -946,16 +962,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_mag0), _mag_topic, &new_report); + orb_publish(_mag_orb_id, _mag_topic, &new_report); } else { - _mag_topic = orb_advertise(ORB_ID(sensor_mag0), &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"); } } From 32ed1eae80f4004daa63b4331a66b774becf651f Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 15 Jul 2014 23:14:50 +0200 Subject: [PATCH 26/59] mpu6000: Support for up to three accels / gyros --- src/drivers/mpu6000/mpu6000.cpp | 48 ++++++++++++++++----------------- 1 file changed, 24 insertions(+), 24 deletions(-) diff --git a/src/drivers/mpu6000/mpu6000.cpp b/src/drivers/mpu6000/mpu6000.cpp index 25fd7d8a8e..09eec0caf3 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; @@ -367,6 +368,7 @@ protected: private: MPU6000 *_parent; orb_advert_t _gyro_topic; + orb_id_t _gyro_orb_id; int _gyro_class_instance; }; @@ -383,6 +385,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_range_scale(0.0f), @@ -505,17 +508,23 @@ MPU6000::init() /* measurement will have generated a report, publish */ switch (_accel_class_instance) { case CLASS_DEVICE_PRIMARY: - _accel_topic = orb_advertise(ORB_ID(sensor_accel0), &arp); + _accel_orb_id = ORB_ID(sensor_accel0); break; case CLASS_DEVICE_SECONDARY: - _accel_topic = orb_advertise(ORB_ID(sensor_accel1), &arp); + _accel_orb_id = ORB_ID(sensor_accel1); + break; + + case CLASS_DEVICE_TERTIARY: + _accel_orb_id = ORB_ID(sensor_accel2); break; } + _accel_topic = orb_advertise(_accel_orb_id, &arp); + if (_accel_topic < 0) { - warnx("failed to create sensor_accel publication"); + warnx("ADVERT FAIL"); } @@ -525,17 +534,23 @@ MPU6000::init() switch (_gyro->_gyro_class_instance) { case CLASS_DEVICE_PRIMARY: - _gyro->_gyro_topic = orb_advertise(ORB_ID(sensor_gyro0), &grp); + _gyro->_gyro_orb_id = ORB_ID(sensor_gyro0); break; case CLASS_DEVICE_SECONDARY: - _gyro->_gyro_topic = orb_advertise(ORB_ID(sensor_gyro1), &grp); + _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("failed to create sensor_gyro publication"); + warnx("ADVERT FAIL"); } out: @@ -1360,28 +1375,12 @@ MPU6000::measure() if (!(_pub_blocked)) { /* publish it */ - 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; - } + orb_publish(_accel_orb_id, _accel_topic, &arb); } if (!(_pub_blocked)) { /* publish it */ - 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; - } + orb_publish(_gyro->_gyro_orb_id, _gyro->_gyro_topic, &grb); } /* stop measuring */ @@ -1402,6 +1401,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) { } From 65367f7a99907fbd6a204ba44ee443816c635482 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 15 Jul 2014 23:16:04 +0200 Subject: [PATCH 27/59] L3GD20: Support for up to three gyros --- src/drivers/l3gd20/l3gd20.cpp | 33 ++++++++++++++++++--------------- 1 file changed, 18 insertions(+), 15 deletions(-) diff --git a/src/drivers/l3gd20/l3gd20.cpp b/src/drivers/l3gd20/l3gd20.cpp index 9e5eb00dba..5e90733ff3 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; @@ -339,6 +340,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), @@ -399,6 +401,20 @@ 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(); @@ -407,12 +423,7 @@ L3GD20::init() struct gyro_report grp; _reports->get(&grp); - if (_class_instance == CLASS_DEVICE_PRIMARY) { - _gyro_topic = orb_advertise(ORB_ID(sensor_gyro0), &grp); - - } else if (_class_instance == CLASS_DEVICE_SECONDARY) { - _gyro_topic = orb_advertise(ORB_ID(sensor_gyro1), &grp); - } + _gyro_topic = orb_advertise(_orb_id, &grp); if (_gyro_topic < 0) { debug("failed to create sensor_gyro publication"); @@ -935,15 +946,7 @@ L3GD20::measure() /* publish for subscribers */ if (!(_pub_blocked)) { /* publish it */ - 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; - } + orb_publish(_orb_id, _gyro_topic, &report); } _read++; From 64e33b8896d717f73a1b1ad3189b97c98c9bddfd Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 15 Jul 2014 23:18:04 +0200 Subject: [PATCH 28/59] uORB: Support up to three topics per sensor --- src/modules/uORB/objects_common.cpp | 3 +++ src/modules/uORB/topics/sensor_combined.h | 24 +++++++++++++++++++++++ 2 files changed, 27 insertions(+) diff --git a/src/modules/uORB/objects_common.cpp b/src/modules/uORB/objects_common.cpp index 679d6ea4f5..08c3ce1ac1 100644 --- a/src/modules/uORB/objects_common.cpp +++ b/src/modules/uORB/objects_common.cpp @@ -48,14 +48,17 @@ #include ORB_DEFINE(sensor_mag0, struct mag_report); ORB_DEFINE(sensor_mag1, struct mag_report); +ORB_DEFINE(sensor_mag2, struct mag_report); #include ORB_DEFINE(sensor_accel0, struct accel_report); ORB_DEFINE(sensor_accel1, struct accel_report); +ORB_DEFINE(sensor_accel2, struct accel_report); #include ORB_DEFINE(sensor_gyro0, struct gyro_report); ORB_DEFINE(sensor_gyro1, struct gyro_report); +ORB_DEFINE(sensor_gyro2, struct gyro_report); #include ORB_DEFINE(sensor_baro0, struct baro_report); diff --git a/src/modules/uORB/topics/sensor_combined.h b/src/modules/uORB/topics/sensor_combined.h index fa3367de9b..06dfcdab30 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 */ From b8b6974e22b7292d5251f1d2c49b2c81e09cc06b Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 15 Jul 2014 23:19:45 +0200 Subject: [PATCH 29/59] sdlog2: Add support for up to three IMU sensors --- src/modules/sdlog2/sdlog2.c | 66 ++++++++++++++++++++++++++++ src/modules/sdlog2/sdlog2_messages.h | 10 +++-- 2 files changed, 73 insertions(+), 3 deletions(-) diff --git a/src/modules/sdlog2/sdlog2.c b/src/modules/sdlog2/sdlog2.c index 0d36fa2c6e..f534c0f4ce 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 87f7f718f5..b14ef04ccf 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"), From 9e97994ef92d3ef3496384c4b11c734138c5f3a3 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 15 Jul 2014 23:20:02 +0200 Subject: [PATCH 30/59] drivers: Up to three units support --- src/drivers/device/device.h | 3 ++- src/drivers/drv_accel.h | 1 + src/drivers/drv_gyro.h | 1 + src/drivers/drv_mag.h | 2 +- 4 files changed, 5 insertions(+), 2 deletions(-) diff --git a/src/drivers/device/device.h b/src/drivers/device/device.h index b65649d9da..813e1542bb 100644 --- a/src/drivers/device/device.h +++ b/src/drivers/device/device.h @@ -540,7 +540,8 @@ private: // class instance for primary driver of each class enum CLASS_DEVICE { CLASS_DEVICE_PRIMARY=0, - CLASS_DEVICE_SECONDARY=1 + 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 a6abaec70f..1f98d966bd 100644 --- a/src/drivers/drv_accel.h +++ b/src/drivers/drv_accel.h @@ -83,6 +83,7 @@ struct accel_scale { */ ORB_DECLARE(sensor_accel0); ORB_DECLARE(sensor_accel1); +ORB_DECLARE(sensor_accel2); /* * ioctl() definitions diff --git a/src/drivers/drv_gyro.h b/src/drivers/drv_gyro.h index 3635cbce14..41b013a443 100644 --- a/src/drivers/drv_gyro.h +++ b/src/drivers/drv_gyro.h @@ -83,6 +83,7 @@ struct gyro_scale { */ 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 a6c509d319..5ddf5d08e8 100644 --- a/src/drivers/drv_mag.h +++ b/src/drivers/drv_mag.h @@ -81,7 +81,7 @@ struct mag_scale { */ ORB_DECLARE(sensor_mag0); ORB_DECLARE(sensor_mag1); - +ORB_DECLARE(sensor_mag2); /* * mag device types, for _device_id From c467d1635efda46bc7200b95741381fc98e161ae Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 15 Jul 2014 23:21:00 +0200 Subject: [PATCH 31/59] Build fixes for example --- src/examples/matlab_csv_serial/matlab_csv_serial.c | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/src/examples/matlab_csv_serial/matlab_csv_serial.c b/src/examples/matlab_csv_serial/matlab_csv_serial.c index 1327d1a232..c66bebeecd 100644 --- a/src/examples/matlab_csv_serial/matlab_csv_serial.c +++ b/src/examples/matlab_csv_serial/matlab_csv_serial.c @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (C) 2014 PX4 Development Team. All rights reserved. + * 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 @@ -60,6 +60,7 @@ #include #include #include +#include #include __EXPORT int matlab_csv_serial_main(int argc, char *argv[]); @@ -153,7 +154,6 @@ int matlab_csv_serial_thread_main(int argc, char *argv[]) /* 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) { @@ -225,7 +225,7 @@ int matlab_csv_serial_thread_main(int argc, char *argv[]) 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_accel1), accel1_sub, &accel1); orb_copy(ORB_ID(sensor_gyro0), gyro0_sub, &gyro0); orb_copy(ORB_ID(sensor_gyro1), gyro1_sub, &gyro1); From b6bac2c88d44fcb7ee0b1fd579f0f0fcc19c2410 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 15 Jul 2014 23:22:04 +0200 Subject: [PATCH 32/59] sensors: Support for up to three sensors of the IMU types --- src/modules/sensors/sensors.cpp | 110 +++++++++++++++++++++++++++++++- 1 file changed, 107 insertions(+), 3 deletions(-) diff --git a/src/modules/sensors/sensors.cpp b/src/modules/sensors/sensors.cpp index 388cd2dcc5..4e8a8c01d3 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), @@ -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 @@ -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 @@ -1060,6 +1156,8 @@ Sensors::mag_poll(struct sensor_combined_s &raw) 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; @@ -1621,6 +1719,12 @@ Sensors::task_main() _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_baro0)); _diff_pres_sub = orb_subscribe(ORB_ID(differential_pressure)); From 43bc2c3ef2a867a015e7198c797d089d6252fdde Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 15 Jul 2014 23:32:03 +0200 Subject: [PATCH 33/59] LSM303D: Support for tertiary sensors --- src/drivers/lsm303d/lsm303d.cpp | 47 ++++++++++++++++----------------- 1 file changed, 23 insertions(+), 24 deletions(-) diff --git a/src/drivers/lsm303d/lsm303d.cpp b/src/drivers/lsm303d/lsm303d.cpp index ff0eda60bb..2750f8755e 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; @@ -485,6 +486,7 @@ private: LSM303D *_parent; orb_advert_t _mag_topic; + orb_id_t _mag_orb_id; int _mag_class_instance; void measure(); @@ -508,6 +510,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), @@ -618,16 +621,22 @@ LSM303D::init() /* 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); + _mag->_mag_orb_id = ORB_ID(sensor_mag0); break; case CLASS_DEVICE_SECONDARY: - _mag->_mag_topic = orb_advertise(ORB_ID(sensor_mag1), &mrp); + _mag->_mag_orb_id = ORB_ID(sensor_mag1); + break; + + case CLASS_DEVICE_TERTIARY: + _mag->_mag_orb_id = ORB_ID(sensor_mag2); break; } + _mag->_mag_topic = orb_advertise(_mag->_mag_orb_id, &mrp); + if (_mag->_mag_topic < 0) { - warnx("failed to create sensor_mag publication"); + warnx("ADVERT ERR"); } _accel_class_instance = register_class_devname(ACCEL_DEVICE_PATH); @@ -639,17 +648,22 @@ LSM303D::init() /* measurement will have generated a report, publish */ switch (_accel_class_instance) { case CLASS_DEVICE_PRIMARY: - _accel_topic = orb_advertise(ORB_ID(sensor_accel0), &arp); + _accel_orb_id = ORB_ID(sensor_accel0); break; case CLASS_DEVICE_SECONDARY: - _accel_topic = orb_advertise(ORB_ID(sensor_accel1), &arp); + _accel_orb_id = ORB_ID(sensor_accel1); break; + case CLASS_DEVICE_TERTIARY: + _accel_orb_id = ORB_ID(sensor_accel2); + break; } + _accel_topic = orb_advertise(_accel_orb_id, &arp); + if (_accel_topic < 0) { - warnx("failed to create sensor_accel publication"); + warnx("ADVERT ERR"); } out: @@ -1570,15 +1584,7 @@ LSM303D::measure() if (!(_pub_blocked)) { /* publish it */ - 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; - } + orb_publish(_accel_orb_id, _accel_topic, &accel_report); } _accel_read++; @@ -1655,15 +1661,7 @@ LSM303D::mag_measure() if (!(_pub_blocked)) { /* publish it */ - 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; - } + orb_publish(_mag->_mag_orb_id, _mag->_mag_topic, &mag_report); } _mag_read++; @@ -1757,6 +1755,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) { } From f89573a6ed6e43c9e5935b92929b6082089226c9 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Wed, 16 Jul 2014 07:51:24 +0200 Subject: [PATCH 34/59] HMC5883 driver: stack size adjustments of start handler and start return review / comments --- src/drivers/hmc5883/hmc5883.cpp | 9 ++++++--- src/drivers/hmc5883/module.mk | 5 ++--- 2 files changed, 8 insertions(+), 6 deletions(-) diff --git a/src/drivers/hmc5883/hmc5883.cpp b/src/drivers/hmc5883/hmc5883.cpp index 4aef43102e..0cde1045c2 100644 --- a/src/drivers/hmc5883/hmc5883.cpp +++ b/src/drivers/hmc5883/hmc5883.cpp @@ -867,7 +867,7 @@ HMC5883::collect() struct { int16_t x, y, z; } report; - int ret = -EIO; + int ret; uint8_t cmd; uint8_t check_counter; @@ -1157,7 +1157,7 @@ int HMC5883::calibrate(struct file *filp, unsigned enable) out: if (OK != ioctl(filp, MAGIOCSSCALE, (long unsigned int)&mscale_previous)) { - warn("WARNING: failed to set new scale / offsets for mag"); + warn("failed to set new scale / offsets for mag"); } /* set back to normal mode */ @@ -1353,6 +1353,9 @@ void usage(); /** * Start the driver. + * + * This function call only returns once the driver + * is either successfully up and running or failed to start. */ void start(int bus, enum Rotation rotation) @@ -1443,7 +1446,7 @@ test(int bus) int fd = open(path, O_RDONLY); if (fd < 0) - err(1, "%s open failed (try 'hmc5883 start' if the driver is not running", path); + err(1, "%s open failed (try 'hmc5883 start')", path); /* do a simple demand read */ sz = read(fd, &report, sizeof(report)); diff --git a/src/drivers/hmc5883/module.mk b/src/drivers/hmc5883/module.mk index 07377556d3..f51a2b580d 100644 --- a/src/drivers/hmc5883/module.mk +++ b/src/drivers/hmc5883/module.mk @@ -37,7 +37,6 @@ MODULE_COMMAND = hmc5883 -# XXX seems excessive, check if 2048 is sufficient -MODULE_STACKSIZE = 4096 - SRCS = hmc5883.cpp + +MODULE_STACKSIZE = 1200 From c2f97e628cf060d35db22526b450b919f0c6cf59 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Wed, 16 Jul 2014 07:51:47 +0200 Subject: [PATCH 35/59] L3GD20 driver: stack size adjustments of start handler and start return review / comments --- src/drivers/l3gd20/l3gd20.cpp | 3 +++ src/drivers/l3gd20/module.mk | 2 ++ 2 files changed, 5 insertions(+) diff --git a/src/drivers/l3gd20/l3gd20.cpp b/src/drivers/l3gd20/l3gd20.cpp index 64d1a7e552..2273f8aecc 100644 --- a/src/drivers/l3gd20/l3gd20.cpp +++ b/src/drivers/l3gd20/l3gd20.cpp @@ -990,6 +990,9 @@ void info(); /** * Start the driver. + * + * This function call only returns once the driver + * started or failed to detect the sensor. */ void start(bool external_bus, enum Rotation rotation) diff --git a/src/drivers/l3gd20/module.mk b/src/drivers/l3gd20/module.mk index 23e77e8712..83752e08be 100644 --- a/src/drivers/l3gd20/module.mk +++ b/src/drivers/l3gd20/module.mk @@ -4,3 +4,5 @@ MODULE_COMMAND = l3gd20 SRCS = l3gd20.cpp + +MODULE_STACKSIZE = 1200 From fa6f69581ed7dca03ca6ee2cfb8565343400f419 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Wed, 16 Jul 2014 07:52:18 +0200 Subject: [PATCH 36/59] LSM303D: start handler fix and start routine review --- src/drivers/lsm303d/lsm303d.cpp | 3 +++ src/drivers/lsm303d/module.mk | 2 +- 2 files changed, 4 insertions(+), 1 deletion(-) diff --git a/src/drivers/lsm303d/lsm303d.cpp b/src/drivers/lsm303d/lsm303d.cpp index 45e7750552..d70bff69ad 100644 --- a/src/drivers/lsm303d/lsm303d.cpp +++ b/src/drivers/lsm303d/lsm303d.cpp @@ -1803,6 +1803,9 @@ void usage(); /** * Start the driver. + * + * This function call only returns once the driver is + * up and running or failed to detect the sensor. */ void start(bool external_bus, enum Rotation rotation) diff --git a/src/drivers/lsm303d/module.mk b/src/drivers/lsm303d/module.mk index e40f718c54..6e9c22799f 100644 --- a/src/drivers/lsm303d/module.mk +++ b/src/drivers/lsm303d/module.mk @@ -5,4 +5,4 @@ MODULE_COMMAND = lsm303d SRCS = lsm303d.cpp - +MODULE_STACKSIZE = 1200 From 91bedc5c1c84fa79945f32462dbe3fe30c0ff5e4 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Wed, 16 Jul 2014 07:54:09 +0200 Subject: [PATCH 37/59] airspeed drivers: Start handlers stack fixes and start handler review --- src/drivers/ets_airspeed/ets_airspeed.cpp | 3 +++ src/drivers/ets_airspeed/module.mk | 3 ++- src/drivers/meas_airspeed/meas_airspeed.cpp | 3 +++ src/drivers/meas_airspeed/module.mk | 3 ++- 4 files changed, 10 insertions(+), 2 deletions(-) diff --git a/src/drivers/ets_airspeed/ets_airspeed.cpp b/src/drivers/ets_airspeed/ets_airspeed.cpp index c15a0cee43..f98d615a2c 100644 --- a/src/drivers/ets_airspeed/ets_airspeed.cpp +++ b/src/drivers/ets_airspeed/ets_airspeed.cpp @@ -286,6 +286,9 @@ void info(); /** * Start the driver. + * + * This function only returns if the sensor is up and running + * or could not be detected successfully. */ void start(int i2c_bus) diff --git a/src/drivers/ets_airspeed/module.mk b/src/drivers/ets_airspeed/module.mk index 15346c5c55..966a5b8190 100644 --- a/src/drivers/ets_airspeed/module.mk +++ b/src/drivers/ets_airspeed/module.mk @@ -36,6 +36,7 @@ # MODULE_COMMAND = ets_airspeed -MODULE_STACKSIZE = 2048 SRCS = ets_airspeed.cpp + +MODULE_STACKSIZE = 1200 diff --git a/src/drivers/meas_airspeed/meas_airspeed.cpp b/src/drivers/meas_airspeed/meas_airspeed.cpp index 07611f9039..d643fcad61 100644 --- a/src/drivers/meas_airspeed/meas_airspeed.cpp +++ b/src/drivers/meas_airspeed/meas_airspeed.cpp @@ -420,6 +420,9 @@ void info(); /** * Start the driver. + * + * This function call only returns once the driver is up and running + * or failed to detect the sensor. */ void start(int i2c_bus) diff --git a/src/drivers/meas_airspeed/module.mk b/src/drivers/meas_airspeed/module.mk index fed4078b63..84192d1b56 100644 --- a/src/drivers/meas_airspeed/module.mk +++ b/src/drivers/meas_airspeed/module.mk @@ -36,6 +36,7 @@ # MODULE_COMMAND = meas_airspeed -MODULE_STACKSIZE = 2048 SRCS = meas_airspeed.cpp + +MODULE_STACKSIZE = 1200 From 5b38b5e37170ffb079ef228f117f7e8b594353c8 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Wed, 16 Jul 2014 07:55:53 +0200 Subject: [PATCH 38/59] MPU6K: Start handler startup and stack review and adjustments --- src/drivers/mpu6000/module.mk | 5 ++--- src/drivers/mpu6000/mpu6000.cpp | 5 ++++- 2 files changed, 6 insertions(+), 4 deletions(-) diff --git a/src/drivers/mpu6000/module.mk b/src/drivers/mpu6000/module.mk index c7d9cd3ef1..6087e1509e 100644 --- a/src/drivers/mpu6000/module.mk +++ b/src/drivers/mpu6000/module.mk @@ -37,7 +37,6 @@ MODULE_COMMAND = mpu6000 -# XXX seems excessive, check if 2048 is not sufficient -MODULE_STACKSIZE = 4096 - SRCS = mpu6000.cpp + +MODULE_STACKSIZE = 1200 diff --git a/src/drivers/mpu6000/mpu6000.cpp b/src/drivers/mpu6000/mpu6000.cpp index 1b3a96a0d4..442f0a37cc 100644 --- a/src/drivers/mpu6000/mpu6000.cpp +++ b/src/drivers/mpu6000/mpu6000.cpp @@ -1437,6 +1437,9 @@ void usage(); /** * Start the driver. + * + * This function only returns if the driver is up and running + * or failed to detect the sensor. */ void start(bool external_bus, enum Rotation rotation) @@ -1507,7 +1510,7 @@ test(bool external_bus) int fd = open(path_accel, O_RDONLY); if (fd < 0) - err(1, "%s open failed (try 'mpu6000 start' if the driver is not running)", + err(1, "%s open failed (try 'mpu6000 start')", path_accel); /* get the driver */ From b08e3d21cdca5c21396b47280b0958d6592a80d2 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Wed, 16 Jul 2014 08:47:12 +0200 Subject: [PATCH 39/59] Making lowpass filter init bullet proof --- src/lib/mathlib/math/filter/LowPassFilter2p.hpp | 12 ++++++++++-- 1 file changed, 10 insertions(+), 2 deletions(-) diff --git a/src/lib/mathlib/math/filter/LowPassFilter2p.hpp b/src/lib/mathlib/math/filter/LowPassFilter2p.hpp index 74cd5d78c0..4360651751 100644 --- a/src/lib/mathlib/math/filter/LowPassFilter2p.hpp +++ b/src/lib/mathlib/math/filter/LowPassFilter2p.hpp @@ -46,10 +46,18 @@ class __EXPORT LowPassFilter2p { public: // constructor - LowPassFilter2p(float sample_freq, float cutoff_freq) { + LowPassFilter2p(float sample_freq, float cutoff_freq) : + _cutoff_freq(cutoff_freq), + _a1(0.0f), + _a2(0.0f), + _b0(0.0f), + _b1(0.0f), + _b2(0.0f), + _delay_element_1(0.0f), + _delay_element_2(0.0f) + { // set initial parameters set_cutoff_frequency(sample_freq, cutoff_freq); - _delay_element_1 = _delay_element_2 = 0; } /** From 8107205b9ede8d9d36feffb50e116bb8e4dda78a Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Wed, 16 Jul 2014 08:47:31 +0200 Subject: [PATCH 40/59] device driver: Fix compile warnings --- src/drivers/device/ringbuffer.h | 4 ++++ src/drivers/device/spi.h | 5 +++++ 2 files changed, 9 insertions(+) diff --git a/src/drivers/device/ringbuffer.h b/src/drivers/device/ringbuffer.h index a9e22eaa6b..b26e2e7c83 100644 --- a/src/drivers/device/ringbuffer.h +++ b/src/drivers/device/ringbuffer.h @@ -162,6 +162,10 @@ private: volatile unsigned _tail; /**< removal point in _item_size units */ unsigned _next(unsigned index); + + /* we don't want this class to be copied */ + RingBuffer(const RingBuffer&); + RingBuffer operator=(const RingBuffer&); }; RingBuffer::RingBuffer(unsigned num_items, size_t item_size) : diff --git a/src/drivers/device/spi.h b/src/drivers/device/spi.h index 54849c8c37..1d98376898 100644 --- a/src/drivers/device/spi.h +++ b/src/drivers/device/spi.h @@ -129,10 +129,15 @@ private: uint32_t _frequency; struct spi_dev_s *_dev; + /* this class does not allow copying */ + SPI(const SPI&); + SPI operator=(const SPI&); + protected: int _bus; int _transfer(uint8_t *send, uint8_t *recv, unsigned len); + }; } // namespace device From f162a3e8d406e52d42ad84ea80ca6678577b9263 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Wed, 16 Jul 2014 08:47:49 +0200 Subject: [PATCH 41/59] LSM303D: Ensure init and constructors --- src/drivers/lsm303d/lsm303d.cpp | 13 +++++++++++++ src/drivers/lsm303d/module.mk | 2 ++ 2 files changed, 15 insertions(+) diff --git a/src/drivers/lsm303d/lsm303d.cpp b/src/drivers/lsm303d/lsm303d.cpp index d70bff69ad..84ec18f286 100644 --- a/src/drivers/lsm303d/lsm303d.cpp +++ b/src/drivers/lsm303d/lsm303d.cpp @@ -461,6 +461,10 @@ private: * @return OK if the value can be supported. */ int mag_set_samplerate(unsigned frequency); + + /* this class cannot be copied */ + LSM303D(const LSM303D&); + LSM303D operator=(const LSM303D&); }; /** @@ -490,20 +494,28 @@ private: void measure(); void measure_trampoline(void *arg); + + /* this class does not allow copying due to ptr data members */ + LSM303D_mag(const LSM303D_mag&); + LSM303D_mag operator=(const LSM303D_mag&); }; LSM303D::LSM303D(int bus, const char* path, spi_dev_e device, enum Rotation rotation) : SPI("LSM303D", path, bus, device, SPIDEV_MODE3, 11*1000*1000 /* will be rounded to 10.4 MHz, within safety margins for LSM303D */), _mag(new LSM303D_mag(this)), + _accel_call{}, + _mag_call{}, _call_accel_interval(0), _call_mag_interval(0), _accel_reports(nullptr), _mag_reports(nullptr), + _accel_scale{}, _accel_range_m_s2(0.0f), _accel_range_scale(0.0f), _accel_samplerate(0), _accel_onchip_filter_bandwith(0), + _mag_scale{}, _mag_range_ga(0.0f), _mag_range_scale(0.0f), _mag_samplerate(0), @@ -524,6 +536,7 @@ LSM303D::LSM303D(int bus, const char* path, spi_dev_e device, enum Rotation rota _reg7_expected(0), _accel_log_fd(-1), _accel_logging_enabled(false), + _last_extreme_us(0), _last_log_us(0), _last_log_sync_us(0), _last_log_reg_us(0), diff --git a/src/drivers/lsm303d/module.mk b/src/drivers/lsm303d/module.mk index 6e9c22799f..b4f3974f4d 100644 --- a/src/drivers/lsm303d/module.mk +++ b/src/drivers/lsm303d/module.mk @@ -6,3 +6,5 @@ MODULE_COMMAND = lsm303d SRCS = lsm303d.cpp MODULE_STACKSIZE = 1200 + +EXTRACXXFLAGS = -Weffc++ From 5baa3690e02067380f35a551e634b67551ed214f Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Wed, 16 Jul 2014 08:48:10 +0200 Subject: [PATCH 42/59] L3GD20: Ensure init and constructors --- src/drivers/l3gd20/l3gd20.cpp | 6 ++++++ src/drivers/l3gd20/module.mk | 2 ++ 2 files changed, 8 insertions(+) diff --git a/src/drivers/l3gd20/l3gd20.cpp b/src/drivers/l3gd20/l3gd20.cpp index 2273f8aecc..f72db82c09 100644 --- a/src/drivers/l3gd20/l3gd20.cpp +++ b/src/drivers/l3gd20/l3gd20.cpp @@ -330,12 +330,18 @@ private: * @return 0 on success, 1 on failure */ int self_test(); + + /* this class does not allow copying */ + L3GD20(const L3GD20&); + L3GD20 operator=(const L3GD20&); }; L3GD20::L3GD20(int bus, const char* path, spi_dev_e device, enum Rotation rotation) : SPI("L3GD20", path, bus, device, SPIDEV_MODE3, 11*1000*1000 /* will be rounded to 10.4 MHz, within margins for L3GD20 */), + _call{}, _call_interval(0), _reports(nullptr), + _gyro_scale{}, _gyro_range_scale(0.0f), _gyro_range_rad_s(0.0f), _gyro_topic(-1), diff --git a/src/drivers/l3gd20/module.mk b/src/drivers/l3gd20/module.mk index 83752e08be..5630e7aeca 100644 --- a/src/drivers/l3gd20/module.mk +++ b/src/drivers/l3gd20/module.mk @@ -6,3 +6,5 @@ MODULE_COMMAND = l3gd20 SRCS = l3gd20.cpp MODULE_STACKSIZE = 1200 + +EXTRACXXFLAGS = -Weffc++ From e68200b4ba53e6594306e7925aaac3e47f0ef217 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Wed, 16 Jul 2014 08:53:47 +0200 Subject: [PATCH 43/59] HMC driver: Full initialization --- src/drivers/hmc5883/hmc5883.cpp | 5 +++++ src/drivers/hmc5883/module.mk | 2 ++ 2 files changed, 7 insertions(+) diff --git a/src/drivers/hmc5883/hmc5883.cpp b/src/drivers/hmc5883/hmc5883.cpp index 0cde1045c2..3a2069680e 100644 --- a/src/drivers/hmc5883/hmc5883.cpp +++ b/src/drivers/hmc5883/hmc5883.cpp @@ -337,6 +337,9 @@ private: */ int check_offset(); + /* this class has pointer data members, do not allow copying it */ + HMC5883(const HMC5883&); + HMC5883 operator=(const HMC5883&); }; /* @@ -347,8 +350,10 @@ extern "C" __EXPORT int hmc5883_main(int argc, char *argv[]); HMC5883::HMC5883(int bus, const char *path, enum Rotation rotation) : I2C("HMC5883", path, bus, HMC5883L_ADDRESS, 400000), + _work{}, _measure_ticks(0), _reports(nullptr), + _scale{}, _range_scale(0), /* default range scale from counts to gauss */ _range_ga(1.3f), _collect_phase(false), diff --git a/src/drivers/hmc5883/module.mk b/src/drivers/hmc5883/module.mk index f51a2b580d..5daa01dc57 100644 --- a/src/drivers/hmc5883/module.mk +++ b/src/drivers/hmc5883/module.mk @@ -40,3 +40,5 @@ MODULE_COMMAND = hmc5883 SRCS = hmc5883.cpp MODULE_STACKSIZE = 1200 + +EXTRACXXFLAGS = -Weffc++ From 23dd7e752d4760a13e90ec7ed6cf003e56c4baff Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Wed, 16 Jul 2014 08:54:00 +0200 Subject: [PATCH 44/59] airspeed driver: better init --- src/drivers/airspeed/airspeed.h | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/src/drivers/airspeed/airspeed.h b/src/drivers/airspeed/airspeed.h index 0b8e949c9d..d6a88714b6 100644 --- a/src/drivers/airspeed/airspeed.h +++ b/src/drivers/airspeed/airspeed.h @@ -107,6 +107,10 @@ private: RingBuffer *_reports; perf_counter_t _buffer_overflows; + /* this class has pointer data members and should not be copied */ + Airspeed(const Airspeed&); + Airspeed& operator=(const Airspeed&); + protected: virtual int probe(); From da4967e8e4256a31550a10090cbf6bfc12166b61 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Wed, 16 Jul 2014 08:54:17 +0200 Subject: [PATCH 45/59] I2C driver: forbid copy constructor --- src/drivers/device/i2c.h | 3 +++ 1 file changed, 3 insertions(+) diff --git a/src/drivers/device/i2c.h b/src/drivers/device/i2c.h index 5498793523..705b398b05 100644 --- a/src/drivers/device/i2c.h +++ b/src/drivers/device/i2c.h @@ -138,6 +138,9 @@ private: uint16_t _address; uint32_t _frequency; struct i2c_dev_s *_dev; + + I2C(const device::I2C&); + I2C operator=(const device::I2C&); }; } // namespace device From 7ca184de45a49ca2bde5733a54843975e193a237 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Wed, 16 Jul 2014 08:54:40 +0200 Subject: [PATCH 46/59] MEAS airspeed: Stricter initialization and constructors --- src/drivers/meas_airspeed/meas_airspeed.cpp | 4 ++-- src/drivers/meas_airspeed/module.mk | 2 ++ 2 files changed, 4 insertions(+), 2 deletions(-) diff --git a/src/drivers/meas_airspeed/meas_airspeed.cpp b/src/drivers/meas_airspeed/meas_airspeed.cpp index d643fcad61..6ab4377167 100644 --- a/src/drivers/meas_airspeed/meas_airspeed.cpp +++ b/src/drivers/meas_airspeed/meas_airspeed.cpp @@ -140,9 +140,9 @@ extern "C" __EXPORT int meas_airspeed_main(int argc, char *argv[]); MEASAirspeed::MEASAirspeed(int bus, int address, const char *path) : Airspeed(bus, address, CONVERSION_INTERVAL, path), _filter(MEAS_RATE, MEAS_DRIVER_FILTER_FREQ), - _t_system_power(-1) + _t_system_power(-1), + system_power{} { - memset(&system_power, 0, sizeof(system_power)); } int diff --git a/src/drivers/meas_airspeed/module.mk b/src/drivers/meas_airspeed/module.mk index 84192d1b56..2a15b669f4 100644 --- a/src/drivers/meas_airspeed/module.mk +++ b/src/drivers/meas_airspeed/module.mk @@ -40,3 +40,5 @@ MODULE_COMMAND = meas_airspeed SRCS = meas_airspeed.cpp MODULE_STACKSIZE = 1200 + +EXTRACXXFLAGS = -Weffc++ From 164b4ef4ce7e4452e92a3854a51b025f45319e59 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Wed, 16 Jul 2014 08:58:18 +0200 Subject: [PATCH 47/59] MPU6K: Cleaner init --- src/drivers/mpu6000/module.mk | 2 ++ src/drivers/mpu6000/mpu6000.cpp | 9 +++++++++ 2 files changed, 11 insertions(+) diff --git a/src/drivers/mpu6000/module.mk b/src/drivers/mpu6000/module.mk index 6087e1509e..5b4893b127 100644 --- a/src/drivers/mpu6000/module.mk +++ b/src/drivers/mpu6000/module.mk @@ -40,3 +40,5 @@ MODULE_COMMAND = mpu6000 SRCS = mpu6000.cpp MODULE_STACKSIZE = 1200 + +EXTRACXXFLAGS = -Weffc++ diff --git a/src/drivers/mpu6000/mpu6000.cpp b/src/drivers/mpu6000/mpu6000.cpp index 442f0a37cc..db9d4d7e11 100644 --- a/src/drivers/mpu6000/mpu6000.cpp +++ b/src/drivers/mpu6000/mpu6000.cpp @@ -343,6 +343,9 @@ private: */ void _set_sample_rate(uint16_t desired_sample_rate_hz); + /* do not allow to copy this class due to pointer data members */ + MPU6000(const MPU6000&); + MPU6000 operator=(const MPU6000&); }; /** @@ -369,6 +372,9 @@ private: orb_advert_t _gyro_topic; int _gyro_class_instance; + /* do not allow to copy this class due to pointer data members */ + MPU6000_gyro(const MPU6000_gyro&); + MPU6000_gyro operator=(const MPU6000_gyro&); }; /** driver 'main' command */ @@ -378,13 +384,16 @@ MPU6000::MPU6000(int bus, const char *path_accel, const char *path_gyro, spi_dev SPI("MPU6000", path_accel, bus, device, SPIDEV_MODE3, MPU6000_LOW_BUS_SPEED), _gyro(new MPU6000_gyro(this, path_gyro)), _product(0), + _call{}, _call_interval(0), _accel_reports(nullptr), + _accel_scale{}, _accel_range_scale(0.0f), _accel_range_m_s2(0.0f), _accel_topic(-1), _accel_class_instance(-1), _gyro_reports(nullptr), + _gyro_scale{}, _gyro_range_scale(0.0f), _gyro_range_rad_s(0.0f), _sample_rate(1000), From 85ccecea9c67b9f0a61fa46ec8dee324114077c1 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Wed, 16 Jul 2014 09:05:15 +0200 Subject: [PATCH 48/59] FMU driver: stricter init --- src/drivers/px4fmu/fmu.cpp | 5 +++++ src/drivers/px4fmu/module.mk | 2 ++ 2 files changed, 7 insertions(+) diff --git a/src/drivers/px4fmu/fmu.cpp b/src/drivers/px4fmu/fmu.cpp index 8cc1141aa5..82977a032f 100644 --- a/src/drivers/px4fmu/fmu.cpp +++ b/src/drivers/px4fmu/fmu.cpp @@ -179,6 +179,9 @@ private: uint32_t gpio_read(void); int gpio_ioctl(file *filp, int cmd, unsigned long arg); + /* do not allow to copy due to ptr data members */ + PX4FMU(const PX4FMU&); + PX4FMU operator=(const PX4FMU&); }; const PX4FMU::GPIOConfig PX4FMU::_gpio_tab[] = { @@ -242,6 +245,7 @@ PX4FMU::PX4FMU() : _task(-1), _armed_sub(-1), _outputs_pub(-1), + _armed{}, _num_outputs(0), _primary_pwm_device(false), _task_should_exit(false), @@ -252,6 +256,7 @@ PX4FMU::PX4FMU() : _groups_subscribed(0), _control_subs{-1}, _poll_fds_num(0), + _pwm_limit{}, _failsafe_pwm{0}, _disarmed_pwm{0}, _num_failsafe_set(0), diff --git a/src/drivers/px4fmu/module.mk b/src/drivers/px4fmu/module.mk index eeb59e1a1f..a60f1a434a 100644 --- a/src/drivers/px4fmu/module.mk +++ b/src/drivers/px4fmu/module.mk @@ -6,3 +6,5 @@ MODULE_COMMAND = fmu SRCS = fmu.cpp MODULE_STACKSIZE = 1200 + +EXTRACXXFLAGS = -Weffc++ From c5b13b7dbcb36f75d88fa24b879c3f202d01da38 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Wed, 16 Jul 2014 09:05:29 +0200 Subject: [PATCH 49/59] IO driver: stricter init --- src/drivers/px4io/module.mk | 2 ++ src/drivers/px4io/px4io.cpp | 5 +++++ 2 files changed, 7 insertions(+) diff --git a/src/drivers/px4io/module.mk b/src/drivers/px4io/module.mk index c14f1f7836..5b838fb75e 100644 --- a/src/drivers/px4io/module.mk +++ b/src/drivers/px4io/module.mk @@ -46,3 +46,5 @@ SRCS = px4io.cpp \ INCLUDE_DIRS += $(NUTTX_SRC)/arch/arm/src/stm32 $(NUTTX_SRC)/arch/arm/src/common MODULE_STACKSIZE = 1200 + +EXTRACXXFLAGS = -Weffc++ diff --git a/src/drivers/px4io/px4io.cpp b/src/drivers/px4io/px4io.cpp index 7d78b0d275..711674886c 100644 --- a/src/drivers/px4io/px4io.cpp +++ b/src/drivers/px4io/px4io.cpp @@ -453,6 +453,9 @@ private: */ void io_handle_vservo(uint16_t vservo, uint16_t vrssi); + /* do not allow to copy this class due to ptr data members */ + PX4IO(const PX4IO&); + PX4IO operator=(const PX4IO&); }; namespace @@ -496,6 +499,8 @@ PX4IO::PX4IO(device::Device *interface) : _to_battery(0), _to_servorail(0), _to_safety(0), + _outputs{}, + _servorail_status{}, _primary_pwm_device(false), _lockdown_override(false), _battery_amp_per_volt(90.0f / 5.0f), // this matches the 3DR current sensor From 654aaa0ca852b95e4e2bec5cf9b77ca3242d1d63 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Wed, 16 Jul 2014 09:05:50 +0200 Subject: [PATCH 50/59] Mixer: forbid copy constructors due to ptr data members --- src/modules/systemlib/mixer/mixer.h | 15 +++++++++++++++ 1 file changed, 15 insertions(+) diff --git a/src/modules/systemlib/mixer/mixer.h b/src/modules/systemlib/mixer/mixer.h index 225570fa40..cdf60febc7 100644 --- a/src/modules/systemlib/mixer/mixer.h +++ b/src/modules/systemlib/mixer/mixer.h @@ -231,6 +231,10 @@ protected: static const char * skipline(const char *buf, unsigned &buflen); private: + + /* do not allow to copy due to prt data members */ + Mixer(const Mixer&); + Mixer& operator=(const Mixer&); }; /** @@ -307,6 +311,10 @@ public: private: Mixer *_first; /**< linked list of mixers */ + + /* do not allow to copy due to pointer data members */ + MixerGroup(const MixerGroup&); + MixerGroup operator=(const MixerGroup&); }; /** @@ -424,6 +432,10 @@ private: mixer_scaler_s &scaler, uint8_t &control_group, uint8_t &control_index); + + /* do not allow to copy due to ptr data members */ + SimpleMixer(const SimpleMixer&); + SimpleMixer operator=(const SimpleMixer&); }; /** @@ -522,6 +534,9 @@ private: unsigned _rotor_count; const Rotor *_rotors; + /* do not allow to copy due to ptr data members */ + MultirotorMixer(const MultirotorMixer&); + MultirotorMixer operator=(const MultirotorMixer&); }; #endif From d39d5cc9da99ec17251428d57d232c30564a663b Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Wed, 16 Jul 2014 11:26:32 +0200 Subject: [PATCH 51/59] SYS_EXT_MAG parameter added for magnetometer selection --- ROMFS/px4fmu_common/init.d/rc.sensors | 27 +++++++++++++++++---------- src/modules/systemlib/system_params.c | 11 +++++++++++ 2 files changed, 28 insertions(+), 10 deletions(-) diff --git a/ROMFS/px4fmu_common/init.d/rc.sensors b/ROMFS/px4fmu_common/init.d/rc.sensors index be54ea98b0..f50e9aff7f 100644 --- a/ROMFS/px4fmu_common/init.d/rc.sensors +++ b/ROMFS/px4fmu_common/init.d/rc.sensors @@ -6,12 +6,6 @@ ms5611 start adc start -# Mag might be external -if hmc5883 start -then - echo "[init] Using HMC5883" -fi - if mpu6000 start then echo "[init] Using MPU6000" @@ -22,12 +16,25 @@ then echo "[init] Using L3GD20(H)" fi -if ver hwcmp PX4FMU_V2 +# Use selected (internal/external) magnetometer +if param compare SYS_EXT_MAG 0 then - # IMPORTANT: EXTERNAL BUSES SHOULD BE SCANNED FIRST - if lsm303d start + if hmc5883 -I start then - echo "[init] Using LSM303D" + echo "[init] Using internal HMC5883" + fi + + if ver hwcmp PX4FMU_V2 + then + if lsm303d start + then + echo "[init] Using internal LSM303D" + fi + fi +else + if hmc5883 -X start + then + echo "[init] Using external HMC5883" fi fi diff --git a/src/modules/systemlib/system_params.c b/src/modules/systemlib/system_params.c index 702e435ac9..8cb9236200 100644 --- a/src/modules/systemlib/system_params.c +++ b/src/modules/systemlib/system_params.c @@ -82,3 +82,14 @@ PARAM_DEFINE_INT32(SYS_USE_IO, 1); * @group System */ PARAM_DEFINE_INT32(SYS_RESTART_TYPE, 2); + +/** +* Set usage of external magnetometer +* +* Set to 1 to use external magnetometer instead of internal one. +* +* @min 0 +* @max 1 +* @group System +*/ +PARAM_DEFINE_INT32(SYS_EXT_MAG, 0); From 0c5ce3d3a23a32556b38bb0936b315d12290e4ba Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Wed, 16 Jul 2014 13:42:47 +0200 Subject: [PATCH 52/59] Hotfix: Allow the HMC5883 driver to continue to operate in auto mode --- src/drivers/hmc5883/hmc5883.cpp | 9 +++++++-- 1 file changed, 7 insertions(+), 2 deletions(-) diff --git a/src/drivers/hmc5883/hmc5883.cpp b/src/drivers/hmc5883/hmc5883.cpp index 4aef43102e..03a7e6f37e 100644 --- a/src/drivers/hmc5883/hmc5883.cpp +++ b/src/drivers/hmc5883/hmc5883.cpp @@ -1341,8 +1341,8 @@ namespace hmc5883 #endif const int ERROR = -1; -HMC5883 *g_dev_int; -HMC5883 *g_dev_ext; +HMC5883 *g_dev_int = nullptr; +HMC5883 *g_dev_ext = nullptr; void start(int bus, enum Rotation rotation); void test(int bus); @@ -1378,6 +1378,11 @@ start(int bus, enum Rotation rotation) errx(0, "already started internal"); g_dev_int = new HMC5883(PX4_I2C_BUS_ONBOARD, HMC5883L_DEVICE_PATH_INT, rotation); if (g_dev_int != nullptr && OK != g_dev_int->init()) { + + /* tear down the failing onboard instance */ + delete g_dev_int; + g_dev_int = nullptr; + if (bus == PX4_I2C_BUS_ONBOARD) { goto fail; } From 698fcb6b8687b5901145ec90462297c398fbf5c3 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Wed, 16 Jul 2014 14:22:48 +0200 Subject: [PATCH 53/59] commander: only warn about lost link if link was working before --- src/modules/commander/commander.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 0c4d48dead..953feec2a7 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -1441,7 +1441,7 @@ int commander_thread_main(int argc, char *argv[]) /* data links check */ bool have_link = false; for (int i = 0; i < TELEMETRY_STATUS_ORB_ID_NUM; i++) { - if (hrt_elapsed_time(&telemetry_last_heartbeat[i]) < DL_TIMEOUT) { + if (telemetry_last_heartbeat[i] != 0 && hrt_elapsed_time(&telemetry_last_heartbeat[i]) < DL_TIMEOUT) { /* handle the case where data link was regained */ if (telemetry_lost[i]) { mavlink_log_critical(mavlink_fd, "data link %i regained", i); From c6c9c49823a4c19e156f4ce70bde781890ab04f9 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Wed, 16 Jul 2014 14:58:43 +0200 Subject: [PATCH 54/59] Implement the external mag param in a fashion that retains backward compatibility --- ROMFS/px4fmu_common/init.d/rc.sensors | 40 +++++++++++++++++---------- src/modules/sensors/sensor_params.c | 13 +++++++++ src/modules/systemlib/system_params.c | 11 -------- 3 files changed, 38 insertions(+), 26 deletions(-) diff --git a/ROMFS/px4fmu_common/init.d/rc.sensors b/ROMFS/px4fmu_common/init.d/rc.sensors index f50e9aff7f..121dc89d3b 100644 --- a/ROMFS/px4fmu_common/init.d/rc.sensors +++ b/ROMFS/px4fmu_common/init.d/rc.sensors @@ -8,33 +8,43 @@ adc start if mpu6000 start then - echo "[init] Using MPU6000" + echo "Internal MPU6000" fi if l3gd20 start then - echo "[init] Using L3GD20(H)" + echo "Internal L3GD20(H)" fi -# Use selected (internal/external) magnetometer -if param compare SYS_EXT_MAG 0 +# MAG selection +if param compare SENS_EXT_MAG 2 then if hmc5883 -I start then - echo "[init] Using internal HMC5883" - fi - - if ver hwcmp PX4FMU_V2 - then - if lsm303d start - then - echo "[init] Using internal LSM303D" - fi + echo "Internal HMC5883" fi else - if hmc5883 -X start + # Use only external as primary + if param compare SENS_EXT_MAG 1 then - echo "[init] Using external HMC5883" + if hmc5883 -X start + then + echo "External HMC5883" + fi + else + # auto-detect the primary, prefer external + if hmc5883 start + then + echo "Default HMC5883" + fi + fi +fi + +if ver hwcmp PX4FMU_V2 +then + if lsm303d start + then + echo "Internal LSM303D" fi fi diff --git a/src/modules/sensors/sensor_params.c b/src/modules/sensors/sensor_params.c index 38b1907612..7ce6ef5ef6 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/systemlib/system_params.c b/src/modules/systemlib/system_params.c index 8cb9236200..702e435ac9 100644 --- a/src/modules/systemlib/system_params.c +++ b/src/modules/systemlib/system_params.c @@ -82,14 +82,3 @@ PARAM_DEFINE_INT32(SYS_USE_IO, 1); * @group System */ PARAM_DEFINE_INT32(SYS_RESTART_TYPE, 2); - -/** -* Set usage of external magnetometer -* -* Set to 1 to use external magnetometer instead of internal one. -* -* @min 0 -* @max 1 -* @group System -*/ -PARAM_DEFINE_INT32(SYS_EXT_MAG, 0); From ede1deaed6303c9ba22663fec950b9a2a66c53e5 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Wed, 16 Jul 2014 15:02:25 +0200 Subject: [PATCH 55/59] Add support for external sensors in startup --- ROMFS/px4fmu_common/init.d/rc.sensors | 15 +++++++++++++++ 1 file changed, 15 insertions(+) diff --git a/ROMFS/px4fmu_common/init.d/rc.sensors b/ROMFS/px4fmu_common/init.d/rc.sensors index 121dc89d3b..5a755b89e4 100644 --- a/ROMFS/px4fmu_common/init.d/rc.sensors +++ b/ROMFS/px4fmu_common/init.d/rc.sensors @@ -6,11 +6,21 @@ ms5611 start adc start +if mpu6000 -X start +then + echo "External MPU6000" +fi + if mpu6000 start then echo "Internal MPU6000" fi +if l3gd20 -X start +then + echo "External L3GD20(H)" +fi + if l3gd20 start then echo "Internal L3GD20(H)" @@ -42,6 +52,11 @@ fi if ver hwcmp PX4FMU_V2 then + if lsm303d -X start + then + echo "External LSM303D" + fi + if lsm303d start then echo "Internal LSM303D" From 0b81d9883a67a6277d85a603fde4ebbbc001d20f Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Wed, 16 Jul 2014 15:17:33 +0200 Subject: [PATCH 56/59] Strip excessive binary text to save some flash, start all three sensor sets --- ROMFS/px4fmu_common/init.d/rc.sensors | 11 ----------- 1 file changed, 11 deletions(-) diff --git a/ROMFS/px4fmu_common/init.d/rc.sensors b/ROMFS/px4fmu_common/init.d/rc.sensors index 5a755b89e4..ecb408a543 100644 --- a/ROMFS/px4fmu_common/init.d/rc.sensors +++ b/ROMFS/px4fmu_common/init.d/rc.sensors @@ -8,22 +8,18 @@ adc start if mpu6000 -X start then - echo "External MPU6000" fi if mpu6000 start then - echo "Internal MPU6000" fi if l3gd20 -X start then - echo "External L3GD20(H)" fi if l3gd20 start then - echo "Internal L3GD20(H)" fi # MAG selection @@ -31,7 +27,6 @@ if param compare SENS_EXT_MAG 2 then if hmc5883 -I start then - echo "Internal HMC5883" fi else # Use only external as primary @@ -39,13 +34,11 @@ else then if hmc5883 -X start then - echo "External HMC5883" fi else # auto-detect the primary, prefer external if hmc5883 start then - echo "Default HMC5883" fi fi fi @@ -54,12 +47,10 @@ if ver hwcmp PX4FMU_V2 then if lsm303d -X start then - echo "External LSM303D" fi if lsm303d start then - echo "Internal LSM303D" fi fi @@ -70,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 From c2565d998359c29a66c9ebbc641ea748186c7455 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Wed, 16 Jul 2014 15:17:54 +0200 Subject: [PATCH 57/59] PCA8574: flash efficiency --- src/drivers/pca8574/module.mk | 2 ++ 1 file changed, 2 insertions(+) diff --git a/src/drivers/pca8574/module.mk b/src/drivers/pca8574/module.mk index 825ee9bb7a..c53ed9ab2b 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 From ca98070f8b479f5d643d810a077ad09e84d32721 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Wed, 16 Jul 2014 15:18:07 +0200 Subject: [PATCH 58/59] RGBLED: flash efficiency --- src/drivers/rgbled/module.mk | 2 ++ 1 file changed, 2 insertions(+) diff --git a/src/drivers/rgbled/module.mk b/src/drivers/rgbled/module.mk index 39b53ec9ed..c287e35f31 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 From d9e3c9423e7d78a71acb31b0a2b3f9cc8b31a55d Mon Sep 17 00:00:00 2001 From: Jean Cyr Date: Wed, 16 Jul 2014 23:41:13 -0400 Subject: [PATCH 59/59] Clean up mavlink module Deal with all mavlink module compiler warnings. No critical errors found --- src/modules/mavlink/mavlink_messages.cpp | 144 +++++++++++++++++++++++ 1 file changed, 144 insertions(+) diff --git a/src/modules/mavlink/mavlink_messages.cpp b/src/modules/mavlink/mavlink_messages.cpp index 7c864f1277..6885bebde5 100644 --- a/src/modules/mavlink/mavlink_messages.cpp +++ b/src/modules/mavlink/mavlink_messages.cpp @@ -258,7 +258,16 @@ private: MavlinkOrbSubscription *status_sub; MavlinkOrbSubscription *pos_sp_triplet_sub; + /* do not allow top copying this class */ + MavlinkStreamHeartbeat(MavlinkStreamHeartbeat &); + MavlinkStreamHeartbeat& operator = (const MavlinkStreamHeartbeat &); + protected: + explicit MavlinkStreamHeartbeat() : MavlinkStream(), + status_sub(nullptr), + pos_sp_triplet_sub(nullptr) + {} + void subscribe(Mavlink *mavlink) { status_sub = mavlink->add_orb_subscription(ORB_ID(vehicle_status)); @@ -322,7 +331,15 @@ public: private: MavlinkOrbSubscription *status_sub; + /* do not allow top copying this class */ + MavlinkStreamSysStatus(MavlinkStreamSysStatus &); + MavlinkStreamSysStatus& operator = (const MavlinkStreamSysStatus &); + protected: + explicit MavlinkStreamSysStatus() : MavlinkStream(), + status_sub(nullptr) + {} + void subscribe(Mavlink *mavlink) { status_sub = mavlink->add_orb_subscription(ORB_ID(vehicle_status)); @@ -384,8 +401,13 @@ private: uint64_t mag_timestamp; uint64_t baro_timestamp; + /* do not allow top copying this class */ + MavlinkStreamHighresIMU(MavlinkStreamHighresIMU &); + MavlinkStreamHighresIMU& operator = (const MavlinkStreamHighresIMU &); + protected: explicit MavlinkStreamHighresIMU() : MavlinkStream(), + sensor_sub(nullptr), sensor_time(0), accel_timestamp(0), gyro_timestamp(0), @@ -469,8 +491,14 @@ private: MavlinkOrbSubscription *att_sub; uint64_t att_time; + /* do not allow top copying this class */ + MavlinkStreamAttitude(MavlinkStreamAttitude &); + MavlinkStreamAttitude& operator = (const MavlinkStreamAttitude &); + + protected: explicit MavlinkStreamAttitude() : MavlinkStream(), + att_sub(nullptr), att_time(0) {} @@ -520,8 +548,13 @@ private: MavlinkOrbSubscription *att_sub; uint64_t att_time; + /* do not allow top copying this class */ + MavlinkStreamAttitudeQuaternion(MavlinkStreamAttitudeQuaternion &); + MavlinkStreamAttitudeQuaternion& operator = (const MavlinkStreamAttitudeQuaternion &); + protected: explicit MavlinkStreamAttitudeQuaternion() : MavlinkStream(), + att_sub(nullptr), att_time(0) {} @@ -589,12 +622,21 @@ private: MavlinkOrbSubscription *airspeed_sub; uint64_t airspeed_time; + /* do not allow top copying this class */ + MavlinkStreamVFRHUD(MavlinkStreamVFRHUD &); + MavlinkStreamVFRHUD& operator = (const MavlinkStreamVFRHUD &); + protected: explicit MavlinkStreamVFRHUD() : MavlinkStream(), + att_sub(nullptr), att_time(0), + pos_sub(nullptr), pos_time(0), + armed_sub(nullptr), armed_time(0), + act_sub(nullptr), act_time(0), + airspeed_sub(nullptr), airspeed_time(0) {} @@ -665,8 +707,13 @@ private: MavlinkOrbSubscription *gps_sub; uint64_t gps_time; + /* do not allow top copying this class */ + MavlinkStreamGPSRawInt(MavlinkStreamGPSRawInt &); + MavlinkStreamGPSRawInt& operator = (const MavlinkStreamGPSRawInt &); + protected: explicit MavlinkStreamGPSRawInt() : MavlinkStream(), + gps_sub(nullptr), gps_time(0) {} @@ -726,9 +773,15 @@ private: MavlinkOrbSubscription *home_sub; uint64_t home_time; + /* do not allow top copying this class */ + MavlinkStreamGlobalPositionInt(MavlinkStreamGlobalPositionInt &); + MavlinkStreamGlobalPositionInt& operator = (const MavlinkStreamGlobalPositionInt &); + protected: explicit MavlinkStreamGlobalPositionInt() : MavlinkStream(), + pos_sub(nullptr), pos_time(0), + home_sub(nullptr), home_time(0) {} @@ -789,8 +842,13 @@ private: MavlinkOrbSubscription *pos_sub; uint64_t pos_time; + /* do not allow top copying this class */ + MavlinkStreamLocalPositionNED(MavlinkStreamLocalPositionNED &); + MavlinkStreamLocalPositionNED& operator = (const MavlinkStreamLocalPositionNED &); + protected: explicit MavlinkStreamLocalPositionNED() : MavlinkStream(), + pos_sub(nullptr), pos_time(0) {} @@ -845,8 +903,13 @@ private: MavlinkOrbSubscription *pos_sub; uint64_t pos_time; + /* do not allow top copying this class */ + MavlinkStreamViconPositionEstimate(MavlinkStreamViconPositionEstimate &); + MavlinkStreamViconPositionEstimate& operator = (const MavlinkStreamViconPositionEstimate &); + protected: explicit MavlinkStreamViconPositionEstimate() : MavlinkStream(), + pos_sub(nullptr), pos_time(0) {} @@ -899,7 +962,15 @@ public: private: MavlinkOrbSubscription *home_sub; + /* do not allow top copying this class */ + MavlinkStreamGPSGlobalOrigin(MavlinkStreamGPSGlobalOrigin &); + MavlinkStreamGPSGlobalOrigin& operator = (const MavlinkStreamGPSGlobalOrigin &); + protected: + explicit MavlinkStreamGPSGlobalOrigin() : MavlinkStream(), + home_sub(nullptr) + {} + void subscribe(Mavlink *mavlink) { home_sub = mavlink->add_orb_subscription(ORB_ID(home_position)); @@ -962,8 +1033,13 @@ private: MavlinkOrbSubscription *act_sub; uint64_t act_time; + /* do not allow top copying this class */ + MavlinkStreamServoOutputRaw(MavlinkStreamServoOutputRaw &); + MavlinkStreamServoOutputRaw& operator = (const MavlinkStreamServoOutputRaw &); + protected: explicit MavlinkStreamServoOutputRaw() : MavlinkStream(), + act_sub(nullptr), act_time(0) {} @@ -1033,10 +1109,17 @@ private: MavlinkOrbSubscription *act_sub; uint64_t act_time; + /* do not allow top copying this class */ + MavlinkStreamHILControls(MavlinkStreamHILControls &); + MavlinkStreamHILControls& operator = (const MavlinkStreamHILControls &); + protected: explicit MavlinkStreamHILControls() : MavlinkStream(), + status_sub(nullptr), status_time(0), + pos_sp_triplet_sub(nullptr), pos_sp_triplet_time(0), + act_sub(nullptr), act_time(0) {} @@ -1159,7 +1242,15 @@ public: private: MavlinkOrbSubscription *pos_sp_triplet_sub; + /* do not allow top copying this class */ + MavlinkStreamGlobalPositionSetpointInt(MavlinkStreamGlobalPositionSetpointInt &); + MavlinkStreamGlobalPositionSetpointInt& operator = (const MavlinkStreamGlobalPositionSetpointInt &); + protected: + explicit MavlinkStreamGlobalPositionSetpointInt() : MavlinkStream(), + pos_sp_triplet_sub(nullptr) + {} + void subscribe(Mavlink *mavlink) { pos_sp_triplet_sub = mavlink->add_orb_subscription(ORB_ID(position_setpoint_triplet)); @@ -1208,8 +1299,13 @@ private: MavlinkOrbSubscription *pos_sp_sub; uint64_t pos_sp_time; + /* do not allow top copying this class */ + MavlinkStreamLocalPositionSetpoint(MavlinkStreamLocalPositionSetpoint &); + MavlinkStreamLocalPositionSetpoint& operator = (const MavlinkStreamLocalPositionSetpoint &); + protected: explicit MavlinkStreamLocalPositionSetpoint() : MavlinkStream(), + pos_sp_sub(nullptr), pos_sp_time(0) {} @@ -1261,8 +1357,13 @@ private: MavlinkOrbSubscription *att_sp_sub; uint64_t att_sp_time; + /* do not allow top copying this class */ + MavlinkStreamRollPitchYawThrustSetpoint(MavlinkStreamRollPitchYawThrustSetpoint &); + MavlinkStreamRollPitchYawThrustSetpoint& operator = (const MavlinkStreamRollPitchYawThrustSetpoint &); + protected: explicit MavlinkStreamRollPitchYawThrustSetpoint() : MavlinkStream(), + att_sp_sub(nullptr), att_sp_time(0) {} @@ -1314,8 +1415,13 @@ private: MavlinkOrbSubscription *att_rates_sp_sub; uint64_t att_rates_sp_time; + /* do not allow top copying this class */ + MavlinkStreamRollPitchYawRatesThrustSetpoint(MavlinkStreamRollPitchYawRatesThrustSetpoint &); + MavlinkStreamRollPitchYawRatesThrustSetpoint& operator = (const MavlinkStreamRollPitchYawRatesThrustSetpoint &); + protected: explicit MavlinkStreamRollPitchYawRatesThrustSetpoint() : MavlinkStream(), + att_rates_sp_sub(nullptr), att_rates_sp_time(0) {} @@ -1367,8 +1473,13 @@ private: MavlinkOrbSubscription *rc_sub; uint64_t rc_time; + /* do not allow top copying this class */ + MavlinkStreamRCChannelsRaw(MavlinkStreamRCChannelsRaw &); + MavlinkStreamRCChannelsRaw& operator = (const MavlinkStreamRCChannelsRaw &); + protected: explicit MavlinkStreamRCChannelsRaw() : MavlinkStream(), + rc_sub(nullptr), rc_time(0) {} @@ -1456,8 +1567,13 @@ private: MavlinkOrbSubscription *manual_sub; uint64_t manual_time; + /* do not allow top copying this class */ + MavlinkStreamManualControl(MavlinkStreamManualControl &); + MavlinkStreamManualControl& operator = (const MavlinkStreamManualControl &); + protected: explicit MavlinkStreamManualControl() : MavlinkStream(), + manual_sub(nullptr), manual_time(0) {} @@ -1510,8 +1626,13 @@ private: MavlinkOrbSubscription *flow_sub; uint64_t flow_time; + /* do not allow top copying this class */ + MavlinkStreamOpticalFlow(MavlinkStreamOpticalFlow &); + MavlinkStreamOpticalFlow& operator = (const MavlinkStreamOpticalFlow &); + protected: explicit MavlinkStreamOpticalFlow() : MavlinkStream(), + flow_sub(nullptr), flow_time(0) {} @@ -1563,8 +1684,13 @@ private: MavlinkOrbSubscription *att_ctrl_sub; uint64_t att_ctrl_time; + /* do not allow top copying this class */ + MavlinkStreamAttitudeControls(MavlinkStreamAttitudeControls &); + MavlinkStreamAttitudeControls& operator = (const MavlinkStreamAttitudeControls &); + protected: explicit MavlinkStreamAttitudeControls() : MavlinkStream(), + att_ctrl_sub(nullptr), att_ctrl_time(0) {} @@ -1626,8 +1752,13 @@ private: MavlinkOrbSubscription *debug_sub; uint64_t debug_time; + /* do not allow top copying this class */ + MavlinkStreamNamedValueFloat(MavlinkStreamNamedValueFloat &); + MavlinkStreamNamedValueFloat& operator = (const MavlinkStreamNamedValueFloat &); + protected: explicit MavlinkStreamNamedValueFloat() : MavlinkStream(), + debug_sub(nullptr), debug_time(0) {} @@ -1678,7 +1809,15 @@ public: private: MavlinkOrbSubscription *status_sub; + /* do not allow top copying this class */ + MavlinkStreamCameraCapture(MavlinkStreamCameraCapture &); + MavlinkStreamCameraCapture& operator = (const MavlinkStreamCameraCapture &); + protected: + explicit MavlinkStreamCameraCapture() : MavlinkStream(), + status_sub(nullptr) + {} + void subscribe(Mavlink *mavlink) { status_sub = mavlink->add_orb_subscription(ORB_ID(vehicle_status)); @@ -1729,8 +1868,13 @@ private: MavlinkOrbSubscription *range_sub; uint64_t range_time; + /* do not allow top copying this class */ + MavlinkStreamDistanceSensor(MavlinkStreamDistanceSensor &); + MavlinkStreamDistanceSensor& operator = (const MavlinkStreamDistanceSensor &); + protected: explicit MavlinkStreamDistanceSensor() : MavlinkStream(), + range_sub(nullptr), range_time(0) {}