From 37e96e230ccb056dc4d43113214901694c00b34b Mon Sep 17 00:00:00 2001 From: TSC21 Date: Tue, 19 May 2015 23:25:33 +0100 Subject: [PATCH 01/19] distance_sensor: first topic commit --- src/modules/uORB/topics/distance_sensor.h | 76 +++++++++++++++++++++++ 1 file changed, 76 insertions(+) create mode 100644 src/modules/uORB/topics/distance_sensor.h diff --git a/src/modules/uORB/topics/distance_sensor.h b/src/modules/uORB/topics/distance_sensor.h new file mode 100644 index 0000000000..ea123c7faf --- /dev/null +++ b/src/modules/uORB/topics/distance_sensor.h @@ -0,0 +1,76 @@ +/**************************************************************************** + * + * Copyright (C) 2015 PX4 Development Team. All rights reserved. + * Author: @author Nuno Marques + * + * 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 distance_sensor.h + * Definition of the distance sensor uORB topic. + */ + +#ifndef TOPIC_DISTANCE_SENSOR_H_ +#define TOPIC_DISTANCE_SENSOR_H_ + +#include +#include +#include "../uORB.h" + +/** + * @addtogroup topics + */ + +/** + * Distance sensor data, with range in centimeters (not in SI). + * + * @see http://en.wikipedia.org/wiki/International_System_of_Units + */ +struct distance_sensor_s { + + uint32_t time_boot_ms; /**< Time since system boot */ + uint16_t min_distance; /**< Minimum distance the sensor can measure in centimeters */ + uint16_t max_distance; /**< Maximum distance the sensor can measure in centimeters */ + uint16_t current_distance; /**< Current distance reading */ + uint8_t type; /**< Type from MAV_DISTANCE_SENSOR enum */ + uint8_t id; /**< Onboard ID of the sensor */ + uint8_t orientation; /**< Direction the sensor faces from MAV_SENSOR_ORIENTATION enum */ + uint8_t covariance; /**< Measurement covariance in centimeters, 0 for unknown / invalid readings */ + +}; + +/** + * @} + */ + +/* register this as object request broker structure */ +ORB_DECLARE(distance_sensor); + +#endif \ No newline at end of file From 364492a325f25fa5dce10af28a03afb435294846 Mon Sep 17 00:00:00 2001 From: TSC21 Date: Tue, 19 May 2015 23:42:15 +0100 Subject: [PATCH 02/19] distance_sensor: added def to objects_common.cpp --- src/modules/uORB/objects_common.cpp | 3 +++ 1 file changed, 3 insertions(+) diff --git a/src/modules/uORB/objects_common.cpp b/src/modules/uORB/objects_common.cpp index eb89b79585..b34f9a742d 100644 --- a/src/modules/uORB/objects_common.cpp +++ b/src/modules/uORB/objects_common.cpp @@ -256,3 +256,6 @@ ORB_DEFINE(time_offset, struct time_offset_s); #include "topics/mc_att_ctrl_status.h" ORB_DEFINE(mc_att_ctrl_status, struct mc_att_ctrl_status_s); + +#include "topics/distance_sensor.h" +ORB_DEFINE(distance_sensor, struct distance_sensor_s); \ No newline at end of file From 30218b0de709a0bf38f4b5c0cc104caf66f4e343 Mon Sep 17 00:00:00 2001 From: TSC21 Date: Wed, 20 May 2015 00:15:27 +0100 Subject: [PATCH 03/19] distance_sensor: added mavlink_receiver handler for msg --- src/modules/mavlink/mavlink_receiver.cpp | 36 +++++++++++++++++++++++- src/modules/mavlink/mavlink_receiver.h | 3 ++ 2 files changed, 38 insertions(+), 1 deletion(-) diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index 3c4fa76a0c..b84d381c6f 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -134,7 +134,8 @@ MavlinkReceiver::MavlinkReceiver(Mavlink *parent) : _att_sp{}, _rates_sp{}, _time_offset_avg_alpha(0.6), - _time_offset(0) + _time_offset(0), + _distance_sensor_pub(-1) { } @@ -211,6 +212,10 @@ MavlinkReceiver::handle_message(mavlink_message_t *msg) handle_message_timesync(msg); break; + case MAVLINK_MSG_ID_DISTANCE_SENSOR: + handle_message_distance_sensor(msg); + break; + default: break; } @@ -495,6 +500,35 @@ MavlinkReceiver::handle_message_set_mode(mavlink_message_t *msg) } } +void +MavlinkReceiver::handle_message_distance_sensor(mavlink_message_t *msg) +{ + /* distance sensor */ + mavlink_distance_sensor_t dist_sensor; + mavlink_msg_distance_sensor_decode(msg, &dist_sensor); + + struct distance_sensor_s d; + memset(&d, 0, sizeof(d)); + + d.time_boot_ms = dist_sensor.time_boot_ms; + d.min_distance = dist_sensor.min_distance; + d.max_distance = dist_sensor.max_distance; + d.current_distance = dist_sensor.current_distance; + d.type = dist_sensor.type; + d.id = dist_sensor.id; + d.orientation = dist_sensor.orientation; + d.covariance = dist_sensor.covariance; + + /// TODO Add sensor rotation according to MAV_SENSOR_ORIENTATION enum + + if (_distance_sensor_pub < 0) { + _distance_sensor_pub = orb_advertise(ORB_ID(distance_sensor), &d); + + } else { + orb_publish(ORB_ID(distance_sensor), _distance_sensor_pub, &d); + } +} + void MavlinkReceiver::handle_message_vicon_position_estimate(mavlink_message_t *msg) { diff --git a/src/modules/mavlink/mavlink_receiver.h b/src/modules/mavlink/mavlink_receiver.h index 887d2f88ed..7b592ab51a 100644 --- a/src/modules/mavlink/mavlink_receiver.h +++ b/src/modules/mavlink/mavlink_receiver.h @@ -74,6 +74,7 @@ #include #include #include +#include #include "mavlink_ftp.h" @@ -135,6 +136,7 @@ private: void handle_message_hil_sensor(mavlink_message_t *msg); void handle_message_hil_gps(mavlink_message_t *msg); void handle_message_hil_state_quaternion(mavlink_message_t *msg); + void handle_message_distance_sensor(mavlink_message_t *msg); void *receive_thread(void *arg); @@ -191,6 +193,7 @@ private: struct vehicle_rates_setpoint_s _rates_sp; double _time_offset_avg_alpha; uint64_t _time_offset; + orb_advert_t _distance_sensor_pub; /* do not allow copying this class */ MavlinkReceiver(const MavlinkReceiver &); From 796b105382c0a9962bf94047b9be03120c064515 Mon Sep 17 00:00:00 2001 From: TSC21 Date: Wed, 20 May 2015 00:54:36 +0100 Subject: [PATCH 04/19] distance_sensor: updated distance_sensor stream sub --- src/modules/mavlink/mavlink_messages.cpp | 42 ++++++++++++++---------- 1 file changed, 25 insertions(+), 17 deletions(-) diff --git a/src/modules/mavlink/mavlink_messages.cpp b/src/modules/mavlink/mavlink_messages.cpp index e62d223e0c..c8f0f6da0b 100644 --- a/src/modules/mavlink/mavlink_messages.cpp +++ b/src/modules/mavlink/mavlink_messages.cpp @@ -69,6 +69,7 @@ #include #include #include +#include #include #include #include @@ -2179,7 +2180,6 @@ protected: } }; - class MavlinkStreamDistanceSensor : public MavlinkStream { public: @@ -2205,12 +2205,12 @@ public: unsigned get_size() { - return _range_sub->is_published() ? (MAVLINK_MSG_ID_DISTANCE_SENSOR_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES) : 0; + return _distance_sensor_sub->is_published() ? (MAVLINK_MSG_ID_DISTANCE_SENSOR_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES) : 0; } private: - MavlinkOrbSubscription *_range_sub; - uint64_t _range_time; + MavlinkOrbSubscription *_distance_sensor_sub; + uint64_t _dist_sensor_time; /* do not allow top copying this class */ MavlinkStreamDistanceSensor(MavlinkStreamDistanceSensor &); @@ -2218,36 +2218,44 @@ private: protected: explicit MavlinkStreamDistanceSensor(Mavlink *mavlink) : MavlinkStream(mavlink), - _range_sub(_mavlink->add_orb_subscription(ORB_ID(sensor_range_finder))), - _range_time(0) + _distance_sensor_sub(_mavlink->add_orb_subscription(ORB_ID(distance_sensor))), + _dist_sensor_time(0) {} void send(const hrt_abstime t) { - struct range_finder_report range; + struct distance_sensor_s dist_sensor; - if (_range_sub->update(&_range_time, &range)) { + if (_distance_sensor_sub->update(&_dist_sensor_time, &dist_sensor)) { mavlink_distance_sensor_t msg; - msg.time_boot_ms = range.timestamp / 1000; + msg.time_boot_ms = dist_sensor.time_boot_ms; - switch (range.type) { - case RANGE_FINDER_TYPE_LASER: + switch (dist_sensor.type) { + case MAV_DISTANCE_SENSOR_ULTRASOUND: msg.type = MAV_DISTANCE_SENSOR_LASER; break; + case MAV_DISTANCE_SENSOR_LASER: + msg.type = MAV_DISTANCE_SENSOR_LASER; + break; + + /*case MAV_DISTANCE_SENSOR_INFRARED: + msg.type = MAV_DISTANCE_SENSOR_INFRARED; + break;*/ //TODO: update mavlink def + default: msg.type = MAV_DISTANCE_SENSOR_LASER; break; } - msg.id = 0; - msg.orientation = 0; - msg.min_distance = range.minimum_distance * 100; - msg.max_distance = range.maximum_distance * 100; - msg.current_distance = range.distance * 100; - msg.covariance = 20; + msg.id = dist_sensor.id; + msg.orientation = dist_sensor.orientation; + msg.min_distance = dist_sensor.min_distance; + msg.max_distance = dist_sensor.max_distance; + msg.current_distance = dist_sensor.current_distance; + msg.covariance = dist_sensor.covariance; _mavlink->send_message(MAVLINK_MSG_ID_DISTANCE_SENSOR, &msg); } From c180b5d8251b5d9259e403c15ce3bb9babf73241 Mon Sep 17 00:00:00 2001 From: TSC21 Date: Wed, 20 May 2015 12:40:15 +0100 Subject: [PATCH 05/19] distance_sensor: added msg definition to 'msg' folder --- CMakeLists.txt | 1 + msg/distance_sensor.msg | 17 +++++++ src/modules/uORB/topics/distance_sensor.h | 58 +++++++++++++---------- 3 files changed, 50 insertions(+), 26 deletions(-) create mode 100644 msg/distance_sensor.msg diff --git a/CMakeLists.txt b/CMakeLists.txt index 457a0bfa74..bfd7b662b1 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -80,6 +80,7 @@ add_message_files( vehicle_global_velocity_setpoint.msg offboard_control_mode.msg vehicle_force_setpoint.msg + distance_sensor.msg ) ## Generate services in the 'srv' folder diff --git a/msg/distance_sensor.msg b/msg/distance_sensor.msg new file mode 100644 index 0000000000..ef11c586fc --- /dev/null +++ b/msg/distance_sensor.msg @@ -0,0 +1,17 @@ +# DISTANCE_SENSOR message data + +uint32 time_boot_ms + +uint16 min_distance +uint16 max_distance +uint16 current_distance + +uint8 type +uint8 MAV_DISTANCE_SENSOR_LASER = 0 +uint8 MAV_DISTANCE_SENSOR_ULTRASOUND = 1 +uint8 MAV_DISTANCE_SENSOR_INFRARED = 2 + +uint8 id + +uint8 orientation +uint8 covariance \ No newline at end of file diff --git a/src/modules/uORB/topics/distance_sensor.h b/src/modules/uORB/topics/distance_sensor.h index ea123c7faf..b3eb9e4f0f 100644 --- a/src/modules/uORB/topics/distance_sensor.h +++ b/src/modules/uORB/topics/distance_sensor.h @@ -1,7 +1,6 @@ /**************************************************************************** * - * Copyright (C) 2015 PX4 Development Team. All rights reserved. - * Author: @author Nuno Marques + * Copyright (C) 2013-2015 PX4 Development Team. All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -32,38 +31,47 @@ * ****************************************************************************/ -/** - * @file distance_sensor.h - * Definition of the distance sensor uORB topic. - */ +/* Auto-generated by genmsg_cpp from file /home/nuno/px4/Firmware/msg/distance_sensor.msg */ -#ifndef TOPIC_DISTANCE_SENSOR_H_ -#define TOPIC_DISTANCE_SENSOR_H_ + +#pragma once #include -#include -#include "../uORB.h" +#include + + +#ifndef __cplusplus +#define MAV_DISTANCE_SENSOR_LASER 0 +#define MAV_DISTANCE_SENSOR_ULTRASOUND 1 +#define MAV_DISTANCE_SENSOR_INFRARED 2 + +#endif /** * @addtogroup topics + * @{ */ -/** - * Distance sensor data, with range in centimeters (not in SI). - * - * @see http://en.wikipedia.org/wiki/International_System_of_Units - */ + +#ifdef __cplusplus +struct __EXPORT distance_sensor_s { +#else struct distance_sensor_s { +#endif + uint32_t time_boot_ms; + uint16_t min_distance; + uint16_t max_distance; + uint16_t current_distance; + uint8_t type; + uint8_t id; + uint8_t orientation; + uint8_t covariance; +#ifdef __cplusplus + static const uint8_t MAV_DISTANCE_SENSOR_LASER = 0; + static const uint8_t MAV_DISTANCE_SENSOR_ULTRASOUND = 1; + static const uint8_t MAV_DISTANCE_SENSOR_INFRARED = 2; - uint32_t time_boot_ms; /**< Time since system boot */ - uint16_t min_distance; /**< Minimum distance the sensor can measure in centimeters */ - uint16_t max_distance; /**< Maximum distance the sensor can measure in centimeters */ - uint16_t current_distance; /**< Current distance reading */ - uint8_t type; /**< Type from MAV_DISTANCE_SENSOR enum */ - uint8_t id; /**< Onboard ID of the sensor */ - uint8_t orientation; /**< Direction the sensor faces from MAV_SENSOR_ORIENTATION enum */ - uint8_t covariance; /**< Measurement covariance in centimeters, 0 for unknown / invalid readings */ - +#endif }; /** @@ -72,5 +80,3 @@ struct distance_sensor_s { /* register this as object request broker structure */ ORB_DECLARE(distance_sensor); - -#endif \ No newline at end of file From 80c7719b42307a2716b6a58848af6e8f61a4b788 Mon Sep 17 00:00:00 2001 From: TSC21 Date: Wed, 20 May 2015 12:51:20 +0100 Subject: [PATCH 06/19] distance_sensor: typo correction on msg.type --- src/modules/mavlink/mavlink_messages.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/mavlink/mavlink_messages.cpp b/src/modules/mavlink/mavlink_messages.cpp index c8f0f6da0b..cc00fc69b8 100644 --- a/src/modules/mavlink/mavlink_messages.cpp +++ b/src/modules/mavlink/mavlink_messages.cpp @@ -2234,7 +2234,7 @@ protected: switch (dist_sensor.type) { case MAV_DISTANCE_SENSOR_ULTRASOUND: - msg.type = MAV_DISTANCE_SENSOR_LASER; + msg.type = MAV_DISTANCE_SENSOR_ULTRASOUND; break; case MAV_DISTANCE_SENSOR_LASER: From 1978f1bcac1d854d035f78672decd8d4ac67f6ce Mon Sep 17 00:00:00 2001 From: TSC21 Date: Wed, 20 May 2015 17:37:54 +0100 Subject: [PATCH 07/19] distance_sensor: remove 'distance_sensor.h' autogenerated header --- src/modules/uORB/topics/distance_sensor.h | 82 ----------------------- 1 file changed, 82 deletions(-) delete mode 100644 src/modules/uORB/topics/distance_sensor.h diff --git a/src/modules/uORB/topics/distance_sensor.h b/src/modules/uORB/topics/distance_sensor.h deleted file mode 100644 index b3eb9e4f0f..0000000000 --- a/src/modules/uORB/topics/distance_sensor.h +++ /dev/null @@ -1,82 +0,0 @@ -/**************************************************************************** - * - * Copyright (C) 2013-2015 PX4 Development Team. All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * 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. - * - ****************************************************************************/ - -/* Auto-generated by genmsg_cpp from file /home/nuno/px4/Firmware/msg/distance_sensor.msg */ - - -#pragma once - -#include -#include - - -#ifndef __cplusplus -#define MAV_DISTANCE_SENSOR_LASER 0 -#define MAV_DISTANCE_SENSOR_ULTRASOUND 1 -#define MAV_DISTANCE_SENSOR_INFRARED 2 - -#endif - -/** - * @addtogroup topics - * @{ - */ - - -#ifdef __cplusplus -struct __EXPORT distance_sensor_s { -#else -struct distance_sensor_s { -#endif - uint32_t time_boot_ms; - uint16_t min_distance; - uint16_t max_distance; - uint16_t current_distance; - uint8_t type; - uint8_t id; - uint8_t orientation; - uint8_t covariance; -#ifdef __cplusplus - static const uint8_t MAV_DISTANCE_SENSOR_LASER = 0; - static const uint8_t MAV_DISTANCE_SENSOR_ULTRASOUND = 1; - static const uint8_t MAV_DISTANCE_SENSOR_INFRARED = 2; - -#endif -}; - -/** - * @} - */ - -/* register this as object request broker structure */ -ORB_DECLARE(distance_sensor); From db48df15c8e141eb8c4eea5a20aec9ebdeb049d2 Mon Sep 17 00:00:00 2001 From: TSC21 Date: Thu, 21 May 2015 12:26:55 +0100 Subject: [PATCH 08/19] Merge branch 'master' --- Makefile | 22 ++- Tools/generate_listener.py | 4 +- makefiles/firmware_posix.mk | 2 +- makefiles/firmware_qurt.mk | 2 +- makefiles/qurt/config_qurt_hello.mk | 73 ++++++++ msg/filtered_bottom_flow.msg | 8 + msg/home_position.msg | 10 + msg/optical_flow.msg | 17 ++ msg/range_finder.msg | 13 ++ msg/sensor_combined.msg | 100 ++++++++++ msg/vehicle_global_position.msg | 20 ++ msg/vehicle_gps_position.msg | 29 +++ nuttx-configs/px4fmu-v2/nsh/defconfig | 4 +- src/drivers/drv_range_finder.h | 37 +--- src/drivers/sf0x/sf0x.cpp | 41 +++-- src/include/containers/List.hpp | 14 +- .../commander/accelerometer_calibration.cpp | 26 ++- src/modules/controllib/block/Block.cpp | 5 + src/modules/controllib/block/Block.hpp | 12 +- src/modules/controllib/block/BlockParam.cpp | 2 + src/modules/controllib/block/BlockParam.hpp | 2 + src/modules/controllib/uorb/blocks.cpp | 1 + src/modules/controllib/uorb/blocks.hpp | 4 - src/modules/mavlink/mavlink_main.cpp | 4 + src/modules/mavlink/mavlink_messages.cpp | 1 - src/modules/mavlink/mavlink_receiver.h | 1 - src/modules/sensors/sensors.cpp | 46 ++--- src/modules/uORB/Publication.cpp | 8 + src/modules/uORB/Publication.hpp | 12 +- src/modules/uORB/Subscription.cpp | 2 + src/modules/uORB/Subscription.hpp | 5 + .../uORB/topics/actuator_controls_effective.h | 79 -------- src/modules/uORB/topics/optical_flow.h | 84 --------- src/modules/uORB/topics/sensor_combined.h | 172 ------------------ .../uORB/topics/vehicle_global_position.h | 87 --------- .../uORB/topics/vehicle_gps_position.h | 94 ---------- .../posix/drivers/airspeedsim/airspeedsim.cpp | 2 +- .../qurt/px4_layer/commands_default.c} | 71 ++++---- .../qurt/px4_layer/commands_hello.c} | 50 +---- src/platforms/qurt/px4_layer/main.cpp | 30 +-- src/platforms/qurt/px4_layer/module.mk | 8 +- src/systemcmds/tests/test_eigen.cpp | 24 +-- src/systemcmds/tests/tests_main.c | 2 +- 43 files changed, 481 insertions(+), 749 deletions(-) create mode 100644 makefiles/qurt/config_qurt_hello.mk create mode 100644 msg/filtered_bottom_flow.msg create mode 100644 msg/home_position.msg create mode 100644 msg/optical_flow.msg create mode 100644 msg/range_finder.msg create mode 100644 msg/sensor_combined.msg create mode 100644 msg/vehicle_global_position.msg create mode 100644 msg/vehicle_gps_position.msg delete mode 100644 src/modules/uORB/topics/actuator_controls_effective.h delete mode 100644 src/modules/uORB/topics/optical_flow.h delete mode 100644 src/modules/uORB/topics/sensor_combined.h delete mode 100644 src/modules/uORB/topics/vehicle_global_position.h delete mode 100644 src/modules/uORB/topics/vehicle_gps_position.h rename src/{modules/uORB/topics/filtered_bottom_flow.h => platforms/qurt/px4_layer/commands_default.c} (63%) rename src/{modules/uORB/topics/home_position.h => platforms/qurt/px4_layer/commands_hello.c} (62%) diff --git a/Makefile b/Makefile index 4a0f13cd19..3f78f32362 100644 --- a/Makefile +++ b/Makefile @@ -33,6 +33,12 @@ # Top-level Makefile for building PX4 firmware images. # +TARGETS := nuttx posix qurt +EXPLICIT_TARGET := $(filter $(TARGETS),$(MAKECMDGOALS)) +ifneq ($(EXPLICIT_TARGET),) + export PX4_TARGET_OS=$(EXPLICIT_TARGET) +endif + # # Get path and tool configuration # @@ -271,14 +277,14 @@ testbuild: $(Q) (cd $(PX4_BASE) && $(MAKE) distclean && $(MAKE) archives && $(MAKE) -j8) $(Q) (zip -r Firmware.zip $(PX4_BASE)/Images) -posix: - make PX4_TARGET_OS=posix +nuttx: + make PX4_TARGET_OS=$@ $(wordlist 2,$(words $(MAKECMDGOALS)),$(MAKECMDGOALS)) -nuttx: - make PX4_TARGET_OS=nuttx +posix: + make PX4_TARGET_OS=$@ $(wordlist 2,$(words $(MAKECMDGOALS)),$(MAKECMDGOALS)) -qurt: - make PX4_TARGET_OS=qurt +qurt: + make PX4_TARGET_OS=$@ $(wordlist 2,$(words $(MAKECMDGOALS)),$(MAKECMDGOALS)) posixrun: Tools/posix_run.sh @@ -327,9 +333,11 @@ help: @$(ECHO) " Available targets:" @$(ECHO) " ------------------" @$(ECHO) "" +ifeq ($(PX4_TARGET_OS),nuttx) @$(ECHO) " archives" @$(ECHO) " Build the NuttX RTOS archives that are used by the firmware build." @$(ECHO) "" +endif @$(ECHO) " all" @$(ECHO) " Build all firmware configs: $(CONFIGS)" @$(ECHO) " A limited set of configs can be built with CONFIGS=" @@ -342,6 +350,7 @@ help: @$(ECHO) " clean" @$(ECHO) " Remove all firmware build pieces." @$(ECHO) "" +ifeq ($(PX4_TARGET_OS),nuttx) @$(ECHO) " distclean" @$(ECHO) " Remove all compilation products, including NuttX RTOS archives." @$(ECHO) "" @@ -350,6 +359,7 @@ help: @$(ECHO) " firmware to the board when the build is complete. Not supported for" @$(ECHO) " all configurations." @$(ECHO) "" +endif @$(ECHO) " testbuild" @$(ECHO) " Perform a complete clean build of the entire tree." @$(ECHO) "" diff --git a/Tools/generate_listener.py b/Tools/generate_listener.py index e52d3cd26f..5cfe4a6281 100755 --- a/Tools/generate_listener.py +++ b/Tools/generate_listener.py @@ -91,6 +91,8 @@ print """ #include #include #include +#define __STDC_FORMAT_MACROS +#include """ for m in messages: print "#include " % m @@ -135,7 +137,7 @@ for index,m in enumerate(messages[1:]): print "\t\t\t}" print "\t\t\tprintf(\"\\n\");" elif item[0] == "uint64": - print "\t\t\tprintf(\"%s: %%lu\\n \",container.%s);" % (item[1], item[1]) + print "\t\t\tprintf(\"%s: %%\" PRIu64 \"\\n \",container.%s);" % (item[1], item[1]) elif item[0] == "uint8": print "\t\t\tprintf(\"%s: %%u\\n \",container.%s);" % (item[1], item[1]) elif item[0] == "bool": diff --git a/makefiles/firmware_posix.mk b/makefiles/firmware_posix.mk index 6acec4e733..912e7bcc3f 100644 --- a/makefiles/firmware_posix.mk +++ b/makefiles/firmware_posix.mk @@ -32,7 +32,7 @@ # # Built products # -FIRMWARES = $(foreach config,$(KNOWN_CONFIGS),$(BUILD_DIR)$(config).build/firmware.a) +FIRMWARES = $(foreach config,$(CONFIGS),$(BUILD_DIR)$(config).build/firmware.a) all: $(FIRMWARES) diff --git a/makefiles/firmware_qurt.mk b/makefiles/firmware_qurt.mk index 41128fa60f..a367e4a327 100644 --- a/makefiles/firmware_qurt.mk +++ b/makefiles/firmware_qurt.mk @@ -32,7 +32,7 @@ # # Built products # -FIRMWARES = $(foreach config,$(KNOWN_CONFIGS),$(BUILD_DIR)$(config).build/firmware.a) +FIRMWARES = $(foreach config,$(CONFIGS),$(BUILD_DIR)$(config).build/firmware.a) all: $(FIRMWARES) diff --git a/makefiles/qurt/config_qurt_hello.mk b/makefiles/qurt/config_qurt_hello.mk new file mode 100644 index 0000000000..4b11a79fe3 --- /dev/null +++ b/makefiles/qurt/config_qurt_hello.mk @@ -0,0 +1,73 @@ +# +# Makefile for the Foo *default* configuration +# + +# +# Board support modules +# +MODULES += drivers/device +#MODULES += drivers/blinkm +#MODULES += drivers/hil +#MODULES += drivers/led +#MODULES += drivers/rgbled +#MODULES += modules/sensors +#MODULES += drivers/ms5611 + +# +# System commands +# +MODULES += systemcmds/param + +# +# General system control +# +#MODULES += modules/mavlink + +# +# Estimation modules (EKF/ SO3 / other filters) +# +#MODULES += modules/attitude_estimator_ekf +#MODULES += modules/ekf_att_pos_estimator + +# +# Vehicle Control +# +#MODULES += modules/mc_att_control + +# +# Library modules +# +MODULES += modules/systemlib +#MODULES += modules/systemlib/mixer +MODULES += modules/uORB +#MODULES += modules/dataman +#MODULES += modules/sdlog2 +#MODULES += modules/simulator +#MODULES += modules/commander + +# +# Libraries +# +#MODULES += lib/mathlib +#MODULES += lib/mathlib/math/filter +#MODULES += lib/geo +#MODULES += lib/geo_lookup +#MODULES += lib/conversion + +# +# QuRT port +# +MODULES += platforms/qurt/px4_layer +#MODULES += platforms/posix/drivers/accelsim +#MODULES += platforms/posix/drivers/gyrosim +#MODULES += platforms/posix/drivers/adcsim +#MODULES += platforms/posix/drivers/barosim + +# +# Unit tests +# +MODULES += platforms/qurt/tests/hello +#MODULES += platforms/posix/tests/vcdev_test +#MODULES += platforms/posix/tests/hrt_test +#MODULES += platforms/posix/tests/wqueue + diff --git a/msg/filtered_bottom_flow.msg b/msg/filtered_bottom_flow.msg new file mode 100644 index 0000000000..815a38414d --- /dev/null +++ b/msg/filtered_bottom_flow.msg @@ -0,0 +1,8 @@ +# Filtered bottom flow in bodyframe. +uint64 timestamp # time of this estimate, in microseconds since system start + +float32 sumx # Integrated bodyframe x flow in meters +float32 sumy # Integrated bodyframe y flow in meters + +float32 vx # Flow bodyframe x speed, m/s +float32 vy # Flow bodyframe y Speed, m/s diff --git a/msg/home_position.msg b/msg/home_position.msg new file mode 100644 index 0000000000..d8aff3f634 --- /dev/null +++ b/msg/home_position.msg @@ -0,0 +1,10 @@ +# GPS home position in WGS84 coordinates. +uint64 timestamp # Timestamp (microseconds since system boot) + +float64 lat # Latitude in degrees +float64 lon # Longitude in degrees +float32 alt # Altitude in meters (AMSL) + +float32 x # X coordinate in meters +float32 y # Y coordinate in meters +float32 z # Z coordinate in meters diff --git a/msg/optical_flow.msg b/msg/optical_flow.msg new file mode 100644 index 0000000000..d34e32b8fd --- /dev/null +++ b/msg/optical_flow.msg @@ -0,0 +1,17 @@ +# Optical flow in NED body frame in SI units. +# @see http://en.wikipedia.org/wiki/International_System_of_Units + +uint64 timestamp # in microseconds since system start +uint8 sensor_id # id of the sensor emitting the flow value +float32 pixel_flow_x_integral # accumulated optical flow in radians around x axis +float32 pixel_flow_y_integral # accumulated optical flow in radians around y axis +float32 gyro_x_rate_integral # accumulated gyro value in radians around x axis +float32 gyro_y_rate_integral # accumulated gyro value in radians around y axis +float32 gyro_z_rate_integral # accumulated gyro value in radians around z axis +float32 ground_distance_m # Altitude / distance to ground in meters +uint32 integration_timespan # accumulation timespan in microseconds +uint32 time_since_last_sonar_update # time since last sonar update in microseconds +uint16 frame_count_since_last_readout # number of accumulated frames in timespan +int16 gyro_temperature # Temperature * 100 in centi-degrees Celsius +uint8 quality # Average of quality of accumulated frames, 0: bad quality, 255: maximum quality + diff --git a/msg/range_finder.msg b/msg/range_finder.msg new file mode 100644 index 0000000000..d2e67543e5 --- /dev/null +++ b/msg/range_finder.msg @@ -0,0 +1,13 @@ +int16 RANGE_FINDER_TYPE_LASER = 0 + +# range finder report structure. Reads from the device must be in multiples of this +# structure. +uint64 timestamp +uint64 error_count +uint16 type # type, following RANGE_FINDER_TYPE enum +float32 distance # in meters +float32 minimum_distance # minimum distance the sensor can measure +float32 maximum_distance # maximum distance the sensor can measure +uint8 valid # 1 == within sensor range, 0 = outside sensor range +float32[12] distance_vector # in meters length should match MB12XX_MAX_RANGEFINDERS +uint8 just_updated # number of the most recent measurement sensor diff --git a/msg/sensor_combined.msg b/msg/sensor_combined.msg new file mode 100644 index 0000000000..c5e427e194 --- /dev/null +++ b/msg/sensor_combined.msg @@ -0,0 +1,100 @@ +# Definition of the sensor_combined uORB topic. + +int32 MAGNETOMETER_MODE_NORMAL = 0 +int32 MAGNETOMETER_MODE_POSITIVE_BIAS = 1 +int32 MAGNETOMETER_MODE_NEGATIVE_BIAS = 2 + +# Sensor readings in raw and SI-unit form. +# +# These values are read from the sensors. Raw values are in sensor-specific units, +# the scaled values are in SI-units, as visible from the ending of the variable +# or the comments. The use of the SI fields is in general advised, as these fields +# are scaled and offset-compensated where possible and do not change with board +# revisions and sensor updates. +# +# Actual data, this is specific to the type of data which is stored in this struct +# A line containing L0GME will be added by the Python logging code generator to the logged dataset. +# +# NOTE: Ordering of fields optimized to align to 32 bit / 4 bytes Change with consideration only + +uint64 timestamp # Timestamp in microseconds since boot, from gyro +# +int16[3] gyro_raw # Raw sensor values of angular velocity +float32[3] gyro_rad_s # Angular velocity in radian per seconds +uint32 gyro_errcount # Error counter for gyro 0 +float32 gyro_temp # Temperature of gyro 0 + +int16[3] accelerometer_raw # Raw acceleration in NED body frame +float32[3] accelerometer_m_s2 # Acceleration in NED body frame, in m/s^2 +int16 accelerometer_mode # Accelerometer measurement mode +float32 accelerometer_range_m_s2 # Accelerometer measurement range in m/s^2 +uint64 accelerometer_timestamp # Accelerometer timestamp +uint32 accelerometer_errcount # Error counter for accel 0 +float32 accelerometer_temp # Temperature of accel 0 + +int16[3] magnetometer_raw # Raw magnetic field in NED body frame +float32[3] magnetometer_ga # Magnetic field in NED body frame, in Gauss +int16 magnetometer_mode # Magnetometer measurement mode +float32 magnetometer_range_ga # measurement range in Gauss +float32 magnetometer_cuttoff_freq_hz # Internal analog low pass frequency of sensor +uint64 magnetometer_timestamp # Magnetometer timestamp +uint32 magnetometer_errcount # Error counter for mag 0 +float32 magnetometer_temp # Temperature of mag 0 + +int16[3] gyro1_raw # Raw sensor values of angular velocity +float32[3] gyro1_rad_s # Angular velocity in radian per seconds +uint64 gyro1_timestamp # Gyro timestamp +uint32 gyro1_errcount # Error counter for gyro 1 +float32 gyro1_temp # Temperature of gyro 1 + +int16[3] accelerometer1_raw # Raw acceleration in NED body frame +float32[3] accelerometer1_m_s2 # Acceleration in NED body frame, in m/s^2 +uint64 accelerometer1_timestamp # Accelerometer timestamp +uint32 accelerometer1_errcount # Error counter for accel 1 +float32 accelerometer1_temp # Temperature of accel 1 + +int16[3] magnetometer1_raw # Raw magnetic field in NED body frame +float32[3] magnetometer1_ga # Magnetic field in NED body frame, in Gauss +uint64 magnetometer1_timestamp # Magnetometer timestamp +uint32 magnetometer1_errcount # Error counter for mag 1 +float32 magnetometer1_temp # Temperature of mag 1 + +int16[3] gyro2_raw # Raw sensor values of angular velocity +float32[3] gyro2_rad_s # Angular velocity in radian per seconds +uint64 gyro2_timestamp # Gyro timestamp +uint32 gyro2_errcount # Error counter for gyro 1 +float32 gyro2_temp # Temperature of gyro 1 + +int16[3] accelerometer2_raw # Raw acceleration in NED body frame +float32[3] accelerometer2_m_s2 # Acceleration in NED body frame, in m/s^2 +uint64 accelerometer2_timestamp # Accelerometer timestamp +uint32 accelerometer2_errcount # Error counter for accel 2 +float32 accelerometer2_temp # Temperature of accel 2 + +int16[3] magnetometer2_raw # Raw magnetic field in NED body frame +float32[3] magnetometer2_ga # Magnetic field in NED body frame, in Gauss +uint64 magnetometer2_timestamp # Magnetometer timestamp +uint32 magnetometer2_errcount # Error counter for mag 2 +float32 magnetometer2_temp # Temperature of mag 2 + +float32 baro_pres_mbar # Barometric pressure, already temp. comp. +float32 baro_alt_meter # Altitude, already temp. comp. +float32 baro_temp_celcius # Temperature in degrees celsius +uint64 baro_timestamp # Barometer timestamp + +float32 baro1_pres_mbar # Barometric pressure, already temp. comp. +float32 baro1_alt_meter # Altitude, already temp. comp. +float32 baro1_temp_celcius # Temperature in degrees celsius +uint64 baro1_timestamp # Barometer timestamp + +float32[10] adc_voltage_v # ADC voltages of ADC Chan 10/11/12/13 or -1 +uint16[10] adc_mapping # Channel indices of each of these values +float32 mcu_temp_celcius # Internal temperature measurement of MCU + +float32 differential_pressure_pa # Airspeed sensor differential pressure +uint64 differential_pressure_timestamp # Last measurement timestamp +float32 differential_pressure_filtered_pa # Low pass filtered airspeed sensor differential pressure reading + +float32 differential_pressure1_pa # Airspeed sensor differential pressure +uint64 differential_pressure1_timestamp # Last measurement timestamp +float32 differential_pressure1_filtered_pa # Low pass filtered airspeed sensor differential pressure reading diff --git a/msg/vehicle_global_position.msg b/msg/vehicle_global_position.msg new file mode 100644 index 0000000000..6a38e2c9d7 --- /dev/null +++ b/msg/vehicle_global_position.msg @@ -0,0 +1,20 @@ +# Fused global position in WGS84. +# This struct contains global position estimation. It is not the raw GPS +# measurement (@see vehicle_gps_position). This topic is usually published by the position +# estimator, which will take more sources of information into account than just GPS, +# e.g. control inputs of the vehicle in a Kalman-filter implementation. +# +uint64 timestamp # Time of this estimate, in microseconds since system start +uint64 time_utc_usec # GPS UTC timestamp in microseconds +float64 lat # Latitude in degrees +float64 lon # Longitude in degrees +float32 alt # Altitude AMSL in meters +float32 vel_n # Ground north velocity, m/s +float32 vel_e # Ground east velocity, m/s +float32 vel_d # Ground downside velocity, m/s +float32 yaw # Yaw in radians -PI..+PI. +float32 eph # Standard deviation of position estimate horizontally +float32 epv # Standard deviation of position vertically +float32 terrain_alt # Terrain altitude in m, WGS84 +bool terrain_alt_valid # Terrain altitude estimate is valid +bool dead_reckoning # True if this position is estimated through dead-reckoning diff --git a/msg/vehicle_gps_position.msg b/msg/vehicle_gps_position.msg new file mode 100644 index 0000000000..25c532ccaa --- /dev/null +++ b/msg/vehicle_gps_position.msg @@ -0,0 +1,29 @@ +# GPS position in WGS84 coordinates. +uint64 timestamp_position # Timestamp for position information +int32 lat # Latitude in 1E-7 degrees +int32 lon # Longitude in 1E-7 degrees +int32 alt # Altitude in 1E-3 meters (millimeters) above MSL + +uint64 timestamp_variance +float32 s_variance_m_s # speed accuracy estimate m/s +float32 c_variance_rad # course accuracy estimate rad +uint8 fix_type # 0-1: no fix, 2: 2D fix, 3: 3D fix, 4: RTCM code differential, 5: Real-Time Kinematic, float, 6: Real-Time Kinematic, fixed, 8: Extrapolated. Some applications will not use the value of this field unless it is at least two, so always correctly fill in the fix. + +float32 eph # GPS HDOP horizontal dilution of position in m +float32 epv # GPS VDOP horizontal dilution of position in m + +int32 noise_per_ms # GPS noise per millisecond +int32 jamming_indicator # indicates jamming is occurring + +uint64 timestamp_velocity # Timestamp for velocity informations +float32 vel_m_s # GPS ground speed (m/s) +float32 vel_n_m_s # GPS ground speed in m/s +float32 vel_e_m_s # GPS ground speed in m/s +float32 vel_d_m_s # GPS ground speed in m/s +float32 cog_rad # Course over ground (NOT heading, but direction of movement) in rad, -PI..PI +bool vel_ned_valid # Flag to indicate if NED speed is valid + +uint64 timestamp_time # Timestamp for time information +uint64 time_utc_usec # Timestamp (microseconds, UTC), this is the timestamp which comes from the gps module. It might be unavailable right after cold start, indicated by a value of 0 + +uint8 satellites_used # Number of satellites used diff --git a/nuttx-configs/px4fmu-v2/nsh/defconfig b/nuttx-configs/px4fmu-v2/nsh/defconfig index ab8f9eef62..19e0f7c632 100644 --- a/nuttx-configs/px4fmu-v2/nsh/defconfig +++ b/nuttx-configs/px4fmu-v2/nsh/defconfig @@ -560,8 +560,8 @@ CONFIG_USART1_2STOP=0 # # USART2 Configuration # -CONFIG_USART2_RXBUFSIZE=512 -CONFIG_USART2_TXBUFSIZE=512 +CONFIG_USART2_RXBUFSIZE=600 +CONFIG_USART2_TXBUFSIZE=2200 CONFIG_USART2_BAUD=57600 CONFIG_USART2_BITS=8 CONFIG_USART2_PARITY=0 diff --git a/src/drivers/drv_range_finder.h b/src/drivers/drv_range_finder.h index cc91569cd9..bd72971b94 100644 --- a/src/drivers/drv_range_finder.h +++ b/src/drivers/drv_range_finder.h @@ -48,39 +48,14 @@ #define RANGE_FINDER0_DEVICE_PATH "/dev/range_finder0" #define MB12XX_MAX_RANGEFINDERS 12 //Maximum number of RangeFinders that can be connected -enum RANGE_FINDER_TYPE { - RANGE_FINDER_TYPE_LASER = 0, -}; +#define range_finder_report range_finder_s +#define __orb_sensor_range_finder __orb_range_finder -/** - * @addtogroup topics - * @{ - */ +#include -/** - * range finder report structure. Reads from the device must be in multiples of this - * structure. - */ -struct range_finder_report { - uint64_t timestamp; - uint64_t error_count; - unsigned type; /**< type, following RANGE_FINDER_TYPE enum */ - float distance; /**< in meters */ - float minimum_distance; /**< minimum distance the sensor can measure */ - float maximum_distance; /**< maximum distance the sensor can measure */ - uint8_t valid; /**< 1 == within sensor range, 0 = outside sensor range */ - float distance_vector[MB12XX_MAX_RANGEFINDERS]; /** in meters */ - uint8_t just_updated; /** number of the most recent measurement sensor */ -}; - -/** - * @} - */ - -/* - * ObjDev tag for raw range finder data. - */ -ORB_DECLARE(sensor_range_finder); +#ifndef RANGE_FINDER_TYPE_LASER +#define RANGE_FINDER_TYPE_LASER 0 +#endif /* * ioctl() definitions diff --git a/src/drivers/sf0x/sf0x.cpp b/src/drivers/sf0x/sf0x.cpp index 5c9978f1f0..c45e73fd9a 100644 --- a/src/drivers/sf0x/sf0x.cpp +++ b/src/drivers/sf0x/sf0x.cpp @@ -260,32 +260,37 @@ SF0X::~SF0X() int SF0X::init() { - /* do regular cdev init */ - if (CDev::init() != OK) { - goto out; - } + /* status */ + int ret = 0; - /* allocate basic report buffers */ - _reports = new RingBuffer(2, sizeof(range_finder_report)); + do { /* create a scope to handle exit conditions using break */ - if (_reports == nullptr) { - warnx("mem err"); - goto out; - } + /* do regular cdev init */ + ret = CDev::init(); + if (ret != OK) break; - /* get a publish handle on the range finder topic */ - struct range_finder_report zero_report; - memset(&zero_report, 0, sizeof(zero_report)); - _range_finder_topic = orb_advertise(ORB_ID(sensor_range_finder), &zero_report); + /* allocate basic report buffers */ + _reports = new RingBuffer(2, sizeof(range_finder_report)); + if (_reports == nullptr) { + warnx("mem err"); + ret = -1; + break; + } - if (_range_finder_topic < 0) { - warnx("advert err"); - } + /* get a publish handle on the range finder topic */ + struct range_finder_report zero_report; + memset(&zero_report, 0, sizeof(zero_report)); + _range_finder_topic = orb_advertise(ORB_ID(sensor_range_finder), &zero_report); + + if (_range_finder_topic < 0) { + warnx("advert err"); + } + } while(0); /* close the fd */ ::close(_fd); _fd = -1; -out: + return OK; } diff --git a/src/include/containers/List.hpp b/src/include/containers/List.hpp index 13cbda9382..1906030abb 100644 --- a/src/include/containers/List.hpp +++ b/src/include/containers/List.hpp @@ -45,6 +45,7 @@ class __EXPORT ListNode public: ListNode() : _sibling(nullptr) { } + virtual ~ListNode() {}; void setSibling(T sibling) { _sibling = sibling; } T getSibling() { return _sibling; } T get() { @@ -52,6 +53,11 @@ public: } protected: T _sibling; +private: + // forbid copy + ListNode(const ListNode& other); + // forbid assignment + ListNode & operator = (const ListNode &); }; template @@ -60,12 +66,18 @@ class __EXPORT List public: List() : _head() { } + virtual ~List() {}; void add(T newNode) { newNode->setSibling(getHead()); setHead(newNode); } T getHead() { return _head; } -private: +protected: void setHead(T &head) { _head = head; } T _head; +private: + // forbid copy + List(const List& other); + // forbid assignment + List& operator = (const List &); }; diff --git a/src/modules/commander/accelerometer_calibration.cpp b/src/modules/commander/accelerometer_calibration.cpp index 1155631ea3..213de16562 100644 --- a/src/modules/commander/accelerometer_calibration.cpp +++ b/src/modules/commander/accelerometer_calibration.cpp @@ -570,6 +570,12 @@ int do_level_calibration(int mavlink_fd) { param_t roll_offset_handler = param_find("SENS_BOARD_X_OFF"); param_t pitch_offset_handler = param_find("SENS_BOARD_Y_OFF"); + // save old values if calibration fails + float roll_offset_current; + float pitch_offset_current; + param_get(roll_offset_handler, &roll_offset_current); + param_get(pitch_offset_handler, &pitch_offset_current); + float zero = 0.0f; param_set(roll_offset_handler, &zero); param_set(pitch_offset_handler, &zero); @@ -602,28 +608,34 @@ int do_level_calibration(int mavlink_fd) { mavlink_and_console_log_info(mavlink_fd, CAL_QGC_PROGRESS_MSG, 100); + bool success = false; if (counter > (cal_time * cal_hz / 2 )) { roll_mean /= counter; pitch_mean /= counter; } else { mavlink_and_console_log_info(mavlink_fd, "not enough measurements taken"); - return 1; } if (fabsf(roll_mean) > 0.8f ) { mavlink_and_console_log_critical(mavlink_fd, "excess roll angle"); - return 1; } else if (fabsf(pitch_mean) > 0.8f ) { mavlink_and_console_log_critical(mavlink_fd, "excess pitch angle"); - return 1; - } - else { + } else { roll_mean *= (float)M_RAD_TO_DEG; pitch_mean *= (float)M_RAD_TO_DEG; param_set(roll_offset_handler, &roll_mean); param_set(pitch_offset_handler, &pitch_mean); + success = true; } - mavlink_and_console_log_info(mavlink_fd, CAL_QGC_DONE_MSG, "level"); - return 0; + if (success) { + mavlink_and_console_log_info(mavlink_fd, CAL_QGC_DONE_MSG, "level"); + return 0; + } else { + // set old parameters + param_set(roll_offset_handler, &roll_offset_current); + param_set(pitch_offset_handler, &pitch_offset_current); + mavlink_and_console_log_critical(mavlink_fd, CAL_QGC_FAILED_MSG, "level"); + return 1; + } } diff --git a/src/modules/controllib/block/Block.cpp b/src/modules/controllib/block/Block.cpp index f3cd877287..083ab9498e 100644 --- a/src/modules/controllib/block/Block.cpp +++ b/src/modules/controllib/block/Block.cpp @@ -208,4 +208,9 @@ void SuperBlock::updateChildPublications() } } + } // namespace control + +template class List; +template class List; +template class List; diff --git a/src/modules/controllib/block/Block.hpp b/src/modules/controllib/block/Block.hpp index d2f9cdf07e..d169d35c5f 100644 --- a/src/modules/controllib/block/Block.hpp +++ b/src/modules/controllib/block/Block.hpp @@ -43,12 +43,9 @@ #include #include - -// forward declaration -namespace uORB { - class SubscriptionNode; - class PublicationNode; -} +#include +#include +#include namespace control { @@ -60,8 +57,8 @@ static const uint16_t maxPublicationsPerBlock = 100; static const uint8_t blockNameLengthMax = 80; // forward declaration -class BlockParamBase; class SuperBlock; +class BlockParamBase; /** */ @@ -137,4 +134,5 @@ protected: List _children; }; + } // namespace control diff --git a/src/modules/controllib/block/BlockParam.cpp b/src/modules/controllib/block/BlockParam.cpp index 8f98da74fa..532a037d40 100644 --- a/src/modules/controllib/block/BlockParam.cpp +++ b/src/modules/controllib/block/BlockParam.cpp @@ -43,6 +43,8 @@ #include "BlockParam.hpp" +#include + namespace control { diff --git a/src/modules/controllib/block/BlockParam.hpp b/src/modules/controllib/block/BlockParam.hpp index 437e43bfb4..cab28c65fd 100644 --- a/src/modules/controllib/block/BlockParam.hpp +++ b/src/modules/controllib/block/BlockParam.hpp @@ -47,6 +47,8 @@ namespace control { +class Block; + /** * A base class for block params that enables traversing linked list. */ diff --git a/src/modules/controllib/uorb/blocks.cpp b/src/modules/controllib/uorb/blocks.cpp index 454d0db19c..7c520219f1 100644 --- a/src/modules/controllib/uorb/blocks.cpp +++ b/src/modules/controllib/uorb/blocks.cpp @@ -38,6 +38,7 @@ */ #include "blocks.hpp" +#include namespace control { diff --git a/src/modules/controllib/uorb/blocks.hpp b/src/modules/controllib/uorb/blocks.hpp index 491d4681de..e3c0811116 100644 --- a/src/modules/controllib/uorb/blocks.hpp +++ b/src/modules/controllib/uorb/blocks.hpp @@ -61,10 +61,6 @@ #include #include -extern "C" { -#include -} - #include "../blocks.hpp" #include #include diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp index 30d2a64164..76f36e694f 100644 --- a/src/modules/mavlink/mavlink_main.cpp +++ b/src/modules/mavlink/mavlink_main.cpp @@ -1454,6 +1454,9 @@ Mavlink::task_main(int argc, char *argv[]) case MAVLINK_MODE_ONBOARD: configure_stream("SYS_STATUS", 1.0f); configure_stream("ATTITUDE", 50.0f); + configure_stream("HIGHRES_IMU", 50.0f); + configure_stream("VFR_HUD", 5.0f); + configure_stream("GPS_RAW_INT", 5.0f); configure_stream("GLOBAL_POSITION_INT", 50.0f); configure_stream("LOCAL_POSITION_NED", 30.0f); configure_stream("CAMERA_CAPTURE", 2.0f); @@ -1462,6 +1465,7 @@ Mavlink::task_main(int argc, char *argv[]) configure_stream("POSITION_TARGET_LOCAL_NED", 10.0f); configure_stream("DISTANCE_SENSOR", 10.0f); configure_stream("OPTICAL_FLOW_RAD", 10.0f); + configure_stream("RC_CHANNELS_RAW", 20.0f); configure_stream("VFR_HUD", 10.0f); configure_stream("SYSTEM_TIME", 1.0f); configure_stream("TIMESYNC", 10.0f); diff --git a/src/modules/mavlink/mavlink_messages.cpp b/src/modules/mavlink/mavlink_messages.cpp index cc00fc69b8..aae3e8d8cb 100644 --- a/src/modules/mavlink/mavlink_messages.cpp +++ b/src/modules/mavlink/mavlink_messages.cpp @@ -60,7 +60,6 @@ #include #include #include -#include #include #include #include diff --git a/src/modules/mavlink/mavlink_receiver.h b/src/modules/mavlink/mavlink_receiver.h index 7b592ab51a..4be73bc990 100644 --- a/src/modules/mavlink/mavlink_receiver.h +++ b/src/modules/mavlink/mavlink_receiver.h @@ -64,7 +64,6 @@ #include #include #include -#include #include #include #include diff --git a/src/modules/sensors/sensors.cpp b/src/modules/sensors/sensors.cpp index c3ac138ad5..acf1726984 100644 --- a/src/modules/sensors/sensors.cpp +++ b/src/modules/sensors/sensors.cpp @@ -2070,35 +2070,25 @@ Sensors::task_main() { /* start individual sensors */ - int ret; - ret = accel_init(); + int ret = 0; + do { /* create a scope to handle exit with break */ + ret = accel_init(); + if (ret) break; + ret = gyro_init(); + if (ret) break; + ret = mag_init(); + if (ret) break; + ret = baro_init(); + if (ret) break; + ret = adc_init(); + if (ret) break; + break; + } while (0); if (ret) { - goto exit_immediate; - } - - ret = gyro_init(); - - if (ret) { - goto exit_immediate; - } - - ret = mag_init(); - - if (ret) { - goto exit_immediate; - } - - ret = baro_init(); - - if (ret) { - goto exit_immediate; - } - - ret = adc_init(); - - if (ret) { - goto exit_immediate; + _sensors_task = -1; + _exit(ret); + return; } /* @@ -2241,8 +2231,6 @@ Sensors::task_main() } warnx("exiting."); - -exit_immediate: _sensors_task = -1; px4_task_exit(ret); } diff --git a/src/modules/uORB/Publication.cpp b/src/modules/uORB/Publication.cpp index eb2d84726f..0ea8e5db51 100644 --- a/src/modules/uORB/Publication.cpp +++ b/src/modules/uORB/Publication.cpp @@ -69,6 +69,14 @@ void * Publication::getDataVoidPtr() { return (void *)(T *)(this); } + +PublicationNode::PublicationNode(const struct orb_metadata *meta, + List * list) : + PublicationBase(meta) { + if (list != nullptr) list->add(this); +} + + template class __EXPORT Publication; template class __EXPORT Publication; template class __EXPORT Publication; diff --git a/src/modules/uORB/Publication.hpp b/src/modules/uORB/Publication.hpp index 8cbe51119e..9b4159cdfd 100644 --- a/src/modules/uORB/Publication.hpp +++ b/src/modules/uORB/Publication.hpp @@ -94,6 +94,11 @@ protected: // attributes const struct orb_metadata *_meta; orb_advert_t _handle; +private: + // forbid copy + PublicationBase(const PublicationBase&) : _meta(), _handle() {}; + // forbid assignment + PublicationBase& operator = (const PublicationBase &); }; /** @@ -120,10 +125,7 @@ public: * that this should be appended to. */ PublicationNode(const struct orb_metadata *meta, - List * list=nullptr) : - PublicationBase(meta) { - if (list != nullptr) list->add(this); - } + List * list=nullptr); /** * This function is the callback for list traversal @@ -136,7 +138,7 @@ public: * Publication wrapper class */ template -class Publication : +class __EXPORT Publication : public T, // this must be first! public PublicationNode { diff --git a/src/modules/uORB/Subscription.cpp b/src/modules/uORB/Subscription.cpp index db47218d9d..0c9433f036 100644 --- a/src/modules/uORB/Subscription.cpp +++ b/src/modules/uORB/Subscription.cpp @@ -53,6 +53,8 @@ #include "topics/vehicle_attitude_setpoint.h" #include "topics/vehicle_rates_setpoint.h" #include "topics/rc_channels.h" +#include "topics/vehicle_control_mode.h" +#include "topics/actuator_armed.h" namespace uORB { diff --git a/src/modules/uORB/Subscription.hpp b/src/modules/uORB/Subscription.hpp index 12301ce96f..31842ed715 100644 --- a/src/modules/uORB/Subscription.hpp +++ b/src/modules/uORB/Subscription.hpp @@ -106,6 +106,11 @@ protected: // attributes const struct orb_metadata *_meta; int _handle; +private: + // forbid copy + SubscriptionBase(const SubscriptionBase& other); + // forbid assignment + SubscriptionBase& operator = (const SubscriptionBase &); }; /** diff --git a/src/modules/uORB/topics/actuator_controls_effective.h b/src/modules/uORB/topics/actuator_controls_effective.h deleted file mode 100644 index 54d84231f0..0000000000 --- a/src/modules/uORB/topics/actuator_controls_effective.h +++ /dev/null @@ -1,79 +0,0 @@ -/**************************************************************************** - * - * Copyright (C) 2012 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 actuator_controls_effective.h - * - * Actuator control topics - mixer inputs. - * - * Values published to these topics are the outputs of the vehicle control - * system and mixing process; they are the control-scale values that are - * then fed to the actual actuator driver. - * - * Each topic can be published by a single controller - */ - -#ifndef TOPIC_ACTUATOR_CONTROLS_EFFECTIVE_H -#define TOPIC_ACTUATOR_CONTROLS_EFFECTIVE_H - -//#include -//#include "../uORB.h" -//#include "actuator_controls.h" -// -//#define NUM_ACTUATOR_CONTROLS_EFFECTIVE NUM_ACTUATOR_CONTROLS -//#define NUM_ACTUATOR_CONTROL_GROUPS_EFFECTIVE NUM_ACTUATOR_CONTROL_GROUPS /**< for sanity checking */ -// -///** -// * @addtogroup topics -// * @{ -// */ -// -//struct actuator_controls_effective_s { -// uint64_t timestamp; -// float control_effective[NUM_ACTUATOR_CONTROLS_EFFECTIVE]; -//}; -// -///** -// * @} -// */ -// -///* actuator control sets; this list can be expanded as more controllers emerge */ -//ORB_DECLARE(actuator_controls_effective_0); -//ORB_DECLARE(actuator_controls_effective_1); -//ORB_DECLARE(actuator_controls_effective_2); -//ORB_DECLARE(actuator_controls_effective_3); -// -///* control sets with pre-defined applications */ -//#define ORB_ID_VEHICLE_ATTITUDE_CONTROLS_EFFECTIVE ORB_ID(actuator_controls_effective_0) - -#endif /* TOPIC_ACTUATOR_CONTROLS_EFFECTIVE_H */ diff --git a/src/modules/uORB/topics/optical_flow.h b/src/modules/uORB/topics/optical_flow.h deleted file mode 100644 index d3dc46ee0b..0000000000 --- a/src/modules/uORB/topics/optical_flow.h +++ /dev/null @@ -1,84 +0,0 @@ -/**************************************************************************** - * - * Copyright (C) 2012 PX4 Development Team. All rights reserved. - * Author: @author Lorenz Meier - * - * 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 optical_flow.h - * Definition of the optical flow uORB topic. - */ - -#ifndef TOPIC_OPTICAL_FLOW_H_ -#define TOPIC_OPTICAL_FLOW_H_ - -#include -#include -#include "../uORB.h" - -/** - * @addtogroup topics - */ - -/** - * Optical flow in NED body frame in SI units. - * - * @see http://en.wikipedia.org/wiki/International_System_of_Units - */ -struct optical_flow_s { - - uint64_t timestamp; /**< in microseconds since system start */ - uint8_t sensor_id; /**< id of the sensor emitting the flow value */ - float pixel_flow_x_integral; /**< accumulated optical flow in radians around x axis */ - float pixel_flow_y_integral; /**< accumulated optical flow in radians around y axis */ - float gyro_x_rate_integral; /**< accumulated gyro value in radians around x axis */ - float gyro_y_rate_integral; /**< accumulated gyro value in radians around y axis */ - float gyro_z_rate_integral; /**< accumulated gyro value in radians around z axis */ - float ground_distance_m; /**< Altitude / distance to ground in meters */ - uint32_t integration_timespan; /** - * @author Julian Oes - * @author Lorenz Meier - */ - -#ifndef SENSOR_COMBINED_H_ -#define SENSOR_COMBINED_H_ - -#include -#include -#include "../uORB.h" - -enum MAGNETOMETER_MODE { - MAGNETOMETER_MODE_NORMAL = 0, - MAGNETOMETER_MODE_POSITIVE_BIAS, - MAGNETOMETER_MODE_NEGATIVE_BIAS -}; - -/** - * @addtogroup topics - * @{ - */ - -/** - * Sensor readings in raw and SI-unit form. - * - * These values are read from the sensors. Raw values are in sensor-specific units, - * the scaled values are in SI-units, as visible from the ending of the variable - * or the comments. The use of the SI fields is in general advised, as these fields - * are scaled and offset-compensated where possible and do not change with board - * revisions and sensor updates. - * - */ -struct sensor_combined_s { - - /* - * Actual data, this is specific to the type of data which is stored in this struct - * A line containing L0GME will be added by the Python logging code generator to the - * logged dataset. - */ - - /* NOTE: Ordering of fields optimized to align to 32 bit / 4 bytes Change with consideration only */ - - uint64_t timestamp; /**< Timestamp in microseconds since boot, from gyro */ - - int16_t gyro_raw[3]; /**< Raw sensor values of angular velocity */ - float gyro_rad_s[3]; /**< Angular velocity in radian per seconds */ - unsigned gyro_errcount; /**< Error counter for gyro 0 */ - float gyro_temp; /**< Temperature of gyro 0 */ - - int16_t accelerometer_raw[3]; /**< Raw acceleration in NED body frame */ - float accelerometer_m_s2[3]; /**< Acceleration in NED body frame, in m/s^2 */ - int accelerometer_mode; /**< Accelerometer measurement mode */ - float accelerometer_range_m_s2; /**< Accelerometer measurement range in m/s^2 */ - uint64_t accelerometer_timestamp; /**< Accelerometer timestamp */ - unsigned accelerometer_errcount; /**< Error counter for accel 0 */ - float accelerometer_temp; /**< Temperature of accel 0 */ - - int16_t magnetometer_raw[3]; /**< Raw magnetic field in NED body frame */ - float magnetometer_ga[3]; /**< Magnetic field in NED body frame, in Gauss */ - int magnetometer_mode; /**< Magnetometer measurement mode */ - float magnetometer_range_ga; /**< ± measurement range in Gauss */ - float magnetometer_cuttoff_freq_hz; /**< Internal analog low pass frequency of sensor */ - uint64_t magnetometer_timestamp; /**< Magnetometer timestamp */ - unsigned magnetometer_errcount; /**< Error counter for mag 0 */ - float magnetometer_temp; /**< Temperature of mag 0 */ - - 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 */ - unsigned gyro1_errcount; /**< Error counter for gyro 1 */ - float gyro1_temp; /**< Temperature of gyro 1 */ - - 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 */ - unsigned accelerometer1_errcount; /**< Error counter for accel 1 */ - float accelerometer1_temp; /**< Temperature of accel 1 */ - - 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 */ - unsigned magnetometer1_errcount; /**< Error counter for mag 1 */ - float magnetometer1_temp; /**< Temperature of mag 1 */ - - 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 */ - unsigned gyro2_errcount; /**< Error counter for gyro 1 */ - float gyro2_temp; /**< Temperature of gyro 1 */ - - 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 */ - unsigned accelerometer2_errcount; /**< Error counter for accel 2 */ - float accelerometer2_temp; /**< Temperature of accel 2 */ - - 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 */ - unsigned magnetometer2_errcount; /**< Error counter for mag 2 */ - float magnetometer2_temp; /**< Temperature of mag 2 */ - - 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 */ - uint64_t baro_timestamp; /**< Barometer timestamp */ - - float baro1_pres_mbar; /**< Barometric pressure, already temp. comp. */ - float baro1_alt_meter; /**< Altitude, already temp. comp. */ - float baro1_temp_celcius; /**< Temperature in degrees celsius */ - uint64_t baro1_timestamp; /**< Barometer timestamp */ - - float adc_voltage_v[10]; /**< ADC voltages of ADC Chan 10/11/12/13 or -1 */ - unsigned adc_mapping[10]; /**< Channel indices of each of these values */ - float mcu_temp_celcius; /**< Internal temperature measurement of MCU */ - - float differential_pressure_pa; /**< Airspeed sensor differential pressure */ - uint64_t differential_pressure_timestamp; /**< Last measurement timestamp */ - float differential_pressure_filtered_pa; /**< Low pass filtered airspeed sensor differential pressure reading */ - - float differential_pressure1_pa; /**< Airspeed sensor differential pressure */ - uint64_t differential_pressure1_timestamp; /**< Last measurement timestamp */ - float differential_pressure1_filtered_pa; /**< Low pass filtered airspeed sensor differential pressure reading */ - -}; - -/** - * @} - */ - -/* register this as object request broker structure */ -ORB_DECLARE(sensor_combined); - -#endif diff --git a/src/modules/uORB/topics/vehicle_global_position.h b/src/modules/uORB/topics/vehicle_global_position.h deleted file mode 100644 index 50c9429616..0000000000 --- a/src/modules/uORB/topics/vehicle_global_position.h +++ /dev/null @@ -1,87 +0,0 @@ -/**************************************************************************** - * - * Copyright (c) 2012-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 vehicle_global_position.h - * Definition of the global fused WGS84 position uORB topic. - * - * @author Thomas Gubler - * @author Julian Oes - * @author Lorenz Meier - */ - -#ifndef VEHICLE_GLOBAL_POSITION_T_H_ -#define VEHICLE_GLOBAL_POSITION_T_H_ - -#include -#include -#include - -/** - * @addtogroup topics - * @{ - */ - - /** - * Fused global position in WGS84. - * - * This struct contains global position estimation. It is not the raw GPS - * measurement (@see vehicle_gps_position). This topic is usually published by the position - * estimator, which will take more sources of information into account than just GPS, - * e.g. control inputs of the vehicle in a Kalman-filter implementation. - */ -struct vehicle_global_position_s { - uint64_t timestamp; /**< Time of this estimate, in microseconds since system start */ - uint64_t time_utc_usec; /**< GPS UTC timestamp in microseconds */ - double lat; /**< Latitude in degrees */ - double lon; /**< Longitude in degrees */ - float alt; /**< Altitude AMSL in meters */ - float vel_n; /**< Ground north velocity, m/s */ - float vel_e; /**< Ground east velocity, m/s */ - float vel_d; /**< Ground downside velocity, m/s */ - float yaw; /**< Yaw in radians -PI..+PI. */ - float eph; /**< Standard deviation of position estimate horizontally */ - float epv; /**< Standard deviation of position vertically */ - float terrain_alt; /**< Terrain altitude in m, WGS84 */ - bool terrain_alt_valid; /**< Terrain altitude estimate is valid */ - bool dead_reckoning; /**< True if this position is estimated through dead-reckoning*/ -}; - -/** - * @} - */ - -/* register this as object request broker structure */ -ORB_DECLARE(vehicle_global_position); - -#endif diff --git a/src/modules/uORB/topics/vehicle_gps_position.h b/src/modules/uORB/topics/vehicle_gps_position.h deleted file mode 100644 index 102914bbbb..0000000000 --- a/src/modules/uORB/topics/vehicle_gps_position.h +++ /dev/null @@ -1,94 +0,0 @@ -/**************************************************************************** - * - * Copyright (C) 2012 PX4 Development Team. All rights reserved. - * Author: @author Thomas Gubler - * @author Julian Oes - * @author Lorenz Meier - * - * 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 vehicle_gps_position.h - * Definition of the GPS WGS84 uORB topic. - */ - -#ifndef TOPIC_VEHICLE_GPS_H_ -#define TOPIC_VEHICLE_GPS_H_ - -#include -#include "../uORB.h" - -/** - * @addtogroup topics - * @{ - */ - -/** - * GPS position in WGS84 coordinates. - */ -struct vehicle_gps_position_s { - uint64_t timestamp_position; /**< Timestamp for position information */ - int32_t lat; /**< Latitude in 1E-7 degrees */ - int32_t lon; /**< Longitude in 1E-7 degrees */ - int32_t alt; /**< Altitude in 1E-3 meters (millimeters) above MSL */ - - uint64_t timestamp_variance; - float s_variance_m_s; /**< speed accuracy estimate m/s */ - float c_variance_rad; /**< course accuracy estimate rad */ - uint8_t fix_type; /**< 0-1: no fix, 2: 2D fix, 3: 3D fix, 4: RTCM code differential, 5: Real-Time Kinematic, float, 6: Real-Time Kinematic, fixed, 8: Extrapolated. Some applications will not use the value of this field unless it is at least two, so always correctly fill in the fix. */ - - float eph; /**< GPS HDOP horizontal dilution of position in m */ - float epv; /**< GPS VDOP horizontal dilution of position in m */ - - unsigned noise_per_ms; /**< */ - unsigned jamming_indicator; /**< */ - - uint64_t timestamp_velocity; /**< Timestamp for velocity informations */ - float vel_m_s; /**< GPS ground speed (m/s) */ - float vel_n_m_s; /**< GPS ground speed in m/s */ - float vel_e_m_s; /**< GPS ground speed in m/s */ - float vel_d_m_s; /**< GPS ground speed in m/s */ - float cog_rad; /**< Course over ground (NOT heading, but direction of movement) in rad, -PI..PI */ - bool vel_ned_valid; /**< Flag to indicate if NED speed is valid */ - - uint64_t timestamp_time; /**< Timestamp for time information */ - uint64_t time_utc_usec; /**< Timestamp (microseconds, UTC), this is the timestamp which comes from the gps module. It might be unavailable right after cold start, indicated by a value of 0 */ - - uint8_t satellites_used; /**< Number of satellites used */ -}; - -/** - * @} - */ - -/* register this as object request broker structure */ -ORB_DECLARE(vehicle_gps_position); - -#endif diff --git a/src/platforms/posix/drivers/airspeedsim/airspeedsim.cpp b/src/platforms/posix/drivers/airspeedsim/airspeedsim.cpp index ea9d92025e..5c2de4e3ce 100644 --- a/src/platforms/posix/drivers/airspeedsim/airspeedsim.cpp +++ b/src/platforms/posix/drivers/airspeedsim/airspeedsim.cpp @@ -211,7 +211,7 @@ Airspeed::ioctl(device::file_t *filp, int cmd, unsigned long arg) bool want_start = (_measure_ticks == 0); /* convert hz to tick interval via microseconds */ - unsigned ticks = USEC2TICK(1000000 / arg); + long ticks = USEC2TICK(1000000 / arg); /* check against maximum rate */ if (ticks < USEC2TICK(_conversion_interval)) diff --git a/src/modules/uORB/topics/filtered_bottom_flow.h b/src/platforms/qurt/px4_layer/commands_default.c similarity index 63% rename from src/modules/uORB/topics/filtered_bottom_flow.h rename to src/platforms/qurt/px4_layer/commands_default.c index c5d5455422..1c4c50f022 100644 --- a/src/modules/uORB/topics/filtered_bottom_flow.h +++ b/src/platforms/qurt/px4_layer/commands_default.c @@ -1,8 +1,6 @@ /**************************************************************************** * - * Copyright (C) 2008-2013 PX4 Development Team. All rights reserved. - * Author: Samuel Zihlmann - * Lorenz Meier + * Copyright (C) 2015 Mark Charlebois. All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -32,42 +30,39 @@ * POSSIBILITY OF SUCH DAMAGE. * ****************************************************************************/ - /** - * @file filtered_bottom_flow.h - * Definition of the filtered bottom flow uORB topic. + * @file commands_default.c + * Commands to run for the "qurt_default" config + * + * @author Mark Charlebois */ -#ifndef TOPIC_FILTERED_BOTTOM_FLOW_H_ -#define TOPIC_FILTERED_BOTTOM_FLOW_H_ - -#include -#include -#include "../uORB.h" - -/** - * @addtogroup topics - * @{ - */ - -/** - * Filtered bottom flow in bodyframe. - */ -struct filtered_bottom_flow_s { - uint64_t timestamp; /**< time of this estimate, in microseconds since system start */ - - float sumx; /**< Integrated bodyframe x flow in meters */ - float sumy; /**< Integrated bodyframe y flow in meters */ - - float vx; /**< Flow bodyframe x speed, m/s */ - float vy; /**< Flow bodyframe y Speed, m/s */ -}; - -/** - * @} - */ - -/* register this as object request broker structure */ -ORB_DECLARE(filtered_bottom_flow); - +const char *get_commands() +{ + static const char *commands = + "hello start\n" + "uorb start\n" + "simulator start -s\n" + "barosim start\n" + "adcsim start\n" + "accelsim start\n" + "gyrosim start\n" + "list_devices\n" + "list_topics\n" + "list_tasks\n" + "param show *\n" + "rgbled start\n" +#if 0 + "sensors start\n" + "param set CAL_GYRO0_ID 2293760\n" + "param set CAL_ACC0_ID 1310720\n" + "hil mode_pwm" + "param set CAL_ACC1_ID 1376256\n" + "param set CAL_MAG0_ID 196608\n" + "mavlink start -d /tmp/ttyS0\n" + "commander start\n" #endif + ; + + return commands; +} diff --git a/src/modules/uORB/topics/home_position.h b/src/platforms/qurt/px4_layer/commands_hello.c similarity index 62% rename from src/modules/uORB/topics/home_position.h rename to src/platforms/qurt/px4_layer/commands_hello.c index 02e17cdd7f..b2ef09b457 100644 --- a/src/modules/uORB/topics/home_position.h +++ b/src/platforms/qurt/px4_layer/commands_hello.c @@ -1,8 +1,6 @@ /**************************************************************************** * - * Copyright (C) 2012-2013 PX4 Development Team. All rights reserved. - * Author: Lorenz Meier - * Julian Oes + * Copyright (C) 2015 Mark Charlebois. All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -32,46 +30,16 @@ * POSSIBILITY OF SUCH DAMAGE. * ****************************************************************************/ - /** - * @file home_position.h - * Definition of the home position uORB topic. + * @file commands_hello.c + * Commands to run for the "qurt_hello" config * - * @author Lorenz Meier - * @author Julian Oes + * @author Mark Charlebois */ -#ifndef TOPIC_HOME_POSITION_H_ -#define TOPIC_HOME_POSITION_H_ +const char *get_commands() +{ + static const char *commands = "hello start"; -#include -#include "../uORB.h" - -/** - * @addtogroup topics - * @{ - */ - -/** - * GPS home position in WGS84 coordinates. - */ -struct home_position_s { - uint64_t timestamp; /**< Timestamp (microseconds since system boot) */ - - double lat; /**< Latitude in degrees */ - double lon; /**< Longitude in degrees */ - float alt; /**< Altitude in meters (AMSL) */ - - float x; /**< X coordinate in meters */ - float y; /**< Y coordinate in meters */ - float z; /**< Z coordinate in meters */ -}; - -/** - * @} - */ - -/* register this as object request broker structure */ -ORB_DECLARE(home_position); - -#endif + return commands; +} diff --git a/src/platforms/qurt/px4_layer/main.cpp b/src/platforms/qurt/px4_layer/main.cpp index 3f42c1c301..538b64229f 100644 --- a/src/platforms/qurt/px4_layer/main.cpp +++ b/src/platforms/qurt/px4_layer/main.cpp @@ -50,30 +50,10 @@ using namespace std; extern void init_app_map(map &apps); extern void list_builtins(map &apps); -static const char *commands = -"hello start\n" -"uorb start\n" -"simulator start -s\n" -"barosim start\n" -"adcsim start\n" -"accelsim start\n" -"gyrosim start\n" -"list_devices\n" -"list_topics\n" -"list_tasks\n" -"param show *\n" -"rgbled start\n" -#if 0 -"hil mode_pwm" -"param set CAL_GYRO0_ID 2293760\n" -"param set CAL_ACC0_ID 1310720\n" -"param set CAL_ACC1_ID 1376256\n" -"param set CAL_MAG0_ID 196608\n" -"sensors start\n" -"mavlink start -d /tmp/ttyS0\n" -"commander start\n" -#endif -; +__BEGIN_DECLS +// The commands to run are specified in a target file: commands_.c +extern const char *get_commands(void); +__END_DECLS static void run_cmd(map &apps, const vector &appargs) { // command is appargs[0] @@ -158,7 +138,7 @@ int main(int argc, char **argv) init_app_map(apps); px4::init_once(); px4::init(argc, argv, "mainapp"); - process_commands(apps, commands); + process_commands(apps, get_commands()); for (;;) {} } diff --git a/src/platforms/qurt/px4_layer/module.mk b/src/platforms/qurt/px4_layer/module.mk index 92f1c6abfb..714263edcb 100644 --- a/src/platforms/qurt/px4_layer/module.mk +++ b/src/platforms/qurt/px4_layer/module.mk @@ -52,6 +52,12 @@ SRCS = \ sq_remfirst.c \ sq_addafter.c \ dq_rem.c \ - main.cpp + main.cpp +ifeq ($(CONFIG),qurt_hello) +SRCS += commands_hello.c +endif +ifeq ($(CONFIG),qurt_default) +SRCS += commands_default.c +endif MAXOPTIMIZATION = -Os diff --git a/src/systemcmds/tests/test_eigen.cpp b/src/systemcmds/tests/test_eigen.cpp index 068e07c388..87035059aa 100644 --- a/src/systemcmds/tests/test_eigen.cpp +++ b/src/systemcmds/tests/test_eigen.cpp @@ -88,7 +88,7 @@ void printEigen(const Eigen::MatrixBase &b) for (int i = 0; i < b.rows(); ++i) { printf("("); - for (int j = 0; j < b.cols(); ++i) { + for (int j = 0; j < b.cols(); ++j) { if (j > 0) { printf(","); } @@ -129,10 +129,10 @@ int test_eigen(int argc, char *argv[]) TEST_OP_VERIFY("Constructor Vector2f(float[])", Eigen::Vector2f v3(data), v3[0] == data[0] && v3[1] == data[1]); TEST_OP_VERIFY("Constructor Vector2f(float, float)", Eigen::Vector2f v3(1.0f, 2.0f), v3(0) == 1.0f && v3(1) == 2.0f); TEST_OP_VERIFY("Vector2f = Vector2f", v = v1, v.isApprox(v1)); - TEST_OP_VERIFY("Vector2f + Vector2f", v + v1, v.isApprox(v1 + v1)); - TEST_OP_VERIFY("Vector2f - Vector2f", v - v1, v.isApprox(v1)); - TEST_OP_VERIFY("Vector2f += Vector2f", v += v1, v.isApprox(v1 + v1)); - TEST_OP_VERIFY("Vector2f -= Vector2f", v -= v1, v.isApprox(v1)); + VERIFY_OP("Vector2f + Vector2f", v = v + v1, v.isApprox(v1 + v1)); + VERIFY_OP("Vector2f - Vector2f", v = v - v1, v.isApprox(v1)); + VERIFY_OP("Vector2f += Vector2f", v += v1, v.isApprox(v1 + v1)); + VERIFY_OP("Vector2f -= Vector2f", v -= v1, v.isApprox(v1)); TEST_OP_VERIFY("Vector2f dot Vector2f", v.dot(v1), fabs(v.dot(v1) - 5.0f) <= FLT_EPSILON); //TEST_OP("Vector2f cross Vector2f", v1.cross(v2)); //cross product for 2d array? } @@ -183,8 +183,8 @@ int test_eigen(int argc, char *argv[]) TEST_OP("Vector<4> = Vector<4>", v = v1); TEST_OP("Vector<4> + Vector<4>", v + v1); TEST_OP("Vector<4> - Vector<4>", v - v1); - //TEST_OP("Vector<4> += Vector<4>", v += v1); - //TEST_OP("Vector<4> -= Vector<4>", v -= v1); + TEST_OP("Vector<4> += Vector<4>", v += v1); + TEST_OP("Vector<4> -= Vector<4>", v -= v1); TEST_OP("Vector<4> dot Vector<4>", v.dot(v1)); } @@ -225,8 +225,8 @@ int test_eigen(int argc, char *argv[]) // test nonsymmetric +, -, +=, -= const Eigen::Matrix m1_orig = - (Eigen::Matrix() << 1, 3, 3, - 4, 6, 6).finished(); + (Eigen::Matrix() << 1, 2, 3, + 4, 5, 6).finished(); Eigen::Matrix m1(m1_orig); @@ -276,6 +276,7 @@ int test_eigen(int argc, char *argv[]) } + /* QUATERNION TESTS CURRENTLY FAILING { // test conversion rotation matrix to quaternion and back Eigen::Matrix3f R_orig; @@ -295,7 +296,7 @@ int test_eigen(int argc, char *argv[]) for (size_t i = 0; i < 3; i++) { for (size_t j = 0; j < 3; j++) { - if (fabsf(R_orig(i, j) - R(i, j)) > 0.00001f) { + if (fabsf(R_orig(i, j) - R(i, j)) > tol) { warnx("Quaternion method 'from_dcm' or 'toRotationMatrix' outside tolerance!"); rc = 1; } @@ -426,5 +427,6 @@ int test_eigen(int argc, char *argv[]) } } } + */ return rc; -} \ No newline at end of file +} diff --git a/src/systemcmds/tests/tests_main.c b/src/systemcmds/tests/tests_main.c index 04b5efe230..546f3ceb57 100644 --- a/src/systemcmds/tests/tests_main.c +++ b/src/systemcmds/tests/tests_main.c @@ -112,7 +112,7 @@ const struct { #ifndef TESTS_MATHLIB_DISABLE {"mathlib", test_mathlib, 0}, #endif - {"eigen", test_eigen, OPT_NOALLTEST | OPT_NOJIGTEST}, + {"eigen", test_eigen, OPT_NOJIGTEST}, {"help", test_help, OPT_NOALLTEST | OPT_NOHELP | OPT_NOJIGTEST}, {NULL, NULL, 0} }; From 1c12d8fc7c4e9a737c79bde0e75b70c466a38b0f Mon Sep 17 00:00:00 2001 From: TSC21 Date: Thu, 21 May 2015 12:41:41 +0100 Subject: [PATCH 09/19] distance_sensor: added MAV_DISTANCE_SENSOR_INFRARED to topic stream --- src/modules/mavlink/mavlink_messages.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/modules/mavlink/mavlink_messages.cpp b/src/modules/mavlink/mavlink_messages.cpp index aae3e8d8cb..58f5dc2362 100644 --- a/src/modules/mavlink/mavlink_messages.cpp +++ b/src/modules/mavlink/mavlink_messages.cpp @@ -2240,9 +2240,9 @@ protected: msg.type = MAV_DISTANCE_SENSOR_LASER; break; - /*case MAV_DISTANCE_SENSOR_INFRARED: + case MAV_DISTANCE_SENSOR_INFRARED: msg.type = MAV_DISTANCE_SENSOR_INFRARED; - break;*/ //TODO: update mavlink def + break; default: msg.type = MAV_DISTANCE_SENSOR_LASER; From 29e7d5de9d32a52912113c19beffbf91ffc311e1 Mon Sep 17 00:00:00 2001 From: TSC21 Date: Fri, 22 May 2015 19:53:52 +0100 Subject: [PATCH 10/19] distance_sensor: update msg template info --- msg/distance_sensor.msg | 16 ++++++++-------- 1 file changed, 8 insertions(+), 8 deletions(-) diff --git a/msg/distance_sensor.msg b/msg/distance_sensor.msg index ef11c586fc..11fc298edd 100644 --- a/msg/distance_sensor.msg +++ b/msg/distance_sensor.msg @@ -1,17 +1,17 @@ # DISTANCE_SENSOR message data -uint32 time_boot_ms +uint32 time_boot_ms # Time since system boot (us) -uint16 min_distance -uint16 max_distance -uint16 current_distance +uint16 min_distance # Minimum distance the sensor can measure (cm) +uint16 max_distance # Maximum distance the sensor can measure (cm) +uint16 current_distance # Current distance reading (cm) -uint8 type +uint8 type # Type from MAV_DISTANCE_SENSOR enum uint8 MAV_DISTANCE_SENSOR_LASER = 0 uint8 MAV_DISTANCE_SENSOR_ULTRASOUND = 1 uint8 MAV_DISTANCE_SENSOR_INFRARED = 2 -uint8 id +uint8 id # Onboard ID of the sensor -uint8 orientation -uint8 covariance \ No newline at end of file +uint8 orientation # Direction the sensor faces from MAV_SENSOR_ORIENTATION enum +uint8 covariance # Measurement covariance (cm), 0 for unknown / invalid readings \ No newline at end of file From 93f8d7c4e8f4ae1aabf5eb3b764a286cef18a2ac Mon Sep 17 00:00:00 2001 From: TSC21 Date: Sat, 23 May 2015 15:03:32 +0100 Subject: [PATCH 11/19] distance_sensor: added distance_sensor to sdlog2 --- src/modules/sdlog2/sdlog2.c | 21 +++++++++++---------- src/modules/sdlog2/sdlog2_messages.h | 10 ++++++---- 2 files changed, 17 insertions(+), 14 deletions(-) diff --git a/src/modules/sdlog2/sdlog2.c b/src/modules/sdlog2/sdlog2.c index 3f1047742e..5ccadde57f 100644 --- a/src/modules/sdlog2/sdlog2.c +++ b/src/modules/sdlog2/sdlog2.c @@ -61,8 +61,6 @@ #include #include -#include - #include #include #include @@ -92,6 +90,7 @@ #include #include #include +#include #include #include #include @@ -1084,7 +1083,7 @@ int sdlog2_thread_main(int argc, char *argv[]) struct vehicle_global_velocity_setpoint_s global_vel_sp; struct battery_status_s battery; struct telemetry_status_s telemetry; - struct range_finder_report range_finder; + struct distance_sensor_s distance_sensor; struct estimator_status_report estimator_status; struct tecs_status_s tecs_status; struct system_power_s system_power; @@ -1174,7 +1173,7 @@ int sdlog2_thread_main(int argc, char *argv[]) int global_vel_sp_sub; int battery_sub; int telemetry_subs[TELEMETRY_STATUS_ORB_ID_NUM]; - int range_finder_sub; + int distance_sensor_sub; int estimator_status_sub; int tecs_status_sub; int system_power_sub; @@ -1208,7 +1207,7 @@ int sdlog2_thread_main(int argc, char *argv[]) subs.esc_sub = -1; subs.global_vel_sp_sub = -1; subs.battery_sub = -1; - subs.range_finder_sub = -1; + subs.distance_sensor_sub = -1; subs.estimator_status_sub = -1; subs.tecs_status_sub = -1; subs.system_power_sub = -1; @@ -1820,12 +1819,14 @@ int sdlog2_thread_main(int argc, char *argv[]) } } - /* --- BOTTOM DISTANCE --- */ - if (copy_if_updated(ORB_ID(sensor_range_finder), &subs.range_finder_sub, &buf.range_finder)) { + /* --- DISTANCE SENSOR --- */ + if (copy_if_updated(ORB_ID(distance_sensor), &subs.distance_sensor_sub, &buf.distance_sensor)) { log_msg.msg_type = LOG_DIST_MSG; - log_msg.body.log_DIST.bottom = buf.range_finder.distance; - log_msg.body.log_DIST.bottom_rate = 0.0f; - log_msg.body.log_DIST.flags = (buf.range_finder.valid ? 1 : 0); + log_msg.body.log_DIST.id = buf.distance_sensor.id; + log_msg.body.log_DIST.type = buf.distance_sensor.type; + log_msg.body.log_DIST.orientation = buf.distance_sensor.orientation; + log_msg.body.log_DIST.current_distance = buf.distance_sensor.current_distance; + log_msg.body.log_DIST.covariance = buf.distance_sensor.covariance; LOGBUFFER_WRITE_AND_COUNT(DIST); } diff --git a/src/modules/sdlog2/sdlog2_messages.h b/src/modules/sdlog2/sdlog2_messages.h index abdf518c51..eee44c43b9 100644 --- a/src/modules/sdlog2/sdlog2_messages.h +++ b/src/modules/sdlog2/sdlog2_messages.h @@ -295,12 +295,14 @@ struct log_BATT_s { float discharged; }; -/* --- DIST - DISTANCE TO SURFACE --- */ +/* --- DIST - RANGE SENSOR DISTANCE --- */ #define LOG_DIST_MSG 21 struct log_DIST_s { - float bottom; - float bottom_rate; - uint8_t flags; + uint8_t id; + uint8_t type; + uint8_t orientation; + uint16_t current_distance; + uint8_t covariance; }; /* LOG IMU1 and IMU2 MSGs consume IDs 22 and 23 */ From 43668cc8c8f6413861277aeab3fa026564b5c4fa Mon Sep 17 00:00:00 2001 From: TSC21 Date: Sat, 23 May 2015 17:49:52 +0100 Subject: [PATCH 12/19] distance_sensor: adopt message in both range drivers and on ekf_att_pos_estimator --- src/drivers/drv_range_finder.h | 12 +-- src/drivers/ll40ls/ll40ls.cpp | 55 ++++++------ src/drivers/mb12xx/mb12xx.cpp | 83 +++++-------------- .../AttitudePositionEstimatorEKF.h | 4 +- .../ekf_att_pos_estimator_main.cpp | 12 +-- src/modules/mavlink/mavlink_messages.cpp | 1 - src/modules/mavlink/mavlink_receiver.cpp | 52 ++++++++---- src/modules/mavlink/mavlink_receiver.h | 3 +- src/modules/uORB/objects_common.cpp | 3 - 9 files changed, 97 insertions(+), 128 deletions(-) diff --git a/src/drivers/drv_range_finder.h b/src/drivers/drv_range_finder.h index bd72971b94..128e8fc0d2 100644 --- a/src/drivers/drv_range_finder.h +++ b/src/drivers/drv_range_finder.h @@ -48,14 +48,10 @@ #define RANGE_FINDER0_DEVICE_PATH "/dev/range_finder0" #define MB12XX_MAX_RANGEFINDERS 12 //Maximum number of RangeFinders that can be connected -#define range_finder_report range_finder_s -#define __orb_sensor_range_finder __orb_range_finder - -#include - -#ifndef RANGE_FINDER_TYPE_LASER -#define RANGE_FINDER_TYPE_LASER 0 -#endif +/* + * ObjDev tag for px4flow data. + */ +ORB_DECLARE(distance_sensor); /* * ioctl() definitions diff --git a/src/drivers/ll40ls/ll40ls.cpp b/src/drivers/ll40ls/ll40ls.cpp index b3368406a1..6a38baf875 100644 --- a/src/drivers/ll40ls/ll40ls.cpp +++ b/src/drivers/ll40ls/ll40ls.cpp @@ -68,6 +68,7 @@ #include #include +#include #include @@ -144,7 +145,7 @@ private: bool _collect_phase; int _class_instance; - orb_advert_t _range_finder_topic; + orb_advert_t _distance_sensor_topic; perf_counter_t _sample_perf; perf_counter_t _comms_errors; @@ -224,7 +225,7 @@ LL40LS::LL40LS(int bus, const char *path, int address) : _measure_ticks(0), _collect_phase(false), _class_instance(-1), - _range_finder_topic(-1), + _distance_sensor_topic(-1), _sample_perf(perf_alloc(PC_ELAPSED, "ll40ls_read")), _comms_errors(perf_alloc(PC_COUNT, "ll40ls_comms_errors")), _buffer_overflows(perf_alloc(PC_COUNT, "ll40ls_buffer_overflows")), @@ -278,7 +279,7 @@ LL40LS::init() } /* allocate basic report buffers */ - _reports = new RingBuffer(2, sizeof(range_finder_report)); + _reports = new RingBuffer(2, sizeof(distance_sensor_s)); if (_reports == nullptr) { goto out; @@ -288,12 +289,12 @@ LL40LS::init() if (_class_instance == CLASS_DEVICE_PRIMARY) { /* get a publish handle on the range finder topic */ - struct range_finder_report rf_report; + struct distance_sensor_s rf_report; measure(); _reports->get(&rf_report); - _range_finder_topic = orb_advertise(ORB_ID(sensor_range_finder), &rf_report); + _distance_sensor_topic = orb_advertise(ORB_ID(distance_sensor), &rf_report); - if (_range_finder_topic < 0) { + if (_distance_sensor_topic < 0) { debug("failed to create sensor_range_finder object. Did you start uOrb?"); } } @@ -497,8 +498,8 @@ LL40LS::ioctl(struct file *filp, int cmd, unsigned long arg) ssize_t LL40LS::read(struct file *filp, char *buffer, size_t buflen) { - unsigned count = buflen / sizeof(struct range_finder_report); - struct range_finder_report *rbuf = reinterpret_cast(buffer); + unsigned count = buflen / sizeof(struct distance_sensor_s); + struct distance_sensor_s *rbuf = reinterpret_cast(buffer); int ret = 0; /* buffer must be large enough */ @@ -664,8 +665,8 @@ LL40LS::collect() } uint16_t distance = (val[0] << 8) | val[1]; - float si_units = distance * 0.01f; /* cm to m */ - struct range_finder_report report; + float si_units = (distance * 1.0f) / 100.0f; /* cm to m */ + struct distance_sensor_s report; if (distance == 0) { _zero_counter++; @@ -688,22 +689,18 @@ LL40LS::collect() _last_distance = distance; - /* this should be fairly close to the end of the measurement, so the best approximation of the time */ - report.timestamp = hrt_absolute_time(); - report.error_count = perf_event_count(_comms_errors); - report.distance = si_units; - report.minimum_distance = get_minimum_distance(); - report.maximum_distance = get_maximum_distance(); - if (si_units > get_minimum_distance() && si_units < get_maximum_distance()) { - report.valid = 1; - } - else { - report.valid = 0; - } + report.time_boot_ms = hrt_absolute_time(); + report.id = 0; + report.type = 1; + report.orientation = 8; + report.current_distance = si_units; + report.min_distance = get_minimum_distance(); + report.max_distance = get_maximum_distance(); + report.covariance = 0.0; /* publish it, if we are the primary */ - if (_range_finder_topic >= 0) { - orb_publish(ORB_ID(sensor_range_finder), _range_finder_topic, &report); + if (_distance_sensor_topic >= 0) { + orb_publish(ORB_ID(distance_sensor), _distance_sensor_topic, &report); } if (_reports->force(&report)) { @@ -958,7 +955,7 @@ void stop(int bus) void test(int bus) { - struct range_finder_report report; + struct distance_sensor_s report; ssize_t sz; int ret; const char *path = (bus==PX4_I2C_BUS_ONBOARD?LL40LS_DEVICE_PATH_INT:LL40LS_DEVICE_PATH_EXT); @@ -977,8 +974,8 @@ test(int bus) } warnx("single read"); - warnx("measurement: %0.2f m", (double)report.distance); - warnx("time: %lld", report.timestamp); + warnx("measurement: %0.2f m", (double)report.current_distance); + warnx("time: %d", report.time_boot_ms); /* start the sensor polling at 2Hz */ if (OK != ioctl(fd, SENSORIOCSPOLLRATE, 2)) { @@ -1006,8 +1003,8 @@ test(int bus) } warnx("periodic read %u", i); - warnx("measurement: %0.3f", (double)report.distance); - warnx("time: %lld", report.timestamp); + warnx("measurement: %0.3f m", (double)report.current_distance); + warnx("time: %d", report.time_boot_ms); } /* reset the sensor polling to default rate */ diff --git a/src/drivers/mb12xx/mb12xx.cpp b/src/drivers/mb12xx/mb12xx.cpp index 1a4ea17cf1..edbb4eefe2 100644 --- a/src/drivers/mb12xx/mb12xx.cpp +++ b/src/drivers/mb12xx/mb12xx.cpp @@ -70,6 +70,7 @@ #include #include +#include #include @@ -131,7 +132,7 @@ private: bool _collect_phase; int _class_instance; - orb_advert_t _range_finder_topic; + orb_advert_t _distance_sensor_topic; perf_counter_t _sample_perf; perf_counter_t _comms_errors; @@ -208,7 +209,7 @@ MB12XX::MB12XX(int bus, int address) : _measure_ticks(0), _collect_phase(false), _class_instance(-1), - _range_finder_topic(-1), + _distance_sensor_topic(-1), _sample_perf(perf_alloc(PC_ELAPSED, "mb12xx_read")), _comms_errors(perf_alloc(PC_COUNT, "mb12xx_comms_errors")), _buffer_overflows(perf_alloc(PC_COUNT, "mb12xx_buffer_overflows")), @@ -255,7 +256,7 @@ MB12XX::init() } /* allocate basic report buffers */ - _reports = new RingBuffer(2, sizeof(range_finder_report)); + _reports = new RingBuffer(2, sizeof(distance_sensor_s)); _index_counter = MB12XX_BASEADDR; /* set temp sonar i2c address to base adress */ set_address(_index_counter); /* set I2c port to temp sonar i2c adress */ @@ -268,11 +269,11 @@ MB12XX::init() if (_class_instance == CLASS_DEVICE_PRIMARY) { /* get a publish handle on the range finder topic */ - struct range_finder_report rf_report = {}; + struct distance_sensor_s rf_report = {}; - _range_finder_topic = orb_advertise(ORB_ID(sensor_range_finder), &rf_report); + _distance_sensor_topic = orb_advertise(ORB_ID(distance_sensor), &rf_report); - if (_range_finder_topic < 0) { + if (_distance_sensor_topic < 0) { log("failed to create sensor_range_finder object. Did you start uOrb?"); } } @@ -469,8 +470,8 @@ ssize_t MB12XX::read(struct file *filp, char *buffer, size_t buflen) { - unsigned count = buflen / sizeof(struct range_finder_report); - struct range_finder_report *rbuf = reinterpret_cast(buffer); + unsigned count = buflen / sizeof(struct distance_sensor_s); + struct distance_sensor_s *rbuf = reinterpret_cast(buffer); int ret = 0; /* buffer must be large enough */ @@ -571,51 +572,20 @@ MB12XX::collect() uint16_t distance = val[0] << 8 | val[1]; float si_units = (distance * 1.0f) / 100.0f; /* cm to m */ - struct range_finder_report report; + struct distance_sensor_s report; - /* this should be fairly close to the end of the measurement, so the best approximation of the time */ - report.timestamp = hrt_absolute_time(); - report.error_count = perf_event_count(_comms_errors); - - /* if only one sonar, write it to the original distance parameter so that it's still used as altitude sonar */ - if (addr_ind.size() == 1) { - report.distance = si_units; - - for (unsigned i = 0; i < (MB12XX_MAX_RANGEFINDERS); i++) { - report.distance_vector[i] = 0; - } - - report.just_updated = 0; - - } else { - /* for multiple sonars connected */ - - /* don't use the orginial single sonar variable */ - report.distance = 0; - - /* intermediate vector _latest_sonar_measurements is used to store the measurements as every cycle the other sonar values of the report are thrown away and/or filled in with garbage. We don't want this. We want the report to give the latest value for each connected sonar */ - _latest_sonar_measurements[_cycle_counter] = si_units; - - for (unsigned i = 0; i < (_latest_sonar_measurements.size()); i++) { - report.distance_vector[i] = _latest_sonar_measurements[i]; - } - - /* a just_updated variable is added to indicate to autopilot (ardupilot or whatever) which sonar has most recently been collected as this could be of use for Kalman filters */ - report.just_updated = _index_counter; - - /* Make sure all elements of the distance vector for which no sonar is connected are zero to prevent strange numbers */ - for (unsigned i = 0; i < (MB12XX_MAX_RANGEFINDERS - addr_ind.size()); i++) { - report.distance_vector[addr_ind.size() + i] = 0; - } - } - - report.minimum_distance = get_minimum_distance(); - report.maximum_distance = get_maximum_distance(); - report.valid = si_units > get_minimum_distance() && si_units < get_maximum_distance() ? 1 : 0; + report.time_boot_ms = hrt_absolute_time(); + report.id = 0; + report.type = 1; + report.orientation = 8; + report.current_distance = si_units; + report.min_distance = get_minimum_distance(); + report.max_distance = get_maximum_distance(); + report.covariance = 0.0; /* publish it, if we are the primary */ - if (_range_finder_topic >= 0) { - orb_publish(ORB_ID(sensor_range_finder), _range_finder_topic, &report); + if (_distance_sensor_topic >= 0) { + orb_publish(ORB_ID(distance_sensor), _distance_sensor_topic, &report); } if (_reports->force(&report)) { @@ -840,7 +810,7 @@ void stop() void test() { - struct range_finder_report report; + struct distance_sensor_s report; ssize_t sz; int ret; @@ -858,8 +828,7 @@ test() } warnx("single read"); - warnx("measurement: %0.2f of sonar %d", (double)report.distance_vector[report.just_updated], report.just_updated); - warnx("time: %lld", report.timestamp); + warnx("time: %d", report.time_boot_ms); /* start the sensor polling at 2Hz */ if (OK != ioctl(fd, SENSORIOCSPOLLRATE, 2)) { @@ -887,13 +856,7 @@ test() } warnx("periodic read %u", i); - - /* Print the sonar rangefinder report sonar distance vector */ - for (uint8_t count = 0; count < MB12XX_MAX_RANGEFINDERS; count++) { - warnx("measurement: %0.3f of sonar %u", (double)report.distance_vector[count], count + 1); - } - - warnx("time: %lld", report.timestamp); + warnx("time: %d", report.time_boot_ms); } /* reset the sensor polling to default rate */ diff --git a/src/modules/ekf_att_pos_estimator/AttitudePositionEstimatorEKF.h b/src/modules/ekf_att_pos_estimator/AttitudePositionEstimatorEKF.h index 4d5e56a961..ee6fcb3ac5 100644 --- a/src/modules/ekf_att_pos_estimator/AttitudePositionEstimatorEKF.h +++ b/src/modules/ekf_att_pos_estimator/AttitudePositionEstimatorEKF.h @@ -58,13 +58,13 @@ #include #include #include +#include #include #include #include #include #include -#include #include #include @@ -162,7 +162,7 @@ private: struct vehicle_local_position_s _local_pos; /**< local vehicle position */ struct vehicle_gps_position_s _gps; /**< GPS position */ struct wind_estimate_s _wind; /**< wind estimate */ - struct range_finder_report _distance; /**< distance estimate */ + struct distance_sensor_s _distance; /**< distance estimate */ struct vehicle_land_detected_s _landDetector; struct actuator_armed_s _armed; 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 1b6e17ac37..3c3e2d2f35 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 @@ -501,7 +501,7 @@ void AttitudePositionEstimatorEKF::task_main() /* * do subscriptions */ - _distance_sub = orb_subscribe(ORB_ID(sensor_range_finder)); + _distance_sub = orb_subscribe(ORB_ID(distance_sensor)); _baro_sub = orb_subscribe_multi(ORB_ID(sensor_baro), 0); _airspeed_sub = orb_subscribe(ORB_ID(airspeed)); _gps_sub = orb_subscribe(ORB_ID(vehicle_gps_position)); @@ -1471,11 +1471,11 @@ void AttitudePositionEstimatorEKF::pollData() orb_check(_distance_sub, &_newRangeData); if (_newRangeData) { - orb_copy(ORB_ID(sensor_range_finder), _distance_sub, &_distance); - - if (_distance.valid) { - _ekf->rngMea = _distance.distance; - _distance_last_valid = _distance.timestamp; + orb_copy(ORB_ID(distance_sensor), _distance_sub, &_distance); + if ((_distance.current_distance > _distance.min_distance) + && (_distance.current_distance < _distance.max_distance)) { + _ekf->rngMea = _distance.current_distance; + _distance_last_valid = _distance.time_boot_ms; } else { _newRangeData = false; diff --git a/src/modules/mavlink/mavlink_messages.cpp b/src/modules/mavlink/mavlink_messages.cpp index 58f5dc2362..650e9bf81a 100644 --- a/src/modules/mavlink/mavlink_messages.cpp +++ b/src/modules/mavlink/mavlink_messages.cpp @@ -71,7 +71,6 @@ #include #include #include -#include #include #include diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index ac7ab99c05..8588e4047f 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -56,7 +56,6 @@ #include #include #include -#include #include #include #include @@ -109,7 +108,7 @@ MavlinkReceiver::MavlinkReceiver(Mavlink *parent) : _battery_pub(-1), _cmd_pub(-1), _flow_pub(-1), - _range_pub(-1), + _distance_sensor_pub(-1), _offboard_control_mode_pub(-1), _actuator_controls_pub(-1), _global_vel_sp_pub(-1), @@ -134,8 +133,7 @@ MavlinkReceiver::MavlinkReceiver(Mavlink *parent) : _att_sp{}, _rates_sp{}, _time_offset_avg_alpha(0.6), - _time_offset(0), - _distance_sensor_pub(-1) + _time_offset(0) { } @@ -417,6 +415,25 @@ MavlinkReceiver::handle_message_optical_flow_rad(mavlink_message_t *msg) } else { orb_publish(ORB_ID(optical_flow), _flow_pub, &f); } + + /* Use distance value for distance sensor topic */ + struct distance_sensor_s d; + memset(&d, 0, sizeof(d)); + + d.time_boot_ms = hrt_absolute_time(); + d.min_distance = 3.0f; + d.max_distance = 50.0f; + d.current_distance = flow.distance; + d.type = 1; + d.id = 0; + d.orientation = 8; + d.covariance = 0.0; + + if (_distance_sensor_pub < 0) { + _distance_sensor_pub = orb_advertise(ORB_ID(distance_sensor), &d); + } else { + orb_publish(ORB_ID(distance_sensor), _distance_sensor_pub, &d); + } } void @@ -449,22 +466,23 @@ MavlinkReceiver::handle_message_hil_optical_flow(mavlink_message_t *msg) orb_publish(ORB_ID(optical_flow), _flow_pub, &f); } - /* Use distance value for range finder report */ - struct range_finder_report r; - memset(&r, 0, sizeof(r)); + /* Use distance value for distance sensor topic */ + struct distance_sensor_s d; + memset(&d, 0, sizeof(d)); - r.timestamp = hrt_absolute_time(); - r.error_count = 0; - r.type = RANGE_FINDER_TYPE_LASER; - r.distance = flow.distance; - r.minimum_distance = 0.0f; - r.maximum_distance = 40.0f; // this is set to match the typical range of real sensors, could be made configurable - r.valid = (r.distance > r.minimum_distance) && (r.distance < r.maximum_distance); + d.time_boot_ms = hrt_absolute_time(); + d.min_distance = 3.0f; + d.max_distance = 50.0f; + d.current_distance = flow.distance; + d.type = 1; + d.id = 0; + d.orientation = 8; + d.covariance = 0.0; - if (_range_pub < 0) { - _range_pub = orb_advertise(ORB_ID(sensor_range_finder), &r); + if (_distance_sensor_pub < 0) { + _distance_sensor_pub = orb_advertise(ORB_ID(distance_sensor), &d); } else { - orb_publish(ORB_ID(sensor_range_finder), _range_pub, &r); + orb_publish(ORB_ID(distance_sensor), _distance_sensor_pub, &d); } } diff --git a/src/modules/mavlink/mavlink_receiver.h b/src/modules/mavlink/mavlink_receiver.h index 4be73bc990..016305e796 100644 --- a/src/modules/mavlink/mavlink_receiver.h +++ b/src/modules/mavlink/mavlink_receiver.h @@ -166,7 +166,7 @@ private: orb_advert_t _battery_pub; orb_advert_t _cmd_pub; orb_advert_t _flow_pub; - orb_advert_t _range_pub; + orb_advert_t _distance_sensor_pub; orb_advert_t _offboard_control_mode_pub; orb_advert_t _actuator_controls_pub; orb_advert_t _global_vel_sp_pub; @@ -192,7 +192,6 @@ private: struct vehicle_rates_setpoint_s _rates_sp; double _time_offset_avg_alpha; uint64_t _time_offset; - orb_advert_t _distance_sensor_pub; /* do not allow copying this class */ MavlinkReceiver(const MavlinkReceiver &); diff --git a/src/modules/uORB/objects_common.cpp b/src/modules/uORB/objects_common.cpp index b34f9a742d..f900543bc2 100644 --- a/src/modules/uORB/objects_common.cpp +++ b/src/modules/uORB/objects_common.cpp @@ -57,9 +57,6 @@ ORB_DEFINE(sensor_gyro, struct gyro_report); #include ORB_DEFINE(sensor_baro, struct baro_report); -#include -ORB_DEFINE(sensor_range_finder, struct range_finder_report); - #include ORB_DEFINE(output_pwm, struct pwm_output_values); From 7e740f262b07881d4abdc56caee8d1f523f8be99 Mon Sep 17 00:00:00 2001 From: TSC21 Date: Sat, 23 May 2015 17:54:52 +0100 Subject: [PATCH 13/19] distance_sensor: removed max number of rangefinders --- src/drivers/drv_range_finder.h | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) diff --git a/src/drivers/drv_range_finder.h b/src/drivers/drv_range_finder.h index 128e8fc0d2..1ddc07c3fb 100644 --- a/src/drivers/drv_range_finder.h +++ b/src/drivers/drv_range_finder.h @@ -46,8 +46,7 @@ #define RANGE_FINDER_BASE_DEVICE_PATH "/dev/range_finder" #define RANGE_FINDER0_DEVICE_PATH "/dev/range_finder0" -#define MB12XX_MAX_RANGEFINDERS 12 //Maximum number of RangeFinders that can be connected - + /* * ObjDev tag for px4flow data. */ From 36dfda4aea3079f843f33134bfeb67836eebfdb5 Mon Sep 17 00:00:00 2001 From: TSC21 Date: Sat, 23 May 2015 17:58:20 +0100 Subject: [PATCH 14/19] distance_sensor: mavlink_receiver: changed to SI units --- src/modules/mavlink/mavlink_receiver.cpp | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index 8588e4047f..25d577dd00 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -421,8 +421,8 @@ MavlinkReceiver::handle_message_optical_flow_rad(mavlink_message_t *msg) memset(&d, 0, sizeof(d)); d.time_boot_ms = hrt_absolute_time(); - d.min_distance = 3.0f; - d.max_distance = 50.0f; + d.min_distance = 0.3f; + d.max_distance = 5.0f; d.current_distance = flow.distance; d.type = 1; d.id = 0; @@ -471,8 +471,8 @@ MavlinkReceiver::handle_message_hil_optical_flow(mavlink_message_t *msg) memset(&d, 0, sizeof(d)); d.time_boot_ms = hrt_absolute_time(); - d.min_distance = 3.0f; - d.max_distance = 50.0f; + d.min_distance = 0.3f; + d.max_distance = 5.0f; d.current_distance = flow.distance; d.type = 1; d.id = 0; From c2c0cbe6829206b73b4b4917c1c1e02629ae8b4a Mon Sep 17 00:00:00 2001 From: TSC21 Date: Sat, 23 May 2015 18:00:42 +0100 Subject: [PATCH 15/19] distance_sensor: msg def: changed to SI units --- msg/distance_sensor.msg | 10 +++++----- 1 file changed, 5 insertions(+), 5 deletions(-) diff --git a/msg/distance_sensor.msg b/msg/distance_sensor.msg index 11fc298edd..8e44d76f79 100644 --- a/msg/distance_sensor.msg +++ b/msg/distance_sensor.msg @@ -2,11 +2,11 @@ uint32 time_boot_ms # Time since system boot (us) -uint16 min_distance # Minimum distance the sensor can measure (cm) -uint16 max_distance # Maximum distance the sensor can measure (cm) -uint16 current_distance # Current distance reading (cm) +uint16 min_distance # Minimum distance the sensor can measure (in SI) +uint16 max_distance # Maximum distance the sensor can measure (in SI) +uint16 current_distance # Current distance reading (in SI) -uint8 type # Type from MAV_DISTANCE_SENSOR enum +uint8 type # Type from MAV_DISTANCE_SENSOR enum uint8 MAV_DISTANCE_SENSOR_LASER = 0 uint8 MAV_DISTANCE_SENSOR_ULTRASOUND = 1 uint8 MAV_DISTANCE_SENSOR_INFRARED = 2 @@ -14,4 +14,4 @@ uint8 MAV_DISTANCE_SENSOR_INFRARED = 2 uint8 id # Onboard ID of the sensor uint8 orientation # Direction the sensor faces from MAV_SENSOR_ORIENTATION enum -uint8 covariance # Measurement covariance (cm), 0 for unknown / invalid readings \ No newline at end of file +uint8 covariance # Measurement covariance (in SI), 0 for unknown / invalid readings \ No newline at end of file From 169eae8cf9353b1a552ac6f70762bf9720917d0a Mon Sep 17 00:00:00 2001 From: TSC21 Date: Sat, 23 May 2015 20:47:14 +0100 Subject: [PATCH 16/19] distance_sensor: removed range_finder.msg def; readded max number of sonars --- msg/range_finder.msg | 13 ------------- 1 file changed, 13 deletions(-) delete mode 100644 msg/range_finder.msg diff --git a/msg/range_finder.msg b/msg/range_finder.msg deleted file mode 100644 index d2e67543e5..0000000000 --- a/msg/range_finder.msg +++ /dev/null @@ -1,13 +0,0 @@ -int16 RANGE_FINDER_TYPE_LASER = 0 - -# range finder report structure. Reads from the device must be in multiples of this -# structure. -uint64 timestamp -uint64 error_count -uint16 type # type, following RANGE_FINDER_TYPE enum -float32 distance # in meters -float32 minimum_distance # minimum distance the sensor can measure -float32 maximum_distance # maximum distance the sensor can measure -uint8 valid # 1 == within sensor range, 0 = outside sensor range -float32[12] distance_vector # in meters length should match MB12XX_MAX_RANGEFINDERS -uint8 just_updated # number of the most recent measurement sensor From 17bc0979e4830fd3bd387b7c6a8d09c2d57433f4 Mon Sep 17 00:00:00 2001 From: TSC21 Date: Sat, 23 May 2015 20:47:35 +0100 Subject: [PATCH 17/19] distance_sensor: re-add max number of sonars --- src/drivers/drv_range_finder.h | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/src/drivers/drv_range_finder.h b/src/drivers/drv_range_finder.h index 1ddc07c3fb..035eb92e0c 100644 --- a/src/drivers/drv_range_finder.h +++ b/src/drivers/drv_range_finder.h @@ -46,7 +46,8 @@ #define RANGE_FINDER_BASE_DEVICE_PATH "/dev/range_finder" #define RANGE_FINDER0_DEVICE_PATH "/dev/range_finder0" - +#define MB12XX_MAX_RANGEFINDERS 12 // Maximum number of Maxbotix sensors on bus + /* * ObjDev tag for px4flow data. */ From 98d02040878aaef7518f60173a0011692951a90c Mon Sep 17 00:00:00 2001 From: TSC21 Date: Sat, 23 May 2015 21:06:19 +0100 Subject: [PATCH 18/19] distance_sensor: update drivers --- src/drivers/mb12xx/mb12xx.cpp | 2 +- src/drivers/sf0x/sf0x.cpp | 46 +++++++++++++++-------------- src/drivers/trone/trone.cpp | 54 +++++++++++++++++------------------ 3 files changed, 52 insertions(+), 50 deletions(-) diff --git a/src/drivers/mb12xx/mb12xx.cpp b/src/drivers/mb12xx/mb12xx.cpp index edbb4eefe2..7e4a5e015e 100644 --- a/src/drivers/mb12xx/mb12xx.cpp +++ b/src/drivers/mb12xx/mb12xx.cpp @@ -575,7 +575,7 @@ MB12XX::collect() struct distance_sensor_s report; report.time_boot_ms = hrt_absolute_time(); - report.id = 0; + report.id = 1; report.type = 1; report.orientation = 8; report.current_distance = si_units; diff --git a/src/drivers/sf0x/sf0x.cpp b/src/drivers/sf0x/sf0x.cpp index c45e73fd9a..403ac89dc1 100644 --- a/src/drivers/sf0x/sf0x.cpp +++ b/src/drivers/sf0x/sf0x.cpp @@ -69,6 +69,7 @@ #include #include +#include #include @@ -128,7 +129,7 @@ private: enum SF0X_PARSE_STATE _parse_state; hrt_abstime _last_read; - orb_advert_t _range_finder_topic; + orb_advert_t _distance_sensor_topic; unsigned _consecutive_fail_count; @@ -194,7 +195,7 @@ SF0X::SF0X(const char *port) : _linebuf_index(0), _parse_state(SF0X_PARSE_STATE0_UNSYNC), _last_read(0), - _range_finder_topic(-1), + _distance_sensor_topic(-1), _consecutive_fail_count(0), _sample_perf(perf_alloc(PC_ELAPSED, "sf0x_read")), _comms_errors(perf_alloc(PC_COUNT, "sf0x_comms_errors")), @@ -270,7 +271,7 @@ SF0X::init() if (ret != OK) break; /* allocate basic report buffers */ - _reports = new RingBuffer(2, sizeof(range_finder_report)); + _reports = new RingBuffer(2, sizeof(distance_sensor_s)); if (_reports == nullptr) { warnx("mem err"); ret = -1; @@ -278,11 +279,11 @@ SF0X::init() } /* get a publish handle on the range finder topic */ - struct range_finder_report zero_report; + struct distance_sensor_s zero_report; memset(&zero_report, 0, sizeof(zero_report)); - _range_finder_topic = orb_advertise(ORB_ID(sensor_range_finder), &zero_report); + _distance_sensor_topic = orb_advertise(ORB_ID(distance_sensor), &zero_report); - if (_range_finder_topic < 0) { + if (_distance_sensor_topic < 0) { warnx("advert err"); } } while(0); @@ -442,8 +443,8 @@ SF0X::ioctl(struct file *filp, int cmd, unsigned long arg) ssize_t SF0X::read(struct file *filp, char *buffer, size_t buflen) { - unsigned count = buflen / sizeof(struct range_finder_report); - struct range_finder_report *rbuf = reinterpret_cast(buffer); + unsigned count = buflen / sizeof(struct distance_sensor_s); + struct distance_sensor_s *rbuf = reinterpret_cast(buffer); int ret = 0; /* buffer must be large enough */ @@ -571,18 +572,19 @@ SF0X::collect() debug("val (float): %8.4f, raw: %s, valid: %s", (double)si_units, _linebuf, ((valid) ? "OK" : "NO")); - struct range_finder_report report; + struct distance_sensor_s report; - /* this should be fairly close to the end of the measurement, so the best approximation of the time */ - report.timestamp = hrt_absolute_time(); - report.error_count = perf_event_count(_comms_errors); - report.distance = si_units; - report.minimum_distance = get_minimum_distance(); - report.maximum_distance = get_maximum_distance(); - report.valid = valid && (si_units > get_minimum_distance() && si_units < get_maximum_distance() ? 1 : 0); + report.time_boot_ms = hrt_absolute_time(); + report.id = 2; + report.type = 0; + report.orientation = 8; + report.current_distance = si_units; + report.min_distance = get_minimum_distance(); + report.max_distance = get_maximum_distance(); + report.covariance = 0.0; /* publish it */ - orb_publish(ORB_ID(sensor_range_finder), _range_finder_topic, &report); + orb_publish(ORB_ID(distance_sensor), _distance_sensor_topic, &report); if (_reports->force(&report)) { perf_count(_buffer_overflows); @@ -815,7 +817,7 @@ void stop() void test() { - struct range_finder_report report; + struct distance_sensor_s report; ssize_t sz; int fd = open(RANGE_FINDER0_DEVICE_PATH, O_RDONLY); @@ -832,8 +834,8 @@ test() } warnx("single read"); - warnx("val: %0.2f m", (double)report.distance); - warnx("time: %lld", report.timestamp); + warnx("val: %0.2f m", (double)report.current_distance); + warnx("time: %d", report.time_boot_ms); /* start the sensor polling at 2 Hz rate */ if (OK != ioctl(fd, SENSORIOCSPOLLRATE, 2)) { @@ -863,8 +865,8 @@ test() } warnx("read #%u", i); - warnx("val: %0.3f m", (double)report.distance); - warnx("time: %lld", report.timestamp); + warnx("val: %0.3f m", (double)report.current_distance); + warnx("time: %d", report.time_boot_ms); } /* reset the sensor polling to the default rate */ diff --git a/src/drivers/trone/trone.cpp b/src/drivers/trone/trone.cpp index a3c8633725..9fde00cafa 100644 --- a/src/drivers/trone/trone.cpp +++ b/src/drivers/trone/trone.cpp @@ -68,6 +68,7 @@ #include #include +#include #include @@ -128,7 +129,7 @@ private: bool _collect_phase; int _class_instance; - orb_advert_t _range_finder_topic; + orb_advert_t _distance_sensor_topic; perf_counter_t _sample_perf; perf_counter_t _comms_errors; @@ -209,7 +210,7 @@ static const uint8_t crc_table[] = { 0xfa, 0xfd, 0xf4, 0xf3 }; -static uint8_t crc8(uint8_t *p, uint8_t len) { +/* static uint8_t crc8(uint8_t *p, uint8_t len) { uint16_t i; uint16_t crc = 0x0; @@ -219,7 +220,7 @@ static uint8_t crc8(uint8_t *p, uint8_t len) { } return crc & 0xFF; -} +}*/ /* * Driver 'main' command. @@ -235,7 +236,7 @@ TRONE::TRONE(int bus, int address) : _measure_ticks(0), _collect_phase(false), _class_instance(-1), - _range_finder_topic(-1), + _distance_sensor_topic(-1), _sample_perf(perf_alloc(PC_ELAPSED, "trone_read")), _comms_errors(perf_alloc(PC_COUNT, "trone_comms_errors")), _buffer_overflows(perf_alloc(PC_COUNT, "trone_buffer_overflows")) @@ -281,7 +282,7 @@ TRONE::init() } /* allocate basic report buffers */ - _reports = new RingBuffer(2, sizeof(range_finder_report)); + _reports = new RingBuffer(2, sizeof(distance_sensor_s)); if (_reports == nullptr) { goto out; @@ -291,12 +292,12 @@ TRONE::init() if (_class_instance == CLASS_DEVICE_PRIMARY) { /* get a publish handle on the range finder topic */ - struct range_finder_report rf_report; + struct distance_sensor_s rf_report; measure(); _reports->get(&rf_report); - _range_finder_topic = orb_advertise(ORB_ID(sensor_range_finder), &rf_report); + _distance_sensor_topic = orb_advertise(ORB_ID(distance_sensor), &rf_report); - if (_range_finder_topic < 0) { + if (_distance_sensor_topic < 0) { debug("failed to create sensor_range_finder object. Did you start uOrb?"); } } @@ -468,8 +469,8 @@ TRONE::ioctl(struct file *filp, int cmd, unsigned long arg) ssize_t TRONE::read(struct file *filp, char *buffer, size_t buflen) { - unsigned count = buflen / sizeof(struct range_finder_report); - struct range_finder_report *rbuf = reinterpret_cast(buffer); + unsigned count = buflen / sizeof(struct distance_sensor_s); + struct distance_sensor_s *rbuf = reinterpret_cast(buffer); int ret = 0; /* buffer must be large enough */ @@ -568,20 +569,20 @@ TRONE::collect() uint16_t distance = (val[0] << 8) | val[1]; float si_units = distance * 0.001f; /* mm to m */ - struct range_finder_report report; - - /* this should be fairly close to the end of the measurement, so the best approximation of the time */ - report.timestamp = hrt_absolute_time(); - report.error_count = perf_event_count(_comms_errors); - report.distance = si_units; - report.minimum_distance = get_minimum_distance(); - report.maximum_distance = get_maximum_distance(); - report.valid = crc8(val, 2) == val[2] && si_units > get_minimum_distance() && si_units < get_maximum_distance() ? 1 : 0; + struct distance_sensor_s report; + report.time_boot_ms = hrt_absolute_time(); + report.id = 3; + report.type = 0; + report.orientation = 8; + report.current_distance = si_units; + report.min_distance = get_minimum_distance(); + report.max_distance = get_maximum_distance(); + report.covariance = 0.0; /* publish it, if we are the primary */ - if (_range_finder_topic >= 0) { - orb_publish(ORB_ID(sensor_range_finder), _range_finder_topic, &report); + if (_distance_sensor_topic >= 0) { + orb_publish(ORB_ID(distance_sensor), _distance_sensor_topic, &report); } if (_reports->force(&report)) { @@ -787,7 +788,7 @@ void stop() void test() { - struct range_finder_report report; + struct distance_sensor_s report; ssize_t sz; int ret; @@ -805,8 +806,8 @@ test() } warnx("single read"); - warnx("measurement: %0.2f m", (double)report.distance); - warnx("time: %lld", report.timestamp); + warnx("measurement: %0.2f m", (double)report.current_distance); + warnx("time: %d", report.time_boot_ms); /* start the sensor polling at 2Hz */ if (OK != ioctl(fd, SENSORIOCSPOLLRATE, 2)) { @@ -834,9 +835,8 @@ test() } warnx("periodic read %u", i); - warnx("valid %u", report.valid); - warnx("measurement: %0.3f", (double)report.distance); - warnx("time: %lld", report.timestamp); + warnx("measurement: %0.3f", (double)report.current_distance); + warnx("time: %d", report.time_boot_ms); } /* reset the sensor polling to default rate */ From f73a942c05dd08a6106aac2a0c7bac0c35092dda Mon Sep 17 00:00:00 2001 From: Ban Siesta Date: Sun, 24 May 2015 09:21:39 +0100 Subject: [PATCH 19/19] Tools: don't try to generate messages for hidden files starting with a dot. --- Tools/px_generate_uorb_topic_headers.py | 20 +++++++++++++------- 1 file changed, 13 insertions(+), 7 deletions(-) diff --git a/Tools/px_generate_uorb_topic_headers.py b/Tools/px_generate_uorb_topic_headers.py index 4f821b0608..75acd6a608 100755 --- a/Tools/px_generate_uorb_topic_headers.py +++ b/Tools/px_generate_uorb_topic_headers.py @@ -54,7 +54,7 @@ On a Debian/Ubuntu system please run: sudo apt-get install python-empy sudo pip install catkin_pkg - + On MacOS please run: sudo pip install empy catkin_pkg @@ -95,13 +95,19 @@ def convert_dir(inputdir, outputdir, templatedir): """ includepath = incl_default + [':'.join([package, inputdir])] for f in os.listdir(inputdir): + # Ignore hidden files + if f.startswith("."): + continue + fn = os.path.join(inputdir, f) - if os.path.isfile(fn): - convert_file( - fn, - outputdir, - templatedir, - includepath) + # Only look at actual files + if not os.path.isfile(fn): + continue + + convert_file(fn, + outputdir, + templatedir, + includepath) def copy_changed(inputdir, outputdir, prefix=''):