From 22f1b784525622ac39c9db50675c3937c7aeecf0 Mon Sep 17 00:00:00 2001 From: Denis Yeldandi Date: Fri, 17 Oct 2014 21:07:34 +0400 Subject: Adding vertical (Z) velocity to inav estimator --- .../position_estimator_inav/position_estimator_inav_main.c | 3 +++ .../position_estimator_inav/position_estimator_inav_params.c | 12 ++++++++++++ .../position_estimator_inav/position_estimator_inav_params.h | 2 ++ 3 files changed, 17 insertions(+) (limited to 'src') diff --git a/src/modules/position_estimator_inav/position_estimator_inav_main.c b/src/modules/position_estimator_inav/position_estimator_inav_main.c index 81bbaa658..a1bfa2dfc 100644 --- a/src/modules/position_estimator_inav/position_estimator_inav_main.c +++ b/src/modules/position_estimator_inav/position_estimator_inav_main.c @@ -846,6 +846,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) float w_xy_gps_p = params.w_xy_gps_p * w_gps_xy; float w_xy_gps_v = params.w_xy_gps_v * w_gps_xy; float w_z_gps_p = params.w_z_gps_p * w_gps_z; + float w_z_gps_v = params.w_z_gps_v * w_gps_z; float w_xy_vision_p = params.w_xy_vision_p; float w_xy_vision_v = params.w_xy_vision_v; @@ -876,6 +877,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) if (use_gps_z) { accel_bias_corr[2] -= corr_gps[2][0] * w_z_gps_p * w_z_gps_p; + accel_bias_corr[2] -= corr_gps[2][1] * w_z_gps_v; } /* transform error vector from NED frame to body frame */ @@ -960,6 +962,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) epv = fminf(epv, gps.epv); inertial_filter_correct(corr_gps[2][0], dt, z_est, 0, w_z_gps_p); + inertial_filter_correct(corr_gps[2][1], dt, z_est, 1, w_z_gps_v); } if (use_vision_z) { diff --git a/src/modules/position_estimator_inav/position_estimator_inav_params.c b/src/modules/position_estimator_inav/position_estimator_inav_params.c index cc0fb4155..1a1b3d310 100644 --- a/src/modules/position_estimator_inav/position_estimator_inav_params.c +++ b/src/modules/position_estimator_inav/position_estimator_inav_params.c @@ -63,6 +63,17 @@ PARAM_DEFINE_FLOAT(INAV_W_Z_BARO, 0.5f); */ PARAM_DEFINE_FLOAT(INAV_W_Z_GPS_P, 0.005f); +/** + * Z velocity weight for GPS + * + * Weight (cutoff frequency) for GPS altitude velocity measurements. + * + * @min 0.0 + * @max 10.0 + * @group Position Estimator INAV + */ +PARAM_DEFINE_FLOAT(INAV_W_Z_GPS_V, 1.0f); + /** * Z axis weight for vision * @@ -281,6 +292,7 @@ int parameters_init(struct position_estimator_inav_param_handles *h) { h->w_z_baro = param_find("INAV_W_Z_BARO"); h->w_z_gps_p = param_find("INAV_W_Z_GPS_P"); + h->w_z_gps_v = param_find("INAV_W_Z_GPS_V"); h->w_z_vision_p = param_find("INAV_W_Z_VIS_P"); h->w_z_sonar = param_find("INAV_W_Z_SONAR"); h->w_xy_gps_p = param_find("INAV_W_XY_GPS_P"); diff --git a/src/modules/position_estimator_inav/position_estimator_inav_params.h b/src/modules/position_estimator_inav/position_estimator_inav_params.h index d0a65e42e..51bbda412 100644 --- a/src/modules/position_estimator_inav/position_estimator_inav_params.h +++ b/src/modules/position_estimator_inav/position_estimator_inav_params.h @@ -44,6 +44,7 @@ struct position_estimator_inav_params { float w_z_baro; float w_z_gps_p; + float w_z_gps_v; float w_z_vision_p; float w_z_sonar; float w_xy_gps_p; @@ -68,6 +69,7 @@ struct position_estimator_inav_params { struct position_estimator_inav_param_handles { param_t w_z_baro; param_t w_z_gps_p; + param_t w_z_gps_v; param_t w_z_vision_p; param_t w_z_sonar; param_t w_xy_gps_p; -- cgit v1.2.3 From 0632d882bb734ac52ff5f7f5d1dc7930046e4600 Mon Sep 17 00:00:00 2001 From: Denis Yeldandi Date: Wed, 21 Jan 2015 12:22:07 +0300 Subject: INAV_W_Z_GPS_V defaults to 0 --- src/modules/position_estimator_inav/position_estimator_inav_params.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) (limited to 'src') diff --git a/src/modules/position_estimator_inav/position_estimator_inav_params.c b/src/modules/position_estimator_inav/position_estimator_inav_params.c index 1a1b3d310..b2771f1a6 100644 --- a/src/modules/position_estimator_inav/position_estimator_inav_params.c +++ b/src/modules/position_estimator_inav/position_estimator_inav_params.c @@ -72,7 +72,7 @@ PARAM_DEFINE_FLOAT(INAV_W_Z_GPS_P, 0.005f); * @max 10.0 * @group Position Estimator INAV */ -PARAM_DEFINE_FLOAT(INAV_W_Z_GPS_V, 1.0f); +PARAM_DEFINE_FLOAT(INAV_W_Z_GPS_V, 0.0f); /** * Z axis weight for vision -- cgit v1.2.3 From eea3c801f4be7efa306af01a8153e8bbee4d43ce Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Sat, 10 Jan 2015 22:26:49 +0300 Subject: UAVCAN perf counters --- src/modules/uavcan/uavcan_main.cpp | 30 ++++++++++++++++++++++++++++++ src/modules/uavcan/uavcan_main.hpp | 9 +++++++-- 2 files changed, 37 insertions(+), 2 deletions(-) (limited to 'src') diff --git a/src/modules/uavcan/uavcan_main.cpp b/src/modules/uavcan/uavcan_main.cpp index 60901e0c7..af5f2ec96 100644 --- a/src/modules/uavcan/uavcan_main.cpp +++ b/src/modules/uavcan/uavcan_main.cpp @@ -81,6 +81,22 @@ UavcanNode::UavcanNode(uavcan::ICanDriver &can_driver, uavcan::ISystemClock &sys if (res < 0) { std::abort(); } + + if (_perfcnt_node_spin_elapsed == nullptr) { + errx(1, "uavcan: couldn't allocate _perfcnt_node_spin_elapsed"); + } + + if (_perfcnt_esc_mixer_output_elapsed == nullptr) { + errx(1, "uavcan: couldn't allocate _perfcnt_esc_mixer_output_elapsed"); + } + + if (_perfcnt_esc_mixer_total_elapsed == nullptr) { + errx(1, "uavcan: couldn't allocate _perfcnt_esc_mixer_total_elapsed"); + } + + if (_perfcnt_esc_mixer_subscriptions == nullptr) { + errx(1, "uavcan: couldn't allocate _perfcnt_esc_mixer_subscriptions"); + } } UavcanNode::~UavcanNode() @@ -118,6 +134,11 @@ UavcanNode::~UavcanNode() } _instance = nullptr; + + perf_free(_perfcnt_node_spin_elapsed); + perf_free(_perfcnt_esc_mixer_output_elapsed); + perf_free(_perfcnt_esc_mixer_total_elapsed); + perf_free(_perfcnt_esc_mixer_subscriptions); } int UavcanNode::start(uavcan::NodeID node_id, uint32_t bitrate) @@ -265,10 +286,12 @@ int UavcanNode::init(uavcan::NodeID node_id) void UavcanNode::node_spin_once() { + perf_begin(_perfcnt_node_spin_elapsed); const int spin_res = _node.spin(uavcan::MonotonicTime()); if (spin_res < 0) { warnx("node spin error %i", spin_res); } + perf_end(_perfcnt_node_spin_elapsed); } /* @@ -344,8 +367,12 @@ int UavcanNode::run() // Mutex is unlocked while the thread is blocked on IO multiplexing (void)pthread_mutex_unlock(&_node_mutex); + perf_end(_perfcnt_esc_mixer_total_elapsed); // end goes first, it's not a mistake + const int poll_ret = ::poll(_poll_fds, _poll_fds_num, PollTimeoutMs); + perf_begin(_perfcnt_esc_mixer_total_elapsed); + (void)pthread_mutex_lock(&_node_mutex); node_spin_once(); // Non-blocking @@ -430,7 +457,9 @@ int UavcanNode::run() } // Output to the bus _outputs.timestamp = hrt_absolute_time(); + perf_begin(_perfcnt_esc_mixer_output_elapsed); _esc_controller.update_outputs(_outputs.output, _outputs.noutputs); + perf_end(_perfcnt_esc_mixer_output_elapsed); } @@ -498,6 +527,7 @@ UavcanNode::arm_actuators(bool arm) void UavcanNode::subscribe() { + perf_count(_perfcnt_esc_mixer_subscriptions); // Subscribe/unsubscribe to required actuator control groups uint32_t sub_groups = _groups_required & ~_groups_subscribed; uint32_t unsub_groups = _groups_subscribed & ~_groups_required; diff --git a/src/modules/uavcan/uavcan_main.hpp b/src/modules/uavcan/uavcan_main.hpp index 98f2e5ad4..8a0993f15 100644 --- a/src/modules/uavcan/uavcan_main.hpp +++ b/src/modules/uavcan/uavcan_main.hpp @@ -34,9 +34,9 @@ #pragma once #include - #include #include +#include #include #include @@ -66,7 +66,7 @@ */ class UavcanNode : public device::CDev { - static constexpr unsigned MemPoolSize = 10752; + static constexpr unsigned MemPoolSize = 10752; ///< Refer to the libuavcan manual to learn why static constexpr unsigned RxQueueLenPerIface = 64; static constexpr unsigned StackSize = 3000; @@ -142,4 +142,9 @@ private: // index into _poll_fds for each _control_subs handle uint8_t _poll_ids[NUM_ACTUATOR_CONTROL_GROUPS_UAVCAN]; + + perf_counter_t _perfcnt_node_spin_elapsed = perf_alloc(PC_ELAPSED, "uavcan_node_spin_elapsed"); + perf_counter_t _perfcnt_esc_mixer_output_elapsed = perf_alloc(PC_ELAPSED, "uavcan_esc_mixer_output_elapsed"); + perf_counter_t _perfcnt_esc_mixer_total_elapsed = perf_alloc(PC_ELAPSED, "uavcan_esc_mixer_total_elapsed"); + perf_counter_t _perfcnt_esc_mixer_subscriptions = perf_alloc(PC_COUNT, "uavcan_esc_mixer_subscriptions"); }; -- cgit v1.2.3 From 7f0f4b3072008239f4481c79769ab1b00f92e707 Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Sun, 11 Jan 2015 00:11:48 +0300 Subject: UavcanEscController broadcasting percounter --- src/modules/uavcan/actuators/esc.cpp | 14 ++++++++++++++ src/modules/uavcan/actuators/esc.hpp | 1 + 2 files changed, 15 insertions(+) (limited to 'src') diff --git a/src/modules/uavcan/actuators/esc.cpp b/src/modules/uavcan/actuators/esc.cpp index 9f682c7e1..995c8987c 100644 --- a/src/modules/uavcan/actuators/esc.cpp +++ b/src/modules/uavcan/actuators/esc.cpp @@ -49,12 +49,24 @@ UavcanEscController::UavcanEscController(uavcan::INode &node) : _uavcan_sub_status(node), _orb_timer(node) { + if (_perfcnt_invalid_input == nullptr) { + errx(1, "uavcan: couldn't allocate _perfcnt_invalid_input"); + } + + if (_perfcnt_scaling_error == nullptr) { + errx(1, "uavcan: couldn't allocate _perfcnt_scaling_error"); + } + + if (_perfcnt_broadcast_elapsed == nullptr) { + errx(1, "uavcan: couldn't allocate _perfcnt_broadcast_elapsed"); + } } UavcanEscController::~UavcanEscController() { perf_free(_perfcnt_invalid_input); perf_free(_perfcnt_scaling_error); + perf_free(_perfcnt_broadcast_elapsed); } int UavcanEscController::init() @@ -129,7 +141,9 @@ void UavcanEscController::update_outputs(float *outputs, unsigned num_outputs) * Publish the command message to the bus * Note that for a quadrotor it takes one CAN frame */ + perf_begin(_perfcnt_broadcast_elapsed); (void)_uavcan_pub_raw_cmd.broadcast(msg); + perf_end(_perfcnt_broadcast_elapsed); } void UavcanEscController::arm_all_escs(bool arm) diff --git a/src/modules/uavcan/actuators/esc.hpp b/src/modules/uavcan/actuators/esc.hpp index 12c035542..498fb9dd8 100644 --- a/src/modules/uavcan/actuators/esc.hpp +++ b/src/modules/uavcan/actuators/esc.hpp @@ -109,4 +109,5 @@ private: */ perf_counter_t _perfcnt_invalid_input = perf_alloc(PC_COUNT, "uavcan_esc_invalid_input"); perf_counter_t _perfcnt_scaling_error = perf_alloc(PC_COUNT, "uavcan_esc_scaling_error"); + perf_counter_t _perfcnt_broadcast_elapsed = perf_alloc(PC_ELAPSED, "uavcan_esc_broadcast_elapsed"); }; -- cgit v1.2.3 From 0901d999924012b482a5379854c24eb9f2c6f345 Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Sun, 11 Jan 2015 04:38:20 +0300 Subject: UAVCAN update (speed optimizations) --- src/lib/uavcan | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) (limited to 'src') diff --git a/src/lib/uavcan b/src/lib/uavcan index c4c45b995..20439bb39 160000 --- a/src/lib/uavcan +++ b/src/lib/uavcan @@ -1 +1 @@ -Subproject commit c4c45b995f5c8192c7a36c4293c201711ceac74d +Subproject commit 20439bb3975c34a24c3c337a1f231fbf973a41e8 -- cgit v1.2.3 From 6bbacc4271680cc262094af7f9a34807ff5c34ed Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Sat, 17 Jan 2015 04:04:31 +0300 Subject: Intrusive changes made for UAVCAN profiling. Will be reverted in the next commit (this one is needed to keep the changes in history) --- ROMFS/px4fmu_common/init.d/rcS | 5 ++++- makefiles/toolchain_gnu-arm-eabi.mk | 2 +- src/modules/uavcan/actuators/esc.cpp | 10 +++++----- src/modules/uavcan/uavcan_main.cpp | 10 ++++++++++ 4 files changed, 20 insertions(+), 7 deletions(-) (limited to 'src') diff --git a/ROMFS/px4fmu_common/init.d/rcS b/ROMFS/px4fmu_common/init.d/rcS index 2c9387ff0..02f4649d5 100644 --- a/ROMFS/px4fmu_common/init.d/rcS +++ b/ROMFS/px4fmu_common/init.d/rcS @@ -8,7 +8,7 @@ # # Default to auto-start mode. # -set MODE autostart +set MODE none set FRC /fs/microsd/etc/rc.txt set FCONFIG /fs/microsd/etc/config.txt @@ -656,6 +656,9 @@ then # End of autostart fi +param set UAVCAN_ENABLE 1 +sh /etc/init.d/rc.uavcan + # There is no further processing, so we can free some RAM # XXX potentially unset all script variables. unset TUNE_ERR diff --git a/makefiles/toolchain_gnu-arm-eabi.mk b/makefiles/toolchain_gnu-arm-eabi.mk index fbe378f50..1bbc5437d 100644 --- a/makefiles/toolchain_gnu-arm-eabi.mk +++ b/makefiles/toolchain_gnu-arm-eabi.mk @@ -105,7 +105,7 @@ ARCHDEFINES += -DCONFIG_ARCH_BOARD_$(CONFIG_BOARD) # optimisation flags # ARCHOPTIMIZATION = $(MAXOPTIMIZATION) \ - -g \ + -g3 \ -fno-strict-aliasing \ -fno-strength-reduce \ -fomit-frame-pointer \ diff --git a/src/modules/uavcan/actuators/esc.cpp b/src/modules/uavcan/actuators/esc.cpp index 995c8987c..f7adbed76 100644 --- a/src/modules/uavcan/actuators/esc.cpp +++ b/src/modules/uavcan/actuators/esc.cpp @@ -98,11 +98,11 @@ void UavcanEscController::update_outputs(float *outputs, unsigned num_outputs) /* * Rate limiting - we don't want to congest the bus */ - const auto timestamp = _node.getMonotonicTime(); - if ((timestamp - _prev_cmd_pub).toUSec() < (1000000 / MAX_RATE_HZ)) { - return; - } - _prev_cmd_pub = timestamp; +// const auto timestamp = _node.getMonotonicTime(); +// if ((timestamp - _prev_cmd_pub).toUSec() < (1000000 / MAX_RATE_HZ)) { +// return; +// } +// _prev_cmd_pub = timestamp; /* * Fill the command message diff --git a/src/modules/uavcan/uavcan_main.cpp b/src/modules/uavcan/uavcan_main.cpp index af5f2ec96..c29cd8323 100644 --- a/src/modules/uavcan/uavcan_main.cpp +++ b/src/modules/uavcan/uavcan_main.cpp @@ -357,6 +357,16 @@ int UavcanNode::run() _actuator_direct_poll_fd_num = add_poll_fd(_actuator_direct_sub); } + _esc_controller.arm_all_escs(true); + while (true) { + for (int i = 0; i < 1000; i++) { + node_spin_once(); + _outputs.noutputs = 8; + _esc_controller.update_outputs(_outputs.output, _outputs.noutputs); + } + ::usleep(1000); + } + while (!_task_should_exit) { // update actuator controls subscriptions if needed if (_groups_subscribed != _groups_required) { -- cgit v1.2.3 From d87bb4dfcb59601bc51d46332bbe0db78d11294a Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Sat, 17 Jan 2015 04:04:48 +0300 Subject: Revert "Intrusive changes made for UAVCAN profiling. Will be reverted in the next commit (this one is needed to keep the changes in history)" This reverts commit 4c301d9dcf180e39186fa6753c7a3d3215b3cfa7. --- ROMFS/px4fmu_common/init.d/rcS | 5 +---- makefiles/toolchain_gnu-arm-eabi.mk | 2 +- src/modules/uavcan/actuators/esc.cpp | 10 +++++----- src/modules/uavcan/uavcan_main.cpp | 10 ---------- 4 files changed, 7 insertions(+), 20 deletions(-) (limited to 'src') diff --git a/ROMFS/px4fmu_common/init.d/rcS b/ROMFS/px4fmu_common/init.d/rcS index 02f4649d5..2c9387ff0 100644 --- a/ROMFS/px4fmu_common/init.d/rcS +++ b/ROMFS/px4fmu_common/init.d/rcS @@ -8,7 +8,7 @@ # # Default to auto-start mode. # -set MODE none +set MODE autostart set FRC /fs/microsd/etc/rc.txt set FCONFIG /fs/microsd/etc/config.txt @@ -656,9 +656,6 @@ then # End of autostart fi -param set UAVCAN_ENABLE 1 -sh /etc/init.d/rc.uavcan - # There is no further processing, so we can free some RAM # XXX potentially unset all script variables. unset TUNE_ERR diff --git a/makefiles/toolchain_gnu-arm-eabi.mk b/makefiles/toolchain_gnu-arm-eabi.mk index 1bbc5437d..fbe378f50 100644 --- a/makefiles/toolchain_gnu-arm-eabi.mk +++ b/makefiles/toolchain_gnu-arm-eabi.mk @@ -105,7 +105,7 @@ ARCHDEFINES += -DCONFIG_ARCH_BOARD_$(CONFIG_BOARD) # optimisation flags # ARCHOPTIMIZATION = $(MAXOPTIMIZATION) \ - -g3 \ + -g \ -fno-strict-aliasing \ -fno-strength-reduce \ -fomit-frame-pointer \ diff --git a/src/modules/uavcan/actuators/esc.cpp b/src/modules/uavcan/actuators/esc.cpp index f7adbed76..995c8987c 100644 --- a/src/modules/uavcan/actuators/esc.cpp +++ b/src/modules/uavcan/actuators/esc.cpp @@ -98,11 +98,11 @@ void UavcanEscController::update_outputs(float *outputs, unsigned num_outputs) /* * Rate limiting - we don't want to congest the bus */ -// const auto timestamp = _node.getMonotonicTime(); -// if ((timestamp - _prev_cmd_pub).toUSec() < (1000000 / MAX_RATE_HZ)) { -// return; -// } -// _prev_cmd_pub = timestamp; + const auto timestamp = _node.getMonotonicTime(); + if ((timestamp - _prev_cmd_pub).toUSec() < (1000000 / MAX_RATE_HZ)) { + return; + } + _prev_cmd_pub = timestamp; /* * Fill the command message diff --git a/src/modules/uavcan/uavcan_main.cpp b/src/modules/uavcan/uavcan_main.cpp index c29cd8323..af5f2ec96 100644 --- a/src/modules/uavcan/uavcan_main.cpp +++ b/src/modules/uavcan/uavcan_main.cpp @@ -357,16 +357,6 @@ int UavcanNode::run() _actuator_direct_poll_fd_num = add_poll_fd(_actuator_direct_sub); } - _esc_controller.arm_all_escs(true); - while (true) { - for (int i = 0; i < 1000; i++) { - node_spin_once(); - _outputs.noutputs = 8; - _esc_controller.update_outputs(_outputs.output, _outputs.noutputs); - } - ::usleep(1000); - } - while (!_task_should_exit) { // update actuator controls subscriptions if needed if (_groups_subscribed != _groups_required) { -- cgit v1.2.3 From c2bc298409585aadce4b60dff5e6fada87c6c436 Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Sat, 17 Jan 2015 20:40:09 +0300 Subject: Disable instrumentation for the uavcan module --- makefiles/toolchain_gnu-arm-eabi.mk | 2 +- src/modules/uavcan/module.mk | 2 ++ 2 files changed, 3 insertions(+), 1 deletion(-) (limited to 'src') diff --git a/makefiles/toolchain_gnu-arm-eabi.mk b/makefiles/toolchain_gnu-arm-eabi.mk index 1bbc5437d..5a7f8390f 100644 --- a/makefiles/toolchain_gnu-arm-eabi.mk +++ b/makefiles/toolchain_gnu-arm-eabi.mk @@ -116,7 +116,7 @@ ARCHOPTIMIZATION = $(MAXOPTIMIZATION) \ # enable precise stack overflow tracking # note - requires corresponding support in NuttX -INSTRUMENTATIONDEFINES = $(ARCHINSTRUMENTATIONDEFINES_$(CONFIG_ARCH)) +INSTRUMENTATIONDEFINES ?= $(ARCHINSTRUMENTATIONDEFINES_$(CONFIG_ARCH)) # Language-specific flags # diff --git a/src/modules/uavcan/module.mk b/src/modules/uavcan/module.mk index e5d30f6c4..64e60bd18 100644 --- a/src/modules/uavcan/module.mk +++ b/src/modules/uavcan/module.mk @@ -40,6 +40,8 @@ MODULE_COMMAND = uavcan MAXOPTIMIZATION = -Os +INSTRUMENTATIONDEFINES = -fno-instrument-functions + # Main SRCS += uavcan_main.cpp \ uavcan_clock.cpp \ -- cgit v1.2.3 From f49f183f74be7d602fee6a1192538273d3d11cac Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Sat, 17 Jan 2015 22:25:33 +0300 Subject: UAVCAN module: -O3 instead of -Os; fixed instrumentation defines --- src/modules/uavcan/module.mk | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) (limited to 'src') diff --git a/src/modules/uavcan/module.mk b/src/modules/uavcan/module.mk index 64e60bd18..4d1b7156b 100644 --- a/src/modules/uavcan/module.mk +++ b/src/modules/uavcan/module.mk @@ -38,9 +38,9 @@ MODULE_COMMAND = uavcan -MAXOPTIMIZATION = -Os +MAXOPTIMIZATION = -O3 -INSTRUMENTATIONDEFINES = -fno-instrument-functions +INSTRUMENTATIONDEFINES = -fno-instrument-functions -ffixed-r10 # Main SRCS += uavcan_main.cpp \ -- cgit v1.2.3 From 1d13edcf9221cfeabfe23eeff046dd7324f532b8 Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Sat, 17 Jan 2015 22:35:59 +0300 Subject: Stack checks made optional: ENABLE_STACK_CHECKS --- makefiles/toolchain_gnu-arm-eabi.mk | 16 ++++++++++++---- src/modules/uavcan/module.mk | 3 ++- 2 files changed, 14 insertions(+), 5 deletions(-) (limited to 'src') diff --git a/makefiles/toolchain_gnu-arm-eabi.mk b/makefiles/toolchain_gnu-arm-eabi.mk index 5a7f8390f..b38f40eb4 100644 --- a/makefiles/toolchain_gnu-arm-eabi.mk +++ b/makefiles/toolchain_gnu-arm-eabi.mk @@ -80,14 +80,22 @@ ARCHCPUFLAGS_CORTEXM3 = -mcpu=cortex-m3 \ -march=armv7-m \ -mfloat-abi=soft -ARCHINSTRUMENTATIONDEFINES_CORTEXM4F = -finstrument-functions \ - -ffixed-r10 +ARCHINSTRUMENTATIONDEFINES_CORTEXM4F = -ffixed-r10 -ARCHINSTRUMENTATIONDEFINES_CORTEXM4 = -finstrument-functions \ - -ffixed-r10 +ARCHINSTRUMENTATIONDEFINES_CORTEXM4 = -ffixed-r10 ARCHINSTRUMENTATIONDEFINES_CORTEXM3 = +# Enabling stack checks if requested +# +ENABLE_STACK_CHECKS ?= 0 +ifneq ($(ENABLE_STACK_CHECKS),0) +$(info Stack checks enabled) +ARCHINSTRUMENTATIONDEFINES_CORTEXM4F += -finstrument-functions +ARCHINSTRUMENTATIONDEFINES_CORTEXM4 += -finstrument-functions +ARCHINSTRUMENTATIONDEFINES_CORTEXM3 += +endif + # Pick the right set of flags for the architecture. # ARCHCPUFLAGS = $(ARCHCPUFLAGS_$(CONFIG_ARCH)) diff --git a/src/modules/uavcan/module.mk b/src/modules/uavcan/module.mk index 4d1b7156b..a3c6e46a0 100644 --- a/src/modules/uavcan/module.mk +++ b/src/modules/uavcan/module.mk @@ -40,7 +40,8 @@ MODULE_COMMAND = uavcan MAXOPTIMIZATION = -O3 -INSTRUMENTATIONDEFINES = -fno-instrument-functions -ffixed-r10 +# Instrumentation makes the CPU load about 3 times higher, see https://github.com/PX4/Firmware/issues/1417 +INSTRUMENTATIONDEFINES += -fno-instrument-functions # Main SRCS += uavcan_main.cpp \ -- cgit v1.2.3 From ed27e583e01bee74a5bf46561b9aea7bd630846b Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Sun, 18 Jan 2015 01:30:46 +0300 Subject: UAVCAN update --- src/lib/uavcan | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) (limited to 'src') diff --git a/src/lib/uavcan b/src/lib/uavcan index 20439bb39..c9227cf6d 160000 --- a/src/lib/uavcan +++ b/src/lib/uavcan @@ -1 +1 @@ -Subproject commit 20439bb3975c34a24c3c337a1f231fbf973a41e8 +Subproject commit c9227cf6d24e0bafee3ada499dea183446aa35b1 -- cgit v1.2.3 From c9eae96cf67ebdf6d202dc7ecac55a5b4a670a50 Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Sun, 18 Jan 2015 04:05:50 +0300 Subject: Frame size fix fix per Lorenz's suggestion --- src/modules/attitude_estimator_ekf/module.mk | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) (limited to 'src') diff --git a/src/modules/attitude_estimator_ekf/module.mk b/src/modules/attitude_estimator_ekf/module.mk index 1158536e1..d158d7a49 100644 --- a/src/modules/attitude_estimator_ekf/module.mk +++ b/src/modules/attitude_estimator_ekf/module.mk @@ -43,6 +43,6 @@ SRCS = attitude_estimator_ekf_main.cpp \ MODULE_STACKSIZE = 1200 -EXTRACFLAGS = -Wno-float-equal -Wframe-larger-than=3600 +EXTRACFLAGS = -Wno-float-equal -Wframe-larger-than=3700 EXTRACXXFLAGS = -Wframe-larger-than=2400 -- cgit v1.2.3 From 2ebd7099de83c603b01bedd278c38a4eb6b77b2b Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Sun, 18 Jan 2015 16:09:46 +0300 Subject: Globally configurable stack checks, R10 is always fixed --- makefiles/toolchain_gnu-arm-eabi.mk | 16 +++++++--------- nuttx-configs/px4fmu-v2/nsh/Make.defs | 6 ++++++ src/modules/uavcan/module.mk | 3 --- 3 files changed, 13 insertions(+), 12 deletions(-) (limited to 'src') diff --git a/makefiles/toolchain_gnu-arm-eabi.mk b/makefiles/toolchain_gnu-arm-eabi.mk index 7e1f510e6..7055137ca 100644 --- a/makefiles/toolchain_gnu-arm-eabi.mk +++ b/makefiles/toolchain_gnu-arm-eabi.mk @@ -80,20 +80,18 @@ ARCHCPUFLAGS_CORTEXM3 = -mcpu=cortex-m3 \ -march=armv7-m \ -mfloat-abi=soft -ARCHINSTRUMENTATIONDEFINES_CORTEXM4F = -ffixed-r10 - -ARCHINSTRUMENTATIONDEFINES_CORTEXM4 = -ffixed-r10 - -ARCHINSTRUMENTATIONDEFINES_CORTEXM3 = - # Enabling stack checks if requested # ENABLE_STACK_CHECKS ?= 0 ifneq ($(ENABLE_STACK_CHECKS),0) $(info Stack checks enabled) -ARCHINSTRUMENTATIONDEFINES_CORTEXM4F += -finstrument-functions -ARCHINSTRUMENTATIONDEFINES_CORTEXM4 += -finstrument-functions -ARCHINSTRUMENTATIONDEFINES_CORTEXM3 += +ARCHINSTRUMENTATIONDEFINES_CORTEXM4F = -finstrument-functions -ffixed-r10 +ARCHINSTRUMENTATIONDEFINES_CORTEXM4 = -finstrument-functions -ffixed-r10 +ARCHINSTRUMENTATIONDEFINES_CORTEXM3 = +else +ARCHINSTRUMENTATIONDEFINES_CORTEXM4F = -ffixed-r10 +ARCHINSTRUMENTATIONDEFINES_CORTEXM4 = -ffixed-r10 +ARCHINSTRUMENTATIONDEFINES_CORTEXM3 = endif # Pick the right set of flags for the architecture. diff --git a/nuttx-configs/px4fmu-v2/nsh/Make.defs b/nuttx-configs/px4fmu-v2/nsh/Make.defs index 99f3b3140..798d58572 100644 --- a/nuttx-configs/px4fmu-v2/nsh/Make.defs +++ b/nuttx-configs/px4fmu-v2/nsh/Make.defs @@ -62,8 +62,14 @@ ARCHCPUFLAGS = -mcpu=cortex-m4 \ # enable precise stack overflow tracking +ENABLE_STACK_CHECKS ?= 0 +ifneq ($(ENABLE_STACK_CHECKS),0) +$(info Stack checks enabled) INSTRUMENTATIONDEFINES = -finstrument-functions \ -ffixed-r10 +else +INSTRUMENTATIONDEFINES = -ffixed-r10 +endif # pull in *just* libm from the toolchain ... this is grody LIBM = "${shell $(CC) $(ARCHCPUFLAGS) -print-file-name=libm.a}" diff --git a/src/modules/uavcan/module.mk b/src/modules/uavcan/module.mk index a3c6e46a0..924b730a2 100644 --- a/src/modules/uavcan/module.mk +++ b/src/modules/uavcan/module.mk @@ -40,9 +40,6 @@ MODULE_COMMAND = uavcan MAXOPTIMIZATION = -O3 -# Instrumentation makes the CPU load about 3 times higher, see https://github.com/PX4/Firmware/issues/1417 -INSTRUMENTATIONDEFINES += -fno-instrument-functions - # Main SRCS += uavcan_main.cpp \ uavcan_clock.cpp \ -- cgit v1.2.3 From 91362223ea5dc5badc34870210c356eb47ebd59d Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Mon, 19 Jan 2015 15:49:18 +0300 Subject: Fixed uninitialized fields of UavcanNode --- src/modules/uavcan/uavcan_main.hpp | 10 +++++----- 1 file changed, 5 insertions(+), 5 deletions(-) (limited to 'src') diff --git a/src/modules/uavcan/uavcan_main.hpp b/src/modules/uavcan/uavcan_main.hpp index 8a0993f15..583c621eb 100644 --- a/src/modules/uavcan/uavcan_main.hpp +++ b/src/modules/uavcan/uavcan_main.hpp @@ -107,11 +107,11 @@ private: int _task = -1; ///< handle to the OS task bool _task_should_exit = false; ///< flag to indicate to tear down the CAN driver int _armed_sub = -1; ///< uORB subscription of the arming status - actuator_armed_s _armed; ///< the arming request of the system + actuator_armed_s _armed = {}; ///< the arming request of the system bool _is_armed = false; ///< the arming status of the actuators on the bus int _test_motor_sub = -1; ///< uORB subscription of the test_motor status - test_motor_s _test_motor; + test_motor_s _test_motor = {}; bool _test_in_progress = false; unsigned _output_count = 0; ///< number of actuators currently available @@ -135,10 +135,10 @@ private: unsigned _poll_fds_num = 0; int _actuator_direct_sub = -1; ///< uORB subscription of the actuator_direct topic - uint8_t _actuator_direct_poll_fd_num; - actuator_direct_s _actuator_direct; + uint8_t _actuator_direct_poll_fd_num = 0; + actuator_direct_s _actuator_direct = {}; - actuator_outputs_s _outputs; + actuator_outputs_s _outputs = {}; // index into _poll_fds for each _control_subs handle uint8_t _poll_ids[NUM_ACTUATOR_CONTROL_GROUPS_UAVCAN]; -- cgit v1.2.3 From 4baf4a032fa87a01863ae475d4ad27496c90db86 Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Mon, 19 Jan 2015 15:54:14 +0300 Subject: Fixed: Passing this->_armed_sub to close, which cannot accept a negative number. --- src/modules/uavcan/uavcan_main.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) (limited to 'src') diff --git a/src/modules/uavcan/uavcan_main.cpp b/src/modules/uavcan/uavcan_main.cpp index af5f2ec96..5b961e2ef 100644 --- a/src/modules/uavcan/uavcan_main.cpp +++ b/src/modules/uavcan/uavcan_main.cpp @@ -513,7 +513,7 @@ UavcanNode::teardown() _control_subs[i] = -1; } } - return ::close(_armed_sub); + return (_armed_sub >= 0) ? ::close(_armed_sub) : 0; } int -- cgit v1.2.3 From f6dc2af3986ba823822525d9865c101d91aa67c3 Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Mon, 19 Jan 2015 16:24:07 +0300 Subject: UAVCAN update --- src/lib/uavcan | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) (limited to 'src') diff --git a/src/lib/uavcan b/src/lib/uavcan index c9227cf6d..8966f9701 160000 --- a/src/lib/uavcan +++ b/src/lib/uavcan @@ -1 +1 @@ -Subproject commit c9227cf6d24e0bafee3ada499dea183446aa35b1 +Subproject commit 8966f970135fb421db31886d6f99ac918594a780 -- cgit v1.2.3 From ae0e2d720926702f5dc1f97aeaa893d30c61fe29 Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Wed, 21 Jan 2015 10:39:14 +0300 Subject: Removing extra UAVCAN perfcounters --- src/modules/uavcan/actuators/esc.cpp | 7 ------- src/modules/uavcan/actuators/esc.hpp | 1 - src/modules/uavcan/uavcan_main.cpp | 6 ------ src/modules/uavcan/uavcan_main.hpp | 1 - 4 files changed, 15 deletions(-) (limited to 'src') diff --git a/src/modules/uavcan/actuators/esc.cpp b/src/modules/uavcan/actuators/esc.cpp index 995c8987c..51589e43e 100644 --- a/src/modules/uavcan/actuators/esc.cpp +++ b/src/modules/uavcan/actuators/esc.cpp @@ -56,17 +56,12 @@ UavcanEscController::UavcanEscController(uavcan::INode &node) : if (_perfcnt_scaling_error == nullptr) { errx(1, "uavcan: couldn't allocate _perfcnt_scaling_error"); } - - if (_perfcnt_broadcast_elapsed == nullptr) { - errx(1, "uavcan: couldn't allocate _perfcnt_broadcast_elapsed"); - } } UavcanEscController::~UavcanEscController() { perf_free(_perfcnt_invalid_input); perf_free(_perfcnt_scaling_error); - perf_free(_perfcnt_broadcast_elapsed); } int UavcanEscController::init() @@ -141,9 +136,7 @@ void UavcanEscController::update_outputs(float *outputs, unsigned num_outputs) * Publish the command message to the bus * Note that for a quadrotor it takes one CAN frame */ - perf_begin(_perfcnt_broadcast_elapsed); (void)_uavcan_pub_raw_cmd.broadcast(msg); - perf_end(_perfcnt_broadcast_elapsed); } void UavcanEscController::arm_all_escs(bool arm) diff --git a/src/modules/uavcan/actuators/esc.hpp b/src/modules/uavcan/actuators/esc.hpp index 498fb9dd8..12c035542 100644 --- a/src/modules/uavcan/actuators/esc.hpp +++ b/src/modules/uavcan/actuators/esc.hpp @@ -109,5 +109,4 @@ private: */ perf_counter_t _perfcnt_invalid_input = perf_alloc(PC_COUNT, "uavcan_esc_invalid_input"); perf_counter_t _perfcnt_scaling_error = perf_alloc(PC_COUNT, "uavcan_esc_scaling_error"); - perf_counter_t _perfcnt_broadcast_elapsed = perf_alloc(PC_ELAPSED, "uavcan_esc_broadcast_elapsed"); }; diff --git a/src/modules/uavcan/uavcan_main.cpp b/src/modules/uavcan/uavcan_main.cpp index 5b961e2ef..4dc03b61b 100644 --- a/src/modules/uavcan/uavcan_main.cpp +++ b/src/modules/uavcan/uavcan_main.cpp @@ -93,10 +93,6 @@ UavcanNode::UavcanNode(uavcan::ICanDriver &can_driver, uavcan::ISystemClock &sys if (_perfcnt_esc_mixer_total_elapsed == nullptr) { errx(1, "uavcan: couldn't allocate _perfcnt_esc_mixer_total_elapsed"); } - - if (_perfcnt_esc_mixer_subscriptions == nullptr) { - errx(1, "uavcan: couldn't allocate _perfcnt_esc_mixer_subscriptions"); - } } UavcanNode::~UavcanNode() @@ -138,7 +134,6 @@ UavcanNode::~UavcanNode() perf_free(_perfcnt_node_spin_elapsed); perf_free(_perfcnt_esc_mixer_output_elapsed); perf_free(_perfcnt_esc_mixer_total_elapsed); - perf_free(_perfcnt_esc_mixer_subscriptions); } int UavcanNode::start(uavcan::NodeID node_id, uint32_t bitrate) @@ -527,7 +522,6 @@ UavcanNode::arm_actuators(bool arm) void UavcanNode::subscribe() { - perf_count(_perfcnt_esc_mixer_subscriptions); // Subscribe/unsubscribe to required actuator control groups uint32_t sub_groups = _groups_required & ~_groups_subscribed; uint32_t unsub_groups = _groups_subscribed & ~_groups_required; diff --git a/src/modules/uavcan/uavcan_main.hpp b/src/modules/uavcan/uavcan_main.hpp index 583c621eb..19b9b4b48 100644 --- a/src/modules/uavcan/uavcan_main.hpp +++ b/src/modules/uavcan/uavcan_main.hpp @@ -146,5 +146,4 @@ private: perf_counter_t _perfcnt_node_spin_elapsed = perf_alloc(PC_ELAPSED, "uavcan_node_spin_elapsed"); perf_counter_t _perfcnt_esc_mixer_output_elapsed = perf_alloc(PC_ELAPSED, "uavcan_esc_mixer_output_elapsed"); perf_counter_t _perfcnt_esc_mixer_total_elapsed = perf_alloc(PC_ELAPSED, "uavcan_esc_mixer_total_elapsed"); - perf_counter_t _perfcnt_esc_mixer_subscriptions = perf_alloc(PC_COUNT, "uavcan_esc_mixer_subscriptions"); }; -- cgit v1.2.3 From 0943bd9a31896996db7a030715eb9d0e8626b0b4 Mon Sep 17 00:00:00 2001 From: David Sidrane Date: Wed, 21 Jan 2015 09:54:49 -1000 Subject: Added Prefix to message to identify it as PX4IO related --- src/drivers/px4io/px4io.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) (limited to 'src') diff --git a/src/drivers/px4io/px4io.cpp b/src/drivers/px4io/px4io.cpp index 1c9a5adc9..8aecbb469 100644 --- a/src/drivers/px4io/px4io.cpp +++ b/src/drivers/px4io/px4io.cpp @@ -2856,7 +2856,7 @@ checkcrc(int argc, char *argv[]) } if (ret != OK) { - printf("check CRC failed - %d\n", ret); + printf("[PX4IO::checkcrc] check CRC failed - %d\n", ret); exit(1); } printf("CRCs match\n"); -- cgit v1.2.3 From e037b7ae9b361812e99c5985a9c543c2ba503de0 Mon Sep 17 00:00:00 2001 From: David Sidrane Date: Wed, 21 Jan 2015 10:02:22 -1000 Subject: Added delay to ensure the the px4io is in loop waiting for a characterit loop. --- src/drivers/px4io/px4io_uploader.cpp | 1 + 1 file changed, 1 insertion(+) (limited to 'src') diff --git a/src/drivers/px4io/px4io_uploader.cpp b/src/drivers/px4io/px4io_uploader.cpp index fb16f891f..02e527695 100644 --- a/src/drivers/px4io/px4io_uploader.cpp +++ b/src/drivers/px4io/px4io_uploader.cpp @@ -621,6 +621,7 @@ int PX4IO_Uploader::reboot() { send(PROTO_REBOOT); + up_udelay(100*1000); // Ensure the farend is in wait for char. send(PROTO_EOC); return OK; -- cgit v1.2.3 From 2a00948c7ad71f849223337d566c41b56f7e1e08 Mon Sep 17 00:00:00 2001 From: David Sidrane Date: Wed, 21 Jan 2015 10:19:24 -1000 Subject: Added prefix to Match Message also --- src/drivers/px4io/px4io.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) (limited to 'src') diff --git a/src/drivers/px4io/px4io.cpp b/src/drivers/px4io/px4io.cpp index 8aecbb469..556eebab6 100644 --- a/src/drivers/px4io/px4io.cpp +++ b/src/drivers/px4io/px4io.cpp @@ -2859,7 +2859,7 @@ checkcrc(int argc, char *argv[]) printf("[PX4IO::checkcrc] check CRC failed - %d\n", ret); exit(1); } - printf("CRCs match\n"); + printf("[PX4IO::checkcrc] CRCs match\n"); exit(0); } -- cgit v1.2.3 From d80a00fad105a2dc98ab6ec415e28fa86f1ceed6 Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Wed, 21 Jan 2015 16:46:04 +0900 Subject: batt_smbus: disable if no batt 10seconds after startup --- src/drivers/batt_smbus/batt_smbus.cpp | 24 ++++++++++++++++++++++-- 1 file changed, 22 insertions(+), 2 deletions(-) (limited to 'src') diff --git a/src/drivers/batt_smbus/batt_smbus.cpp b/src/drivers/batt_smbus/batt_smbus.cpp index 2b5fef4d7..92b752a28 100644 --- a/src/drivers/batt_smbus/batt_smbus.cpp +++ b/src/drivers/batt_smbus/batt_smbus.cpp @@ -91,6 +91,7 @@ #define BATT_SMBUS_MANUFACTURE_INFO 0x25 ///< cell voltage register #define BATT_SMBUS_CURRENT 0x2a ///< current register #define BATT_SMBUS_MEASUREMENT_INTERVAL_MS (1000000 / 10) ///< time in microseconds, measure at 10hz +#define BATT_SMBUS_TIMEOUT_MS 10000000 ///< timeout looking for battery 10seconds after startup #define BATT_SMBUS_PEC_POLYNOMIAL 0x07 ///< Polynomial for calculating PEC @@ -171,11 +172,13 @@ private: uint8_t get_PEC(uint8_t cmd, bool reading, const uint8_t buff[], uint8_t len) const; // internal variables + bool _enabled; ///< true if we have successfully connected to battery work_s _work; ///< work queue for scheduling reads RingBuffer *_reports; ///< buffer of recorded voltages, currents struct battery_status_s _last_report; ///< last published report, used for test() orb_advert_t _batt_topic; ///< uORB battery topic orb_id_t _batt_orb_id; ///< uORB battery topic ID + uint64_t _start_time; ///< system time we first attempt to communicate with battery }; namespace @@ -189,13 +192,18 @@ extern "C" __EXPORT int batt_smbus_main(int argc, char *argv[]); BATT_SMBUS::BATT_SMBUS(int bus, uint16_t batt_smbus_addr) : I2C("batt_smbus", BATT_SMBUS_DEVICE_PATH, bus, batt_smbus_addr, 400000), + _enabled(false), _work{}, _reports(nullptr), _batt_topic(-1), - _batt_orb_id(nullptr) + _batt_orb_id(nullptr), + _start_time(0) { // work_cancel in the dtor will explode if we don't do this... memset(&_work, 0, sizeof(_work)); + + // capture startup time + _start_time = hrt_absolute_time(); } BATT_SMBUS::~BATT_SMBUS() @@ -330,11 +338,20 @@ BATT_SMBUS::cycle_trampoline(void *arg) void BATT_SMBUS::cycle() { + // get current time + uint64_t now = hrt_absolute_time(); + + // exit without rescheduling if we have failed to find a battery after 10 seconds + if (!_enabled && (now - _start_time > BATT_SMBUS_TIMEOUT_MS)) { + warnx("did not find smart battery"); + return; + } + // read data from sensor struct battery_status_s new_report; // set time of reading - new_report.timestamp = hrt_absolute_time(); + new_report.timestamp = now; // read voltage uint16_t tmp; @@ -375,6 +392,9 @@ BATT_SMBUS::cycle() // notify anyone waiting for data poll_notify(POLLIN); + + // record we are working + _enabled = true; } // schedule a fresh cycle call when the measurement is done -- cgit v1.2.3 From 7faef870c8201696372661e063336069d91123b9 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 22 Jan 2015 09:24:13 +0100 Subject: Fix UAVCAN dependency generation issue --- src/lib/uavcan | 2 +- src/modules/uavcan/module.mk | 4 ++-- 2 files changed, 3 insertions(+), 3 deletions(-) (limited to 'src') diff --git a/src/lib/uavcan b/src/lib/uavcan index 8966f9701..7719f7692 160000 --- a/src/lib/uavcan +++ b/src/lib/uavcan @@ -1 +1 @@ -Subproject commit 8966f970135fb421db31886d6f99ac918594a780 +Subproject commit 7719f7692aba67f01b6321773bb7be13f23d2f68 diff --git a/src/modules/uavcan/module.mk b/src/modules/uavcan/module.mk index 924b730a2..600cb47f3 100644 --- a/src/modules/uavcan/module.mk +++ b/src/modules/uavcan/module.mk @@ -57,7 +57,7 @@ SRCS += sensors/sensor_bridge.cpp \ # # libuavcan # -include $(PX4_LIB_DIR)/uavcan/libuavcan/include.mk +include $(PX4_LIB_DIR)uavcan/libuavcan/include.mk SRCS += $(LIBUAVCAN_SRC) INCLUDE_DIRS += $(LIBUAVCAN_INC) # Since actual compiler mode is C++11, the library will default to UAVCAN_CPP11, but it will fail to compile @@ -67,7 +67,7 @@ override EXTRADEFINES := $(EXTRADEFINES) -DUAVCAN_CPP_VERSION=UAVCAN_CPP03 -DUAV # # libuavcan drivers for STM32 # -include $(PX4_LIB_DIR)/uavcan/libuavcan_drivers/stm32/driver/include.mk +include $(PX4_LIB_DIR)uavcan/libuavcan_drivers/stm32/driver/include.mk SRCS += $(LIBUAVCAN_STM32_SRC) INCLUDE_DIRS += $(LIBUAVCAN_STM32_INC) override EXTRADEFINES := $(EXTRADEFINES) -DUAVCAN_STM32_NUTTX -DUAVCAN_STM32_NUM_IFACES=2 -- cgit v1.2.3 From 20a8b26ac8c34ccf906b7775804a59afa1311f19 Mon Sep 17 00:00:00 2001 From: Daniel Agar Date: Tue, 20 Jan 2015 20:40:57 -0500 Subject: Fixed coverity CID #12538 --- src/modules/navigator/geofence.cpp | 32 ++++++++++++++++---------------- 1 file changed, 16 insertions(+), 16 deletions(-) (limited to 'src') diff --git a/src/modules/navigator/geofence.cpp b/src/modules/navigator/geofence.cpp index 4482fb36b..4fa580678 100644 --- a/src/modules/navigator/geofence.cpp +++ b/src/modules/navigator/geofence.cpp @@ -257,6 +257,7 @@ Geofence::loadFromFile(const char *filename) int pointCounter = 0; bool gotVertical = false; const char commentChar = '#'; + int rc = ERROR; /* Make sure no data is left in the datamanager */ clearDm(); @@ -269,10 +270,10 @@ Geofence::loadFromFile(const char *filename) /* create geofence points from valid lines and store in DM */ for (;;) { - /* get a line, bail on error/EOF */ - if (fgets(line, sizeof(line), fp) == NULL) + if (fgets(line, sizeof(line), fp) == NULL) { break; + } /* Trim leading whitespace */ size_t textStart = 0; @@ -299,7 +300,7 @@ Geofence::loadFromFile(const char *filename) if (sscanf(line, "DMS %f %f %f %f %f %f", &lat_d, &lat_m, &lat_s, &lon_d, &lon_m, &lon_s) != 6) { warnx("Scanf to parse DMS geofence vertex failed."); - return ERROR; + goto error; } // warnx("Geofence DMS: %.5f %.5f %.5f ; %.5f %.5f %.5f", (double)lat_d, (double)lat_m, (double)lat_s, (double)lon_d, (double)lon_m, (double)lon_s); @@ -311,43 +312,42 @@ Geofence::loadFromFile(const char *filename) /* Handle decimal degree format */ if (sscanf(line, "%f %f", &(vertex.lat), &(vertex.lon)) != 2) { warnx("Scanf to parse geofence vertex failed."); - return ERROR; + goto error; } } - if (dm_write(DM_KEY_FENCE_POINTS, pointCounter, DM_PERSIST_POWER_ON_RESET, &vertex, sizeof(vertex)) != sizeof(vertex)) - return ERROR; + if (dm_write(DM_KEY_FENCE_POINTS, pointCounter, DM_PERSIST_POWER_ON_RESET, &vertex, sizeof(vertex)) != sizeof(vertex)) { + goto error; + } warnx("Geofence: point: %d, lat %.5f: lon: %.5f", pointCounter, (double)vertex.lat, (double)vertex.lon); pointCounter++; } else { /* Parse the line as the vertical limits */ - if (sscanf(line, "%f %f", &_altitude_min, &_altitude_max) != 2) - return ERROR; - + if (sscanf(line, "%f %f", &_altitude_min, &_altitude_max) != 2) { + goto error; + } warnx("Geofence: alt min: %.4f, alt_max: %.4f", (double)_altitude_min, (double)_altitude_max); gotVertical = true; } - - } - fclose(fp); - /* Check if import was successful */ - if(gotVertical && pointCounter > 0) - { + if (gotVertical && pointCounter > 0) { _verticesCount = pointCounter; warnx("Geofence: imported successfully"); mavlink_log_info(_mavlinkFd, "Geofence imported"); + rc = OK; } else { warnx("Geofence: import error"); mavlink_log_critical(_mavlinkFd, "#audio: Geofence import error"); } - return ERROR; +error: + fclose(fp); + return rc; } int Geofence::clearDm() -- cgit v1.2.3 From ce11cc9f32c1d86e0ebdab3d114517995add2595 Mon Sep 17 00:00:00 2001 From: Daniel Agar Date: Tue, 20 Jan 2015 20:42:01 -0500 Subject: geofence.cpp format --- src/modules/navigator/geofence.cpp | 84 ++++++++++++++++++++++++-------------- 1 file changed, 54 insertions(+), 30 deletions(-) (limited to 'src') diff --git a/src/modules/navigator/geofence.cpp b/src/modules/navigator/geofence.cpp index 4fa580678..9bc9be245 100644 --- a/src/modules/navigator/geofence.cpp +++ b/src/modules/navigator/geofence.cpp @@ -58,17 +58,17 @@ static const int ERROR = -1; Geofence::Geofence() : - SuperBlock(NULL, "GF"), - _fence_pub(-1), - _altitude_min(0), - _altitude_max(0), - _verticesCount(0), - _param_geofence_on(this, "ON"), - _param_altitude_mode(this, "ALTMODE"), - _param_source(this, "SOURCE"), - _param_counter_threshold(this, "COUNT"), - _outside_counter(0), - _mavlinkFd(-1) + SuperBlock(NULL, "GF"), + _fence_pub(-1), + _altitude_min(0), + _altitude_max(0), + _verticesCount(0), + _param_geofence_on(this, "ON"), + _param_altitude_mode(this, "ALTMODE"), + _param_source(this, "SOURCE"), + _param_counter_threshold(this, "COUNT"), + _outside_counter(0), + _mavlinkFd(-1) { /* Load initial params */ updateParams(); @@ -92,22 +92,26 @@ bool Geofence::inside(const struct vehicle_global_position_s &global_position, f bool Geofence::inside(const struct vehicle_global_position_s &global_position, - const struct vehicle_gps_position_s &gps_position,float baro_altitude_amsl) { + const struct vehicle_gps_position_s &gps_position, float baro_altitude_amsl) +{ updateParams(); if (getAltitudeMode() == Geofence::GF_ALT_MODE_WGS84) { if (getSource() == Geofence::GF_SOURCE_GLOBALPOS) { return inside(global_position); + } else { return inside((double)gps_position.lat * 1.0e-7, (double)gps_position.lon * 1.0e-7, - (double)gps_position.alt * 1.0e-3); + (double)gps_position.alt * 1.0e-3); } + } else { if (getSource() == Geofence::GF_SOURCE_GLOBALPOS) { return inside(global_position, baro_altitude_amsl); + } else { return inside((double)gps_position.lat * 1.0e-7, (double)gps_position.lon * 1.0e-7, - baro_altitude_amsl); + baro_altitude_amsl); } } } @@ -120,9 +124,12 @@ bool Geofence::inside(double lat, double lon, float altitude) _outside_counter = 0; return inside_fence; } { + _outside_counter++; - if(_outside_counter > _param_counter_threshold.get()) { + + if (_outside_counter > _param_counter_threshold.get()) { return inside_fence; + } else { return true; } @@ -133,8 +140,9 @@ bool Geofence::inside(double lat, double lon, float altitude) bool Geofence::inside_polygon(double lat, double lon, float altitude) { /* Return true if geofence is disabled */ - if (_param_geofence_on.get() != 1) + if (_param_geofence_on.get() != 1) { return true; + } if (valid()) { @@ -159,20 +167,22 @@ bool Geofence::inside_polygon(double lat, double lon, float altitude) if (dm_read(DM_KEY_FENCE_POINTS, i, &temp_vertex_i, sizeof(struct fence_vertex_s)) != sizeof(struct fence_vertex_s)) { break; } + if (dm_read(DM_KEY_FENCE_POINTS, j, &temp_vertex_j, sizeof(struct fence_vertex_s)) != sizeof(struct fence_vertex_s)) { break; } // skip vertex 0 (return point) if (((double)temp_vertex_i.lon >= lon) != ((double)temp_vertex_j.lon >= lon) && - (lat <= (double)(temp_vertex_j.lat - temp_vertex_i.lat) * (lon - (double)temp_vertex_i.lon) / - (double)(temp_vertex_j.lon - temp_vertex_i.lon) + (double)temp_vertex_i.lat)) { - c = !c; + (lat <= (double)(temp_vertex_j.lat - temp_vertex_i.lat) * (lon - (double)temp_vertex_i.lon) / + (double)(temp_vertex_j.lon - temp_vertex_i.lon) + (double)temp_vertex_i.lat)) { + c = !c; } } return c; + } else { /* Empty fence --> accept all points */ return true; @@ -188,8 +198,9 @@ bool Geofence::valid() { // NULL fence is valid - if (isEmpty()) + if (isEmpty()) { return true; + } // Otherwise if ((_verticesCount < 4) || (_verticesCount > GEOFENCE_MAX_VERTICES)) { @@ -214,26 +225,33 @@ Geofence::addPoint(int argc, char *argv[]) return; } - if (argc < 3) + if (argc < 3) { errx(1, "Specify: -clear | sequence latitude longitude [-publish]"); + } ix = atoi(argv[0]); - if (ix >= DM_KEY_FENCE_POINTS_MAX) + + if (ix >= DM_KEY_FENCE_POINTS_MAX) { errx(1, "Sequence must be less than %d", DM_KEY_FENCE_POINTS_MAX); + } lat = strtod(argv[1], &end); lon = strtod(argv[2], &end); last = 0; - if ((argc > 3) && (strcmp(argv[3], "-publish") == 0)) + + if ((argc > 3) && (strcmp(argv[3], "-publish") == 0)) { last = 1; + } vertex.lat = (float)lat; vertex.lon = (float)lon; if (dm_write(DM_KEY_FENCE_POINTS, ix, DM_PERSIST_POWER_ON_RESET, &vertex, sizeof(vertex)) == sizeof(vertex)) { - if (last) + if (last) { publishFence((unsigned)ix + 1); + } + return; } @@ -243,10 +261,12 @@ Geofence::addPoint(int argc, char *argv[]) void Geofence::publishFence(unsigned vertices) { - if (_fence_pub == -1) + if (_fence_pub == -1) { _fence_pub = orb_advertise(ORB_ID(fence), &vertices); - else + + } else { orb_publish(ORB_ID(fence), _fence_pub, &vertices); + } } int @@ -264,6 +284,7 @@ Geofence::loadFromFile(const char *filename) /* open the mixer definition file */ fp = fopen(GEOFENCE_FILENAME, "r"); + if (fp == NULL) { return ERROR; } @@ -277,7 +298,8 @@ Geofence::loadFromFile(const char *filename) /* Trim leading whitespace */ size_t textStart = 0; - while((textStart < sizeof(line)/sizeof(char)) && isspace(line[textStart])) textStart++; + + while ((textStart < sizeof(line) / sizeof(char)) && isspace(line[textStart])) { textStart++; } /* if the line starts with #, skip */ if (line[textStart] == commentChar) { @@ -305,8 +327,8 @@ Geofence::loadFromFile(const char *filename) // warnx("Geofence DMS: %.5f %.5f %.5f ; %.5f %.5f %.5f", (double)lat_d, (double)lat_m, (double)lat_s, (double)lon_d, (double)lon_m, (double)lon_s); - vertex.lat = lat_d + lat_m/60.0f + lat_s/3600.0f; - vertex.lon = lon_d + lon_m/60.0f + lon_s/3600.0f; + vertex.lat = lat_d + lat_m / 60.0f + lat_s / 3600.0f; + vertex.lon = lon_d + lon_m / 60.0f + lon_s / 3600.0f; } else { /* Handle decimal degree format */ @@ -320,9 +342,10 @@ Geofence::loadFromFile(const char *filename) goto error; } - warnx("Geofence: point: %d, lat %.5f: lon: %.5f", pointCounter, (double)vertex.lat, (double)vertex.lon); + warnx("Geofence: point: %d, lat %.5f: lon: %.5f", pointCounter, (double)vertex.lat, (double)vertex.lon); pointCounter++; + } else { /* Parse the line as the vertical limits */ if (sscanf(line, "%f %f", &_altitude_min, &_altitude_max) != 2) { @@ -340,6 +363,7 @@ Geofence::loadFromFile(const char *filename) warnx("Geofence: imported successfully"); mavlink_log_info(_mavlinkFd, "Geofence imported"); rc = OK; + } else { warnx("Geofence: import error"); mavlink_log_critical(_mavlinkFd, "#audio: Geofence import error"); -- cgit v1.2.3 From 9f32a85409034b2601e42959706a6e0c1a144ce0 Mon Sep 17 00:00:00 2001 From: Daniel Agar Date: Tue, 20 Jan 2015 20:46:56 -0500 Subject: Fixed coverity CID #12530 --- src/drivers/hmc5883/hmc5883.cpp | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) (limited to 'src') diff --git a/src/drivers/hmc5883/hmc5883.cpp b/src/drivers/hmc5883/hmc5883.cpp index 95fbed0ba..a06be72d9 100644 --- a/src/drivers/hmc5883/hmc5883.cpp +++ b/src/drivers/hmc5883/hmc5883.cpp @@ -1351,13 +1351,15 @@ start_bus(struct hmc5883_bus_option &bus, enum Rotation rotation) } int fd = open(bus.devpath, O_RDONLY); - if (fd < 0) + if (fd < 0) { return false; + } if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0) { close(fd); errx(1,"Failed to setup poll rate"); } + close(fd); return true; } -- cgit v1.2.3 From e10d6bf603e8728061465271957486b727387d1f Mon Sep 17 00:00:00 2001 From: Daniel Agar Date: Tue, 20 Jan 2015 20:56:18 -0500 Subject: Fixed coverity CID #12521 --- src/modules/mavlink/mavlink_receiver.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) (limited to 'src') diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index dfbf00b66..e5a21651d 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -435,7 +435,7 @@ MavlinkReceiver::handle_message_hil_optical_flow(mavlink_message_t *msg) /* Use distance value for range finder report */ struct range_finder_report r; - memset(&r, 0, sizeof(f)); + memset(&r, 0, sizeof(r)); r.timestamp = hrt_absolute_time(); r.error_count = 0; -- cgit v1.2.3 From 193a210b517c958ebce3aaae8cc9b5709ff9b52b Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Fri, 23 Jan 2015 14:32:29 +0100 Subject: Multi sonar support by jverbeke --- src/drivers/drv_range_finder.h | 3 + src/drivers/mb12xx/mb12xx.cpp | 189 ++++++++++++++++++++++++++++++++--------- 2 files changed, 152 insertions(+), 40 deletions(-) (limited to 'src') diff --git a/src/drivers/drv_range_finder.h b/src/drivers/drv_range_finder.h index 0f8362f58..12d51aeaa 100644 --- a/src/drivers/drv_range_finder.h +++ b/src/drivers/drv_range_finder.h @@ -45,6 +45,7 @@ #include "drv_orb_dev.h" #define RANGE_FINDER_DEVICE_PATH "/dev/range_finder" +#define MB12XX_MAX_RANGEFINDERS 12 //Maximum number of RangeFinders that can be connected enum RANGE_FINDER_TYPE { RANGE_FINDER_TYPE_LASER = 0, @@ -67,6 +68,8 @@ struct range_finder_report { 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 */ }; /** diff --git a/src/drivers/mb12xx/mb12xx.cpp b/src/drivers/mb12xx/mb12xx.cpp index 9cf568658..90f1791bd 100644 --- a/src/drivers/mb12xx/mb12xx.cpp +++ b/src/drivers/mb12xx/mb12xx.cpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2013 PX4 Development Team. All rights reserved. + * 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 @@ -54,6 +54,7 @@ #include #include #include +#include #include #include @@ -72,7 +73,7 @@ #include /* Configuration Constants */ -#define MB12XX_BUS PX4_I2C_BUS_EXPANSION +#define MB12XX_BUS PX4_I2C_BUS_EXPANSION #define MB12XX_BASEADDR 0x70 /* 7-bit address. 8-bit address is 0xE0 */ #define MB12XX_DEVICE_PATH "/dev/mb12xx" @@ -83,10 +84,12 @@ #define MB12XX_SET_ADDRESS_2 0xA5 /* Change address 2 Register */ /* Device limits */ -#define MB12XX_MIN_DISTANCE (0.20f) -#define MB12XX_MAX_DISTANCE (7.65f) +#define MB12XX_MIN_DISTANCE (0.20f) +#define MB12XX_MAX_DISTANCE (7.65f) + +#define MB12XX_CONVERSION_INTERVAL 100000 /* 60ms for one sonar */ +#define TICKS_BETWEEN_SUCCESIVE_FIRES 100000 /* 30ms between each sonar measurement (watch out for interference!) */ -#define MB12XX_CONVERSION_INTERVAL 60000 /* 60ms */ /* oddly, ERROR is not defined for c++ */ #ifdef ERROR @@ -133,6 +136,14 @@ private: perf_counter_t _comms_errors; perf_counter_t _buffer_overflows; + uint8_t _cycle_counter; /* counter in cycle to change i2c adresses */ + int _cycling_rate; /* */ + uint8_t _index_counter; /* temporary sonar i2c address */ + std::vector addr_ind; /* temp sonar i2c address vector */ + std::vector + _latest_sonar_measurements; /* vector to store latest sonar measurements in before writing to report */ + + /** * Test whether the device supported by the driver is present at a * specific address. @@ -140,7 +151,7 @@ private: * @param address The I2C bus address to probe. * @return True if the device is present. */ - int probe_address(uint8_t address); + int probe_address(uint8_t address); /** * Initialise the automatic measurement state machine and start it. @@ -170,15 +181,15 @@ private: * and start a new one. */ void cycle(); - int measure(); - int collect(); + int measure(); + int collect(); /** * Static trampoline from the workq context; because we don't have a * generic workq wrapper yet. * * @param arg Instance pointer for the driver that is polling. */ - static void cycle_trampoline(void *arg); + static void cycle_trampoline(void *arg); }; @@ -200,12 +211,16 @@ MB12XX::MB12XX(int bus, int address) : _range_finder_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")) + _buffer_overflows(perf_alloc(PC_COUNT, "mb12xx_buffer_overflows")), + _cycle_counter(0), /* initialising counter for cycling function to zero */ + _cycling_rate(0), /* initialising cycling rate (which can differ depending on one sonar or multiple) */ + _index_counter(0) /* initialising temp sonar i2c address to zero */ + { - // enable debug() calls + /* enable debug() calls */ _debug_enabled = false; - // work_cancel in the dtor will explode if we don't do this... + /* work_cancel in the dtor will explode if we don't do this... */ memset(&_work, 0, sizeof(_work)); } @@ -223,7 +238,7 @@ MB12XX::~MB12XX() unregister_class_devname(RANGE_FINDER_DEVICE_PATH, _class_instance); } - // free perf counters + /* free perf counters */ perf_free(_sample_perf); perf_free(_comms_errors); perf_free(_buffer_overflows); @@ -242,6 +257,9 @@ MB12XX::init() /* allocate basic report buffers */ _reports = new RingBuffer(2, sizeof(range_finder_report)); + _index_counter = MB12XX_BASEADDR; /* set temp sonar i2c address to base adress */ + set_address(_index_counter); /* set I2c port to temp sonar i2c adress */ + if (_reports == nullptr) { goto out; } @@ -250,16 +268,53 @@ MB12XX::init() if (_class_instance == CLASS_DEVICE_PRIMARY) { /* get a publish handle on the range finder topic */ - struct range_finder_report rf_report; - measure(); - _reports->get(&rf_report); + struct range_finder_report rf_report = {}; + _range_finder_topic = orb_advertise(ORB_ID(sensor_range_finder), &rf_report); if (_range_finder_topic < 0) { - debug("failed to create sensor_range_finder object. Did you start uOrb?"); + log("failed advert - uORB started?"); } } + /* + * check for connected rangefinders on each i2c port: + * We start from i2c base address (0x70 = 112) and count downwards. + * So second iteration it uses i2c address 111, third iteration 110 and so on + */ + for (int counter = 0; counter <= MB12XX_MAX_RANGEFINDERS; counter++) { + /* set temp sonar i2c address to base adress - counter */ + _index_counter = MB12XX_BASEADDR - counter; + /* set I2c port to temp sonar i2c adress */ + set_address(_index_counter); + int ret2 = measure(); + + if (ret2 == 0) { /* sonar is present -> store address_index in array */ + addr_ind.push_back(_index_counter); + debug("sonar added"); + _latest_sonar_measurements.push_back(200); + } + } + + _index_counter = MB12XX_BASEADDR; + /* set i2c port back to base adress for rest of driver */ + set_address(_index_counter); + + /* if only one sonar detected, no special timing is required between firing, so use default */ + if (addr_ind.size() == 1) { + _cycling_rate = MB12XX_CONVERSION_INTERVAL; + + } else { + _cycling_rate = TICKS_BETWEEN_SUCCESIVE_FIRES; + } + + /* show the connected sonars in terminal */ + for (int i = 0; i < addr_ind.size(); i++) { + log("sonar %d with address %d added", (i + 1), addr_ind[i]); + } + + debug("Number of sonars connected: %d", addr_ind.size()); + ret = OK; /* sensor is ok, but we don't really know if it is within range */ _sensor_ok = true; @@ -325,11 +380,12 @@ MB12XX::ioctl(struct file *filp, int cmd, unsigned long arg) bool want_start = (_measure_ticks == 0); /* set interval for next measurement to minimum legal value */ - _measure_ticks = USEC2TICK(MB12XX_CONVERSION_INTERVAL); + _measure_ticks = USEC2TICK(_cycling_rate); /* if we need to start the poll state machine, do it */ if (want_start) { start(); + } return OK; @@ -344,7 +400,7 @@ MB12XX::ioctl(struct file *filp, int cmd, unsigned long arg) unsigned ticks = USEC2TICK(1000000 / arg); /* check against maximum rate */ - if (ticks < USEC2TICK(MB12XX_CONVERSION_INTERVAL)) { + if (ticks < USEC2TICK(_cycling_rate)) { return -EINVAL; } @@ -414,6 +470,7 @@ MB12XX::ioctl(struct file *filp, int cmd, unsigned long arg) 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); int ret = 0; @@ -453,7 +510,7 @@ MB12XX::read(struct file *filp, char *buffer, size_t buflen) } /* wait for it to complete */ - usleep(MB12XX_CONVERSION_INTERVAL); + usleep(_cycling_rate * 2); /* run the collection phase */ if (OK != collect()) { @@ -474,17 +531,19 @@ MB12XX::read(struct file *filp, char *buffer, size_t buflen) int MB12XX::measure() { + int ret; /* * Send the command to begin a measurement. */ + uint8_t cmd = MB12XX_TAKE_RANGE_REG; ret = transfer(&cmd, 1, nullptr, 0); if (OK != ret) { perf_count(_comms_errors); - log("i2c::transfer returned %d", ret); + debug("i2c::transfer returned %d", ret); return ret; } @@ -506,7 +565,7 @@ MB12XX::collect() ret = transfer(nullptr, 0, &val[0], 2); if (ret < 0) { - log("error reading from sensor: %d", ret); + debug("error reading from sensor: %d", ret); perf_count(_comms_errors); perf_end(_sample_perf); return ret; @@ -519,10 +578,35 @@ MB12XX::collect() /* 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; + + /* assign the first measurement to the plain distance field */ + report.distance = _latest_sonar_measurements[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.valid = ((_latest_sonar_measurements[0] > get_minimum_distance()) + && (_latest_sonar_measurements[0] < get_maximum_distance())) ? 1 : 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++) { + if (i < addr_ind.size()) { + /* set data of connected sensor */ + report.distance_vector[i] = _latest_sonar_measurements[i]; + + } else { + /* set unconnected slots to zero */ + report.distance_vector[i] = 0; + } + } + + /* a just_updated variable is added to indicate to higher level software which sonar has most recently been + * collected as this could be of use for Kalman filters + */ + report.just_updated = _index_counter; /* publish it, if we are the primary */ if (_range_finder_topic >= 0) { @@ -545,12 +629,13 @@ MB12XX::collect() void MB12XX::start() { + /* reset the report ring and state machine */ _collect_phase = false; _reports->flush(); /* schedule a cycle to start things */ - work_queue(HPWORK, &_work, (worker_t)&MB12XX::cycle_trampoline, this, 1); + work_queue(HPWORK, &_work, (worker_t)&MB12XX::cycle_trampoline, this, 5); /* notify about state change */ struct subsystem_info_s info = { @@ -564,8 +649,10 @@ MB12XX::start() if (pub > 0) { orb_publish(ORB_ID(subsystem_info), pub, &info); + } else { pub = orb_advertise(ORB_ID(subsystem_info), &info); + } } @@ -578,21 +665,25 @@ MB12XX::stop() void MB12XX::cycle_trampoline(void *arg) { + MB12XX *dev = (MB12XX *)arg; dev->cycle(); + } void MB12XX::cycle() { - /* collection phase? */ if (_collect_phase) { + /*sonar from previous iteration collect is now read out */ + _index_counter = addr_ind[_cycle_counter]; + set_address(_index_counter); /* perform collection */ if (OK != collect()) { - log("collection error"); - /* restart the measurement state machine */ + debug("collection error"); + /* if error restart the measurement state machine */ start(); return; } @@ -600,25 +691,37 @@ MB12XX::cycle() /* next phase is measurement */ _collect_phase = false; - /* - * Is there a collect->measure gap? - */ - if (_measure_ticks > USEC2TICK(MB12XX_CONVERSION_INTERVAL)) { + /* change i2c adress to next sonar */ + _cycle_counter = _cycle_counter + 1; + + if (_cycle_counter >= addr_ind.size()) { + _cycle_counter = 0; + } + + /* Is there a collect->measure gap? Yes, and the timing is set equal to the cycling_rate + Otherwise the next sonar would fire without the first one having received its reflected sonar pulse */ + + if (_measure_ticks > USEC2TICK(_cycling_rate)) { /* schedule a fresh cycle call when we are ready to measure again */ work_queue(HPWORK, &_work, (worker_t)&MB12XX::cycle_trampoline, this, - _measure_ticks - USEC2TICK(MB12XX_CONVERSION_INTERVAL)); - + _measure_ticks - USEC2TICK(_cycling_rate)); return; } } - /* measurement phase */ + /* Measurement (firing) phase */ + + /* ensure sonar i2c adress is still correct */ + _index_counter = addr_ind[_cycle_counter]; + set_address(_index_counter); + + /* Perform measurement */ if (OK != measure()) { - log("measure error"); + debug("ERR ADDR: %d", _index_counter); } /* next phase is collection */ @@ -629,7 +732,8 @@ MB12XX::cycle() &_work, (worker_t)&MB12XX::cycle_trampoline, this, - USEC2TICK(MB12XX_CONVERSION_INTERVAL)); + USEC2TICK(_cycling_rate)); + } void @@ -750,7 +854,7 @@ test() } warnx("single read"); - warnx("measurement: %0.2f m", (double)report.distance); + warnx("measurement: %0.2f of sonar %d", (double)report.distance_vector[report.just_updated], report.just_updated); warnx("time: %lld", report.timestamp); /* start the sensor polling at 2Hz */ @@ -779,7 +883,12 @@ test() } warnx("periodic read %u", i); - warnx("measurement: %0.3f", (double)report.distance); + + /* 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); } @@ -830,7 +939,7 @@ info() exit(0); } -} // namespace +} /* namespace */ int mb12xx_main(int argc, char *argv[]) -- cgit v1.2.3 From a23c39eec414ffb9d6808e3402d21f322e14930f Mon Sep 17 00:00:00 2001 From: Jon Verbeke Date: Tue, 20 Jan 2015 16:38:59 +0100 Subject: Sonar driver by jverbeke --- src/drivers/mb12xx/mb12xx.cpp | 95 +++++++++++++++++++++---------------------- 1 file changed, 47 insertions(+), 48 deletions(-) (limited to 'src') diff --git a/src/drivers/mb12xx/mb12xx.cpp b/src/drivers/mb12xx/mb12xx.cpp index 90f1791bd..0ef81e431 100644 --- a/src/drivers/mb12xx/mb12xx.cpp +++ b/src/drivers/mb12xx/mb12xx.cpp @@ -34,6 +34,7 @@ /** * @file mb12xx.cpp * @author Greg Hulands + * @author Jon Verbeke * * Driver for the Maxbotix sonar range finders connected via I2C. */ @@ -136,12 +137,11 @@ private: perf_counter_t _comms_errors; perf_counter_t _buffer_overflows; - uint8_t _cycle_counter; /* counter in cycle to change i2c adresses */ - int _cycling_rate; /* */ - uint8_t _index_counter; /* temporary sonar i2c address */ + uint8_t _cycle_counter; /* counter in cycle to change i2c adresses */ + int _cycling_rate; /* */ + uint8_t _index_counter; /* temporary sonar i2c address */ std::vector addr_ind; /* temp sonar i2c address vector */ - std::vector - _latest_sonar_measurements; /* vector to store latest sonar measurements in before writing to report */ + std::vector _latest_sonar_measurements; /* vector to store latest sonar measurements in before writing to report */ /** @@ -149,9 +149,9 @@ private: * specific address. * * @param address The I2C bus address to probe. - * @return True if the device is present. + * @return True if the device is present. */ - int probe_address(uint8_t address); + int probe_address(uint8_t address); /** * Initialise the automatic measurement state machine and start it. @@ -181,8 +181,8 @@ private: * and start a new one. */ void cycle(); - int measure(); - int collect(); + int measure(); + int collect(); /** * Static trampoline from the workq context; because we don't have a * generic workq wrapper yet. @@ -273,20 +273,16 @@ MB12XX::init() _range_finder_topic = orb_advertise(ORB_ID(sensor_range_finder), &rf_report); if (_range_finder_topic < 0) { - log("failed advert - uORB started?"); + log("failed to create sensor_range_finder object. Did you start uOrb?"); } } - /* - * check for connected rangefinders on each i2c port: - * We start from i2c base address (0x70 = 112) and count downwards. - * So second iteration it uses i2c address 111, third iteration 110 and so on - */ - for (int counter = 0; counter <= MB12XX_MAX_RANGEFINDERS; counter++) { - /* set temp sonar i2c address to base adress - counter */ - _index_counter = MB12XX_BASEADDR - counter; - /* set I2c port to temp sonar i2c adress */ - set_address(_index_counter); + /* check for connected rangefinders on each i2c port: + We start from i2c base address (0x70 = 112) and count downwards + So second iteration it uses i2c address 111, third iteration 110 and so on*/ + for (unsigned counter = 0; counter <= MB12XX_MAX_RANGEFINDERS; counter++) { + _index_counter = MB12XX_BASEADDR - counter; /* set temp sonar i2c address to base adress - counter */ + set_address(_index_counter); /* set I2c port to temp sonar i2c adress */ int ret2 = measure(); if (ret2 == 0) { /* sonar is present -> store address_index in array */ @@ -297,8 +293,7 @@ MB12XX::init() } _index_counter = MB12XX_BASEADDR; - /* set i2c port back to base adress for rest of driver */ - set_address(_index_counter); + set_address(_index_counter); /* set i2c port back to base adress for rest of driver */ /* if only one sonar detected, no special timing is required between firing, so use default */ if (addr_ind.size() == 1) { @@ -309,7 +304,7 @@ MB12XX::init() } /* show the connected sonars in terminal */ - for (int i = 0; i < addr_ind.size(); i++) { + for (unsigned i = 0; i < addr_ind.size(); i++) { log("sonar %d with address %d added", (i + 1), addr_ind[i]); } @@ -397,7 +392,7 @@ MB12XX::ioctl(struct file *filp, int cmd, unsigned long arg) bool want_start = (_measure_ticks == 0); /* convert hz to tick interval via microseconds */ - unsigned ticks = USEC2TICK(1000000 / arg); + int ticks = USEC2TICK(1000000 / arg); /* check against maximum rate */ if (ticks < USEC2TICK(_cycling_rate)) { @@ -579,34 +574,39 @@ MB12XX::collect() report.timestamp = hrt_absolute_time(); report.error_count = perf_event_count(_comms_errors); - /* assign the first measurement to the plain distance field */ - report.distance = _latest_sonar_measurements[0]; - report.minimum_distance = get_minimum_distance(); - report.maximum_distance = get_maximum_distance(); - report.valid = ((_latest_sonar_measurements[0] > get_minimum_distance()) - && (_latest_sonar_measurements[0] < get_maximum_distance())) ? 1 : 0; + /* 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; - /* 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 < (MB12XX_MAX_RANGEFINDERS); i++) { + report.distance_vector[i] = 0; + } + + report.just_updated = 0; - for (unsigned i = 0; i < (_latest_sonar_measurements.size()); i++) { - if (i < addr_ind.size()) { - /* set data of connected sensor */ + } 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]; + } - } else { - /* set unconnected slots to zero */ - report.distance_vector[i] = 0; + /* 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; } } - /* a just_updated variable is added to indicate to higher level software which sonar has most recently been - * collected as this could be of use for Kalman filters - */ - report.just_updated = _index_counter; + report.valid = si_units > get_minimum_distance() && si_units < get_maximum_distance() ? 1 : 0; /* publish it, if we are the primary */ if (_range_finder_topic >= 0) { @@ -676,8 +676,7 @@ void MB12XX::cycle() { if (_collect_phase) { - /*sonar from previous iteration collect is now read out */ - _index_counter = addr_ind[_cycle_counter]; + _index_counter = addr_ind[_cycle_counter]; /*sonar from previous iteration collect is now read out */ set_address(_index_counter); /* perform collection */ @@ -721,7 +720,7 @@ MB12XX::cycle() /* Perform measurement */ if (OK != measure()) { - debug("ERR ADDR: %d", _index_counter); + debug("measure error sonar adress %d", _index_counter); } /* next phase is collection */ -- cgit v1.2.3 From b14e849fc4bcf81bbffd47064e2850a929652f6d Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Fri, 23 Jan 2015 16:33:24 +0100 Subject: Sonar: Set min/max distance properly, add usleep for detection --- src/drivers/mb12xx/mb12xx.cpp | 5 +++++ 1 file changed, 5 insertions(+) (limited to 'src') diff --git a/src/drivers/mb12xx/mb12xx.cpp b/src/drivers/mb12xx/mb12xx.cpp index 0ef81e431..a4fb7bcc2 100644 --- a/src/drivers/mb12xx/mb12xx.cpp +++ b/src/drivers/mb12xx/mb12xx.cpp @@ -277,6 +277,9 @@ MB12XX::init() } } + // XXX we should find out why we need to wait 200 ms here + usleep(200000); + /* check for connected rangefinders on each i2c port: We start from i2c base address (0x70 = 112) and count downwards So second iteration it uses i2c address 111, third iteration 110 and so on*/ @@ -606,6 +609,8 @@ MB12XX::collect() } } + 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; /* publish it, if we are the primary */ -- cgit v1.2.3 From cbf4a5af1611ee0969ea0b8d265441308e2dbc91 Mon Sep 17 00:00:00 2001 From: James Goppert Date: Fri, 23 Jan 2015 22:25:48 -0500 Subject: Simplified i2c handling for flow. --- src/drivers/px4flow/i2c_frame.h | 82 ++++++++++++++++++++++++++++++++++++++ src/drivers/px4flow/px4flow.cpp | 88 ++++++----------------------------------- 2 files changed, 95 insertions(+), 75 deletions(-) create mode 100644 src/drivers/px4flow/i2c_frame.h (limited to 'src') diff --git a/src/drivers/px4flow/i2c_frame.h b/src/drivers/px4flow/i2c_frame.h new file mode 100644 index 000000000..b391b1f6a --- /dev/null +++ b/src/drivers/px4flow/i2c_frame.h @@ -0,0 +1,82 @@ +/**************************************************************************** + * + * 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. + * + ****************************************************************************/ + +/** + * @file i2c_frame.h + * Definition of i2c frames. + * @author Thomas Boehm + * @author James Goppert + */ + +#ifndef I2C_FRAME_H_ +#define I2C_FRAME_H_ +#include + + +typedef struct i2c_frame +{ + uint16_t frame_count; + int16_t pixel_flow_x_sum; + int16_t pixel_flow_y_sum; + int16_t flow_comp_m_x; + int16_t flow_comp_m_y; + int16_t qual; + int16_t gyro_x_rate; + int16_t gyro_y_rate; + int16_t gyro_z_rate; + uint8_t gyro_range; + uint8_t sonar_timestamp; + int16_t ground_distance; +} i2c_frame; + +#define I2C_FRAME_SIZE (sizeof(i2c_frame)) + + +typedef struct i2c_integral_frame +{ + uint16_t frame_count_since_last_readout; + int16_t pixel_flow_x_integral; + int16_t pixel_flow_y_integral; + int16_t gyro_x_rate_integral; + int16_t gyro_y_rate_integral; + int16_t gyro_z_rate_integral; + uint32_t integration_timespan; + uint32_t sonar_timestamp; + uint16_t ground_distance; + int16_t gyro_temperature; + uint8_t qual; +} i2c_integral_frame; + +#define I2C_INTEGRAL_FRAME_SIZE (sizeof(i2c_integral_frame)) + +#endif /* I2C_FRAME_H_ */ diff --git a/src/drivers/px4flow/px4flow.cpp b/src/drivers/px4flow/px4flow.cpp index 9c9c1b0f8..bb0cdbbb6 100644 --- a/src/drivers/px4flow/px4flow.cpp +++ b/src/drivers/px4flow/px4flow.cpp @@ -93,38 +93,11 @@ static const int ERROR = -1; # error This requires CONFIG_SCHED_WORKQUEUE. #endif -struct i2c_frame { - uint16_t frame_count; - int16_t pixel_flow_x_sum; - int16_t pixel_flow_y_sum; - int16_t flow_comp_m_x; - int16_t flow_comp_m_y; - int16_t qual; - int16_t gyro_x_rate; - int16_t gyro_y_rate; - int16_t gyro_z_rate; - uint8_t gyro_range; - uint8_t sonar_timestamp; - int16_t ground_distance; -}; -struct i2c_frame f; +#include "i2c_frame.h" -struct i2c_integral_frame { - uint16_t frame_count_since_last_readout; - int16_t pixel_flow_x_integral; - int16_t pixel_flow_y_integral; - int16_t gyro_x_rate_integral; - int16_t gyro_y_rate_integral; - int16_t gyro_z_rate_integral; - uint32_t integration_timespan; - uint32_t time_since_last_sonar_update; - uint16_t ground_distance; - int16_t gyro_temperature; - uint8_t qual; -} __attribute__((packed)); +struct i2c_frame f; struct i2c_integral_frame f_integral; - class PX4FLOW: public device::I2C { public: @@ -150,8 +123,7 @@ private: RingBuffer *_reports; bool _sensor_ok; int _measure_ticks; - bool _collect_phase; - + bool _collect_phase; orb_advert_t _px4flow_topic; perf_counter_t _sample_perf; @@ -261,10 +233,10 @@ out: int PX4FLOW::probe() { - uint8_t val[22]; + uint8_t val[I2C_FRAME_SIZE]; // to be sure this is not a ll40ls Lidar (which can also be on - // 0x42) we check if a 22 byte transfer works from address + // 0x42) we check if a I2C_FRAME_SIZE byte transfer works from address // 0. The ll40ls gives an error for that, whereas the flow // happily returns some data if (transfer(nullptr, 0, &val[0], 22) != OK) { @@ -469,16 +441,16 @@ PX4FLOW::collect() int ret = -EIO; /* read from the sensor */ - uint8_t val[47] = { 0 }; + uint8_t val[I2C_FRAME_SIZE + I2C_INTEGRAL_FRAME_SIZE] = { 0 }; perf_begin(_sample_perf); if (PX4FLOW_REG == 0x00) { - ret = transfer(nullptr, 0, &val[0], 47); // read 47 bytes (22+25 : frame1 + frame2) + ret = transfer(nullptr, 0, &val[0], I2C_FRAME_SIZE + I2C_INTEGRAL_FRAME_SIZE); } if (PX4FLOW_REG == 0x16) { - ret = transfer(nullptr, 0, &val[0], 25); // read 25 bytes (only frame2) + ret = transfer(nullptr, 0, &val[0], I2C_INTEGRAL_FRAME_SIZE); } if (ret < 0) { @@ -489,46 +461,12 @@ PX4FLOW::collect() } if (PX4FLOW_REG == 0) { - f.frame_count = val[1] << 8 | val[0]; - f.pixel_flow_x_sum = val[3] << 8 | val[2]; - f.pixel_flow_y_sum = val[5] << 8 | val[4]; - f.flow_comp_m_x = val[7] << 8 | val[6]; - f.flow_comp_m_y = val[9] << 8 | val[8]; - f.qual = val[11] << 8 | val[10]; - f.gyro_x_rate = val[13] << 8 | val[12]; - f.gyro_y_rate = val[15] << 8 | val[14]; - f.gyro_z_rate = val[17] << 8 | val[16]; - f.gyro_range = val[18]; - f.sonar_timestamp = val[19]; - f.ground_distance = val[21] << 8 | val[20]; - - f_integral.frame_count_since_last_readout = val[23] << 8 | val[22]; - f_integral.pixel_flow_x_integral = val[25] << 8 | val[24]; - f_integral.pixel_flow_y_integral = val[27] << 8 | val[26]; - f_integral.gyro_x_rate_integral = val[29] << 8 | val[28]; - f_integral.gyro_y_rate_integral = val[31] << 8 | val[30]; - f_integral.gyro_z_rate_integral = val[33] << 8 | val[32]; - f_integral.integration_timespan = val[37] << 24 | val[36] << 16 - | val[35] << 8 | val[34]; - f_integral.time_since_last_sonar_update = val[41] << 24 | val[40] << 16 - | val[39] << 8 | val[38]; - f_integral.ground_distance = val[43] << 8 | val[42]; - f_integral.gyro_temperature = val[45] << 8 | val[44]; - f_integral.qual = val[46]; + memcpy(&f, val, I2C_FRAME_SIZE); + memcpy(&f_integral, &(val[I2C_FRAME_SIZE]), I2C_INTEGRAL_FRAME_SIZE); } if (PX4FLOW_REG == 0x16) { - f_integral.frame_count_since_last_readout = val[1] << 8 | val[0]; - f_integral.pixel_flow_x_integral = val[3] << 8 | val[2]; - f_integral.pixel_flow_y_integral = val[5] << 8 | val[4]; - f_integral.gyro_x_rate_integral = val[7] << 8 | val[6]; - f_integral.gyro_y_rate_integral = val[9] << 8 | val[8]; - f_integral.gyro_z_rate_integral = val[11] << 8 | val[10]; - f_integral.integration_timespan = val[15] << 24 | val[14] << 16 | val[13] << 8 | val[12]; - f_integral.time_since_last_sonar_update = val[19] << 24 | val[18] << 16 | val[17] << 8 | val[16]; - f_integral.ground_distance = val[21] << 8 | val[20]; - f_integral.gyro_temperature = val[23] << 8 | val[22]; - f_integral.qual = val[24]; + memcpy(&f_integral, val, I2C_INTEGRAL_FRAME_SIZE); } @@ -544,7 +482,7 @@ PX4FLOW::collect() report.gyro_y_rate_integral = static_cast(f_integral.gyro_y_rate_integral) / 10000.0f; //convert to radians report.gyro_z_rate_integral = static_cast(f_integral.gyro_z_rate_integral) / 10000.0f; //convert to radians report.integration_timespan = f_integral.integration_timespan; //microseconds - report.time_since_last_sonar_update = f_integral.time_since_last_sonar_update;//microseconds + report.time_since_last_sonar_update = f_integral.sonar_timestamp;//microseconds report.gyro_temperature = f_integral.gyro_temperature;//Temperature * 100 in centi-degrees Celsius report.sensor_id = 0; @@ -828,7 +766,7 @@ test() warnx("ground_distance: %0.2f m", (double) f_integral.ground_distance / 1000); warnx("time since last sonar update [us]: %i", - f_integral.time_since_last_sonar_update); + f_integral.sonar_timestamp); warnx("quality integration average : %i", f_integral.qual); warnx("quality : %i", f.qual); -- cgit v1.2.3 From 18d756dd599f93abcd5dc89f323a5df77384ceac Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 26 Jan 2015 08:59:19 +0100 Subject: USB startup: Ensure that we are not talking to the peripheral too soon. Startup does not take longer due to smart rearrangement of launch calls --- ROMFS/px4fmu_common/init.d/rcS | 26 +++++--------------------- src/systemcmds/nshterm/module.mk | 2 +- src/systemcmds/nshterm/nshterm.c | 6 ++++++ 3 files changed, 12 insertions(+), 22 deletions(-) (limited to 'src') diff --git a/ROMFS/px4fmu_common/init.d/rcS b/ROMFS/px4fmu_common/init.d/rcS index 2c9387ff0..4d337171a 100644 --- a/ROMFS/px4fmu_common/init.d/rcS +++ b/ROMFS/px4fmu_common/init.d/rcS @@ -5,6 +5,11 @@ # NOTE: COMMENT LINES ARE REMOVED BEFORE STORED IN ROMFS. # +# +# Start CDC/ACM serial driver +# +sercon + # # Default to auto-start mode. # @@ -43,29 +48,8 @@ else fi unset FRC -# if this is an APM build then there will be a rc.APM script -# from an EXTERNAL_SCRIPTS build option -if [ -f /etc/init.d/rc.APM ] -then - if sercon - then - echo "[i] USB interface connected" - fi - - echo "[i] Running rc.APM" - # if APM startup is successful then nsh will exit - sh /etc/init.d/rc.APM -fi - if [ $MODE == autostart ] then - echo "[i] AUTOSTART mode" - - # - # Start CDC/ACM serial driver - # - sercon - # Try to get an USB console nshterm /dev/ttyACM0 & diff --git a/src/systemcmds/nshterm/module.mk b/src/systemcmds/nshterm/module.mk index a12bc369e..4e2710572 100644 --- a/src/systemcmds/nshterm/module.mk +++ b/src/systemcmds/nshterm/module.mk @@ -38,7 +38,7 @@ MODULE_COMMAND = nshterm SRCS = nshterm.c -MODULE_STACKSIZE = 1600 +MODULE_STACKSIZE = 1500 MAXOPTIMIZATION = -Os diff --git a/src/systemcmds/nshterm/nshterm.c b/src/systemcmds/nshterm/nshterm.c index ceaea35b6..50547a562 100644 --- a/src/systemcmds/nshterm/nshterm.c +++ b/src/systemcmds/nshterm/nshterm.c @@ -50,6 +50,7 @@ #include #include #include +#include #include @@ -67,6 +68,11 @@ nshterm_main(int argc, char *argv[]) int armed_fd = orb_subscribe(ORB_ID(actuator_armed)); struct actuator_armed_s armed; + /* back off 800 ms to avoid running into the USB setup timing */ + while (hrt_absolute_time() < 800U * 1000U) { + usleep(50000); + } + /* try to bring up the console - stop doing so if the system gets armed */ while (true) { -- cgit v1.2.3 From 9cc94fcb2dde1a591c20e008ca59d1f876c2070a Mon Sep 17 00:00:00 2001 From: Johan Jansen Date: Mon, 26 Jan 2015 13:51:34 +0100 Subject: mc_pos_control: Protect against NaN and Inf setpoints --- src/modules/mc_pos_control/mc_pos_control_main.cpp | 9 ++++++++- 1 file changed, 8 insertions(+), 1 deletion(-) (limited to 'src') diff --git a/src/modules/mc_pos_control/mc_pos_control_main.cpp b/src/modules/mc_pos_control/mc_pos_control_main.cpp index 3b631e2ce..962dc6d4a 100644 --- a/src/modules/mc_pos_control/mc_pos_control_main.cpp +++ b/src/modules/mc_pos_control/mc_pos_control_main.cpp @@ -726,11 +726,18 @@ MulticopterPositionControl::control_auto(float dt) reset_alt_sp(); } + //Poll position setpoint bool updated; orb_check(_pos_sp_triplet_sub, &updated); - if (updated) { orb_copy(ORB_ID(position_setpoint_triplet), _pos_sp_triplet_sub, &_pos_sp_triplet); + + //Make sure that the position setpoint is valid + if (!isfinite(_pos_sp_triplet.current.lat) || + !isfinite(_pos_sp_triplet.current.lon) || + !isfinite(_pos_sp_triplet.current.alt)) { + _pos_sp_triplet.current.valid = false; + } } if (_pos_sp_triplet.current.valid) { -- cgit v1.2.3 From 9c627255ccc980270fe56b6c4ddeb494e1ce0f50 Mon Sep 17 00:00:00 2001 From: Johan Jansen Date: Mon, 26 Jan 2015 14:22:36 +0100 Subject: MPU6000: Increase gyro offset tolerance to 7 dps --- src/drivers/mpu6000/mpu6000.cpp | 9 +++++---- 1 file changed, 5 insertions(+), 4 deletions(-) (limited to 'src') diff --git a/src/drivers/mpu6000/mpu6000.cpp b/src/drivers/mpu6000/mpu6000.cpp index 168b34ea9..81d156af1 100644 --- a/src/drivers/mpu6000/mpu6000.cpp +++ b/src/drivers/mpu6000/mpu6000.cpp @@ -921,18 +921,18 @@ MPU6000::gyro_self_test() if (self_test()) return 1; - /* evaluate gyro offsets, complain if offset -> zero or larger than 6 dps */ - if (fabsf(_gyro_scale.x_offset) > 0.1f || fabsf(_gyro_scale.x_offset) < 0.000001f) + /* evaluate gyro offsets, complain if offset -> zero or larger than 7 dps */ + if (fabsf(_gyro_scale.x_offset) > 0.12f || fabsf(_gyro_scale.x_offset) < 0.000001f) return 1; if (fabsf(_gyro_scale.x_scale - 1.0f) > 0.3f) return 1; - if (fabsf(_gyro_scale.y_offset) > 0.1f || fabsf(_gyro_scale.y_offset) < 0.000001f) + if (fabsf(_gyro_scale.y_offset) > 0.12f || fabsf(_gyro_scale.y_offset) < 0.000001f) return 1; if (fabsf(_gyro_scale.y_scale - 1.0f) > 0.3f) return 1; - if (fabsf(_gyro_scale.z_offset) > 0.1f || fabsf(_gyro_scale.z_offset) < 0.000001f) + if (fabsf(_gyro_scale.z_offset) > 0.12f || fabsf(_gyro_scale.z_offset) < 0.000001f) return 1; if (fabsf(_gyro_scale.z_scale - 1.0f) > 0.3f) return 1; @@ -941,6 +941,7 @@ MPU6000::gyro_self_test() } + /* perform a self-test comparison to factory trim values. This takes about 200ms and will return OK if the current values are within 14% -- cgit v1.2.3 From b8fade7dcfa98d0963c7f23529168baa64f7fc1a Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 26 Jan 2015 20:04:56 +0100 Subject: MPU6K: Improve gyro self test to allow more realistic deviations from nominal state --- src/drivers/mpu6000/mpu6000.cpp | 27 ++++++++++++++++++++------- 1 file changed, 20 insertions(+), 7 deletions(-) (limited to 'src') diff --git a/src/drivers/mpu6000/mpu6000.cpp b/src/drivers/mpu6000/mpu6000.cpp index 81d156af1..1c35bae2d 100644 --- a/src/drivers/mpu6000/mpu6000.cpp +++ b/src/drivers/mpu6000/mpu6000.cpp @@ -921,22 +921,35 @@ MPU6000::gyro_self_test() if (self_test()) return 1; - /* evaluate gyro offsets, complain if offset -> zero or larger than 7 dps */ - if (fabsf(_gyro_scale.x_offset) > 0.12f || fabsf(_gyro_scale.x_offset) < 0.000001f) + const float max_offset = 0.34f; + const float max_scale = 0.3f; + + /* evaluate gyro offsets, complain if offset -> zero or larger than 20 dps. */ + if (fabsf(_gyro_scale.x_offset) > max_offset) return 1; - if (fabsf(_gyro_scale.x_scale - 1.0f) > 0.3f) + + /* evaluate gyro scale, complain if off by more than 30% */ + if (fabsf(_gyro_scale.x_scale - 1.0f) > max_scale) return 1; - if (fabsf(_gyro_scale.y_offset) > 0.12f || fabsf(_gyro_scale.y_offset) < 0.000001f) + if (fabsf(_gyro_scale.y_offset) > max_offset) return 1; - if (fabsf(_gyro_scale.y_scale - 1.0f) > 0.3f) + if (fabsf(_gyro_scale.y_scale - 1.0f) > max_scale) return 1; - if (fabsf(_gyro_scale.z_offset) > 0.12f || fabsf(_gyro_scale.z_offset) < 0.000001f) + if (fabsf(_gyro_scale.z_offset) > max_offset) return 1; - if (fabsf(_gyro_scale.z_scale - 1.0f) > 0.3f) + if (fabsf(_gyro_scale.z_scale - 1.0f) > max_scale) return 1; + /* check if all scales are zero */ + if ((fabsf(_gyro_scale.x_offset) < 0.000001f) && + (fabsf(_gyro_scale.y_offset) < 0.000001f) && + (fabsf(_gyro_scale.z_offset) < 0.000001f)) { + /* if all are zero, this device is not calibrated */ + return 1; + } + return 0; } -- cgit v1.2.3 From 52f3fe1d9af53e5f1dd40714aa177fcc4c5e0047 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 26 Jan 2015 20:23:42 +0100 Subject: Added more docs to offset as suggested by @velocoderaptor, thanks! --- src/drivers/mpu6000/mpu6000.cpp | 10 ++++++++++ 1 file changed, 10 insertions(+) (limited to 'src') diff --git a/src/drivers/mpu6000/mpu6000.cpp b/src/drivers/mpu6000/mpu6000.cpp index 1c35bae2d..2be758244 100644 --- a/src/drivers/mpu6000/mpu6000.cpp +++ b/src/drivers/mpu6000/mpu6000.cpp @@ -921,7 +921,17 @@ MPU6000::gyro_self_test() if (self_test()) return 1; + /* + * Maximum deviation of 20 degrees, according to + * http://www.invensense.com/mems/gyro/documents/PS-MPU-6000A-00v3.4.pdf + * Section 6.1, initial ZRO tolerance + */ const float max_offset = 0.34f; + /* 30% scale error is chosen to catch completely faulty units but + * to let some slight scale error pass. Requires a rate table or correlation + * with mag rotations + data fit to + * calibrate properly and is not done by default. + */ const float max_scale = 0.3f; /* evaluate gyro offsets, complain if offset -> zero or larger than 20 dps. */ -- cgit v1.2.3 From e95e09628484875cf65686561eef8434195df96b Mon Sep 17 00:00:00 2001 From: Roman Bapst Date: Wed, 28 Jan 2015 09:31:42 +0100 Subject: added references --- src/modules/mc_pos_control/mc_pos_control_main.cpp | 6 ++++++ 1 file changed, 6 insertions(+) (limited to 'src') diff --git a/src/modules/mc_pos_control/mc_pos_control_main.cpp b/src/modules/mc_pos_control/mc_pos_control_main.cpp index 962dc6d4a..cad6cf3ae 100644 --- a/src/modules/mc_pos_control/mc_pos_control_main.cpp +++ b/src/modules/mc_pos_control/mc_pos_control_main.cpp @@ -35,6 +35,12 @@ * @file mc_pos_control_main.cpp * Multicopter position controller. * + * Original publication for the desired attitude generation: + * Daniel Mellinger and Vijay Kumar. Minimum Snap Trajectory Generation and Control for Quadrotors. + * Int. Conf. on Robotics and Automation, Shanghai, China, May 2011 + * + * Also inspired by https://pixhawk.org/firmware/apps/fw_pos_control_l1 + * * The controller has two loops: P loop for position error and PID loop for velocity error. * Output of velocity controller is thrust vector that splitted to thrust direction * (i.e. rotation matrix for multicopter orientation) and thrust module (i.e. multicopter thrust itself). -- cgit v1.2.3 From 97471bf0aae5c069fe37f063f2a8b5eae08755c9 Mon Sep 17 00:00:00 2001 From: Roman Bapst Date: Wed, 28 Jan 2015 09:31:42 +0100 Subject: added references --- src/modules/mc_att_control/mc_att_control_main.cpp | 4 ++++ 1 file changed, 4 insertions(+) (limited to 'src') diff --git a/src/modules/mc_att_control/mc_att_control_main.cpp b/src/modules/mc_att_control/mc_att_control_main.cpp index a094ed2c6..c828f1f94 100644 --- a/src/modules/mc_att_control/mc_att_control_main.cpp +++ b/src/modules/mc_att_control/mc_att_control_main.cpp @@ -35,6 +35,10 @@ * @file mc_att_control_main.cpp * Multicopter attitude controller. * + * Publication for the desired attitude tracking: + * Daniel Mellinger and Vijay Kumar. Minimum Snap Trajectory Generation and Control for Quadrotors. + * Int. Conf. on Robotics and Automation, Shanghai, China, May 2011. + * * @author Tobias Naegeli * @author Lorenz Meier * @author Anton Babushkin -- cgit v1.2.3 From 5444972347606333dd61206011263e4a2e5d83e8 Mon Sep 17 00:00:00 2001 From: hauptmech Date: Mon, 19 Jan 2015 16:50:52 +1300 Subject: Add call to access the mcu unique id. Expose via the 'ver' command. This is prep for verifying calibration parameters against the hardware they were gathered on. --- src/modules/systemlib/mcu_version.c | 11 +++++++++-- src/modules/systemlib/mcu_version.h | 11 +++++++++++ src/systemcmds/ver/ver.c | 14 +++++++++++++- 3 files changed, 33 insertions(+), 3 deletions(-) (limited to 'src') diff --git a/src/modules/systemlib/mcu_version.c b/src/modules/systemlib/mcu_version.c index 4bcf95784..24f4e4207 100644 --- a/src/modules/systemlib/mcu_version.c +++ b/src/modules/systemlib/mcu_version.c @@ -47,7 +47,8 @@ #ifdef CONFIG_ARCH_CHIP_STM32 #include -#define DBGMCU_IDCODE 0xE0042000 +#define DBGMCU_IDCODE 0xE0042000 //STM DocID018909 Rev 8 Sect 38.18 (MCU device ID code) +#define UNIQUE_ID 0x1FFF7A10 //STM DocID018909 Rev 8 Sect 39.1 (Unique device ID Register) #define STM32F40x_41x 0x413 #define STM32F42x_43x 0x419 @@ -57,7 +58,13 @@ #endif - +/** Copy the 96bit MCU Unique ID into the provided pointer */ +void mcu_unique_id(uint32_t *uid_96_bit) +{ + uid_96_bit[0] = getreg32(UNIQUE_ID); + uid_96_bit[1] = getreg32(UNIQUE_ID+4); + uid_96_bit[2] = getreg32(UNIQUE_ID+8); +} int mcu_version(char* rev, char** revstr) { diff --git a/src/modules/systemlib/mcu_version.h b/src/modules/systemlib/mcu_version.h index 1b3d0aba9..c8a0bf5cd 100644 --- a/src/modules/systemlib/mcu_version.h +++ b/src/modules/systemlib/mcu_version.h @@ -33,6 +33,8 @@ #pragma once +#include + /* magic numbers from reference manual */ enum MCU_REV { MCU_REV_STM32F4_REV_A = 0x1000, @@ -42,6 +44,15 @@ enum MCU_REV { MCU_REV_STM32F4_REV_3 = 0x2001 }; + +/** + * Reports the microcontroller unique id. + * + * This ID is guaranteed to be unique for every mcu. + * @param uid_96_bit A uint32_t[3] array to copy the data to. + */ +__EXPORT void mcu_unique_id(uint32_t *uid_96_bit); + /** * Reports the microcontroller version of the main CPU. * diff --git a/src/systemcmds/ver/ver.c b/src/systemcmds/ver/ver.c index 2ead3e632..087eb52e3 100644 --- a/src/systemcmds/ver/ver.c +++ b/src/systemcmds/ver/ver.c @@ -54,6 +54,7 @@ static const char sz_ver_bdate_str[] = "bdate"; static const char sz_ver_gcc_str[] = "gcc"; static const char sz_ver_all_str[] = "all"; static const char mcu_ver_str[] = "mcu"; +static const char mcu_uid_str[] = "uid"; static void usage(const char *reason) { @@ -61,7 +62,7 @@ static void usage(const char *reason) printf("%s\n", reason); } - printf("usage: ver {hw|hwcmp|git|bdate|gcc|all|mcu}\n\n"); + printf("usage: ver {hw|hwcmp|git|bdate|gcc|all|mcu|uid}\n\n"); } __EXPORT int ver_main(int argc, char *argv[]); @@ -141,6 +142,17 @@ int ver_main(int argc, char *argv[]) ret = 0; } + if (show_all || !strncmp(argv[1], mcu_uid_str, sizeof(mcu_uid_str))) { + uint32_t uid[3]; + + mcu_unique_id(uid); + + printf("UID: %X:%X:%X \n",uid[0],uid[1],uid[2]); + + ret = 0; + } + + if (ret == 1) { errx(1, "unknown command.\n"); } -- cgit v1.2.3 From 21d3dcb1905432e349f70012ac0a4f79e5e7935a Mon Sep 17 00:00:00 2001 From: hauptmech Date: Wed, 28 Jan 2015 15:40:44 +1300 Subject: Fix config utility to work with all devices of each type, not just the primary one. --- src/systemcmds/config/config.c | 70 +++++++++++++++++++++--------------------- 1 file changed, 35 insertions(+), 35 deletions(-) (limited to 'src') diff --git a/src/systemcmds/config/config.c b/src/systemcmds/config/config.c index 077bc47c9..9f13edb18 100644 --- a/src/systemcmds/config/config.c +++ b/src/systemcmds/config/config.c @@ -36,7 +36,7 @@ * @author Lorenz Meier * @author Julian Oes * - * config tool. + * config tool. Takes the device name as the first parameter. */ #include @@ -71,18 +71,18 @@ int config_main(int argc, char *argv[]) { if (argc >= 2) { - if (!strcmp(argv[1], "gyro")) { - do_gyro(argc - 2, argv + 2); - } else if (!strcmp(argv[1], "accel")) { - do_accel(argc - 2, argv + 2); - } else if (!strcmp(argv[1], "mag")) { - do_mag(argc - 2, argv + 2); + if (!strncmp(argv[1], "/dev/gyro",9)) { + do_gyro(argc - 1, argv + 1); + } else if (!strncmp(argv[1], "/dev/accel",10)) { + do_accel(argc - 1, argv + 1); + } else if (!strncmp(argv[1], "/dev/mag",8)) { + do_mag(argc - 1, argv + 1); } else { do_device(argc - 1, argv + 1); } } - errx(1, "expected a command, try 'gyro', 'accel', 'mag'"); + errx(1, "expected a device, try '/dev/gyro', '/dev/accel', '/dev/mag'"); } static void @@ -133,41 +133,41 @@ do_gyro(int argc, char *argv[]) { int fd; - fd = open(GYRO_DEVICE_PATH, 0); + fd = open(argv[0], 0); if (fd < 0) { - warn("%s", GYRO_DEVICE_PATH); + warn("%s", argv[0]); errx(1, "FATAL: no gyro found"); } else { int ret; - if (argc == 2 && !strcmp(argv[0], "sampling")) { + if (argc == 3 && !strcmp(argv[1], "sampling")) { /* set the gyro internal sampling rate up to at least i Hz */ - ret = ioctl(fd, GYROIOCSSAMPLERATE, strtoul(argv[1], NULL, 0)); + ret = ioctl(fd, GYROIOCSSAMPLERATE, strtoul(argv[2], NULL, 0)); if (ret) errx(ret,"sampling rate could not be set"); - } else if (argc == 2 && !strcmp(argv[0], "rate")) { + } else if (argc == 3 && !strcmp(argv[1], "rate")) { /* set the driver to poll at i Hz */ - ret = ioctl(fd, SENSORIOCSPOLLRATE, strtoul(argv[1], NULL, 0)); + ret = ioctl(fd, SENSORIOCSPOLLRATE, strtoul(argv[2], NULL, 0)); if (ret) errx(ret,"pollrate could not be set"); - } else if (argc == 2 && !strcmp(argv[0], "range")) { + } else if (argc == 3 && !strcmp(argv[1], "range")) { /* set the range to i dps */ - ret = ioctl(fd, GYROIOCSRANGE, strtoul(argv[1], NULL, 0)); + ret = ioctl(fd, GYROIOCSRANGE, strtoul(argv[2], NULL, 0)); if (ret) errx(ret,"range could not be set"); - } else if (argc == 1 && !strcmp(argv[0], "check")) { + } else if (argc == 2 && !strcmp(argv[1], "check")) { ret = ioctl(fd, GYROIOCSELFTEST, 0); if (ret) { @@ -206,41 +206,41 @@ do_mag(int argc, char *argv[]) { int fd; - fd = open(MAG_DEVICE_PATH, 0); + fd = open(argv[0], 0); if (fd < 0) { - warn("%s", MAG_DEVICE_PATH); + warn("%s", argv[0]); errx(1, "FATAL: no magnetometer found"); } else { int ret; - if (argc == 2 && !strcmp(argv[0], "sampling")) { + if (argc == 3 && !strcmp(argv[1], "sampling")) { /* set the mag internal sampling rate up to at least i Hz */ - ret = ioctl(fd, MAGIOCSSAMPLERATE, strtoul(argv[1], NULL, 0)); + ret = ioctl(fd, MAGIOCSSAMPLERATE, strtoul(argv[2], NULL, 0)); if (ret) errx(ret,"sampling rate could not be set"); - } else if (argc == 2 && !strcmp(argv[0], "rate")) { + } else if (argc == 3 && !strcmp(argv[1], "rate")) { /* set the driver to poll at i Hz */ - ret = ioctl(fd, SENSORIOCSPOLLRATE, strtoul(argv[1], NULL, 0)); + ret = ioctl(fd, SENSORIOCSPOLLRATE, strtoul(argv[2], NULL, 0)); if (ret) errx(ret,"pollrate could not be set"); - } else if (argc == 2 && !strcmp(argv[0], "range")) { + } else if (argc == 3 && !strcmp(argv[1], "range")) { /* set the range to i G */ - ret = ioctl(fd, MAGIOCSRANGE, strtoul(argv[1], NULL, 0)); + ret = ioctl(fd, MAGIOCSRANGE, strtoul(argv[2], NULL, 0)); if (ret) errx(ret,"range could not be set"); - } else if(argc == 1 && !strcmp(argv[0], "check")) { + } else if(argc == 2 && !strcmp(argv[1], "check")) { ret = ioctl(fd, MAGIOCSELFTEST, 0); if (ret) { @@ -282,41 +282,41 @@ do_accel(int argc, char *argv[]) { int fd; - fd = open(ACCEL_DEVICE_PATH, 0); + fd = open(argv[0], 0); if (fd < 0) { - warn("%s", ACCEL_DEVICE_PATH); + warn("%s", argv[0]); errx(1, "FATAL: no accelerometer found"); } else { int ret; - if (argc == 2 && !strcmp(argv[0], "sampling")) { + if (argc == 3 && !strcmp(argv[1], "sampling")) { /* set the accel internal sampling rate up to at least i Hz */ - ret = ioctl(fd, ACCELIOCSSAMPLERATE, strtoul(argv[1], NULL, 0)); + ret = ioctl(fd, ACCELIOCSSAMPLERATE, strtoul(argv[2], NULL, 0)); if (ret) errx(ret,"sampling rate could not be set"); - } else if (argc == 2 && !strcmp(argv[0], "rate")) { + } else if (argc == 3 && !strcmp(argv[1], "rate")) { /* set the driver to poll at i Hz */ - ret = ioctl(fd, SENSORIOCSPOLLRATE, strtoul(argv[1], NULL, 0)); + ret = ioctl(fd, SENSORIOCSPOLLRATE, strtoul(argv[2], NULL, 0)); if (ret) errx(ret,"pollrate could not be set"); - } else if (argc == 2 && !strcmp(argv[0], "range")) { + } else if (argc == 3 && !strcmp(argv[1], "range")) { /* set the range to i G */ - ret = ioctl(fd, ACCELIOCSRANGE, strtoul(argv[1], NULL, 0)); + ret = ioctl(fd, ACCELIOCSRANGE, strtoul(argv[2], NULL, 0)); if (ret) errx(ret,"range could not be set"); - } else if(argc == 1 && !strcmp(argv[0], "check")) { + } else if(argc == 2 && !strcmp(argv[1], "check")) { ret = ioctl(fd, ACCELIOCSELFTEST, 0); if (ret) { -- cgit v1.2.3