From db48df15c8e141eb8c4eea5a20aec9ebdeb049d2 Mon Sep 17 00:00:00 2001 From: TSC21 Date: Thu, 21 May 2015 12:26:55 +0100 Subject: [PATCH] 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} };