From 7c33e1fdb5c89b192586f7d0ba406d07431d9c51 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 22 Aug 2015 13:51:28 +0200 Subject: [PATCH 01/26] Update NuttX submodule --- NuttX | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/NuttX b/NuttX index 678eea3d2c..8891d035df 160000 --- a/NuttX +++ b/NuttX @@ -1 +1 @@ -Subproject commit 678eea3d2c157c686439597abb0367b0f1e643f4 +Subproject commit 8891d035df45c8be570cfbd9419b438679faf7ee From a38d954d9afc08984b747af45fab1684c5e57650 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 22 Aug 2015 13:52:05 +0200 Subject: [PATCH 02/26] Update MAVLink submodule --- mavlink/include/mavlink/v1.0 | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/mavlink/include/mavlink/v1.0 b/mavlink/include/mavlink/v1.0 index d23a551e10..c390a63abf 160000 --- a/mavlink/include/mavlink/v1.0 +++ b/mavlink/include/mavlink/v1.0 @@ -1 +1 @@ -Subproject commit d23a551e108d92c7a49a66d162cb9d542bbe6be1 +Subproject commit c390a63abfce15af0629bdf207c4b6a183fed118 From 4ec16efe686afbad267f58e7b859fa6638f46afb Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Sun, 16 Aug 2015 22:58:09 +0300 Subject: [PATCH 03/26] Libuavcan update --- src/lib/uavcan | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/lib/uavcan b/src/lib/uavcan index 46793c5e06..181b4f6094 160000 --- a/src/lib/uavcan +++ b/src/lib/uavcan @@ -1 +1 @@ -Subproject commit 46793c5e0699d494cb9ec10ca70b6d833acbec46 +Subproject commit 181b4f609491df942a2a263d4b6dac78004e2ce0 From 473d321af8bead4bd45299ad723b4c7beec94bce Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Sun, 16 Aug 2015 23:04:41 +0300 Subject: [PATCH 04/26] UAVCAN Magnetometer driver update - no logical changes --- src/modules/uavcan/sensors/mag.cpp | 10 +++++----- src/modules/uavcan/sensors/mag.hpp | 8 ++++---- 2 files changed, 9 insertions(+), 9 deletions(-) diff --git a/src/modules/uavcan/sensors/mag.cpp b/src/modules/uavcan/sensors/mag.cpp index d08f5ed32b..ef9613b1f7 100644 --- a/src/modules/uavcan/sensors/mag.cpp +++ b/src/modules/uavcan/sensors/mag.cpp @@ -45,7 +45,7 @@ UavcanMagnetometerBridge::UavcanMagnetometerBridge(uavcan::INode &node) : UavcanCDevSensorBridgeBase("uavcan_mag", "/dev/uavcan/mag", MAG_BASE_DEVICE_PATH, ORB_ID(sensor_mag)), _sub_mag(node) { - _device_id.devid_s.devtype = DRV_MAG_DEVTYPE_HMC5883; + _device_id.devid_s.devtype = DRV_MAG_DEVTYPE_HMC5883; // <-- Why? _scale.x_scale = 1.0F; _scale.y_scale = 1.0F; @@ -141,16 +141,16 @@ int UavcanMagnetometerBridge::ioctl(struct file *filp, int cmd, unsigned long ar } } -void UavcanMagnetometerBridge::mag_sub_cb(const uavcan::ReceivedDataStructure +void UavcanMagnetometerBridge::mag_sub_cb(const uavcan::ReceivedDataStructure &msg) { lock(); _report.range_ga = 1.3F; // Arbitrary number, doesn't really mean anything _report.timestamp = msg.getMonotonicTimestamp().toUSec(); - _report.x = (msg.magnetic_field[0] - _scale.x_offset) * _scale.x_scale; - _report.y = (msg.magnetic_field[1] - _scale.y_offset) * _scale.y_scale; - _report.z = (msg.magnetic_field[2] - _scale.z_offset) * _scale.z_scale; + _report.x = (msg.magnetic_field_ga[0] - _scale.x_offset) * _scale.x_scale; + _report.y = (msg.magnetic_field_ga[1] - _scale.y_offset) * _scale.y_scale; + _report.z = (msg.magnetic_field_ga[2] - _scale.z_offset) * _scale.z_scale; unlock(); publish(msg.getSrcNodeID().get(), &_report); diff --git a/src/modules/uavcan/sensors/mag.hpp b/src/modules/uavcan/sensors/mag.hpp index b74afeb0be..a13883a494 100644 --- a/src/modules/uavcan/sensors/mag.hpp +++ b/src/modules/uavcan/sensors/mag.hpp @@ -40,7 +40,7 @@ #include "sensor_bridge.hpp" #include -#include +#include class UavcanMagnetometerBridge : public UavcanCDevSensorBridgeBase { @@ -57,14 +57,14 @@ private: ssize_t read(struct file *filp, char *buffer, size_t buflen); int ioctl(struct file *filp, int cmd, unsigned long arg) override; - void mag_sub_cb(const uavcan::ReceivedDataStructure &msg); + void mag_sub_cb(const uavcan::ReceivedDataStructure &msg); typedef uavcan::MethodBinder < UavcanMagnetometerBridge *, void (UavcanMagnetometerBridge::*) - (const uavcan::ReceivedDataStructure &) > + (const uavcan::ReceivedDataStructure &) > MagCbBinder; - uavcan::Subscriber _sub_mag; + uavcan::Subscriber _sub_mag; mag_scale _scale = {}; mag_report _report = {}; }; From 3860dbdc8cfec81971f709b696b6d1f3a7a8ca8b Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Sun, 16 Aug 2015 23:07:16 +0300 Subject: [PATCH 05/26] UAVCAN Barometer driver fix - reporting temperature in Celsius degrees, not in Kelvin --- src/modules/uavcan/sensors/baro.cpp | 8 +++----- src/modules/uavcan/sensors/baro.hpp | 2 +- 2 files changed, 4 insertions(+), 6 deletions(-) diff --git a/src/modules/uavcan/sensors/baro.cpp b/src/modules/uavcan/sensors/baro.cpp index 8198d1d994..50655d9180 100644 --- a/src/modules/uavcan/sensors/baro.cpp +++ b/src/modules/uavcan/sensors/baro.cpp @@ -46,9 +46,7 @@ UavcanBarometerBridge::UavcanBarometerBridge(uavcan::INode &node) : _sub_air_pressure_data(node), _sub_air_temperature_data(node), _reports(nullptr) -{ - last_temperature = 0.0f; -} +{ } int UavcanBarometerBridge::init() { @@ -154,7 +152,7 @@ int UavcanBarometerBridge::ioctl(struct file *filp, int cmd, unsigned long arg) void UavcanBarometerBridge::air_temperature_sub_cb(const uavcan::ReceivedDataStructure &msg) { - last_temperature = msg.static_temperature; + last_temperature_kelvin = msg.static_temperature; } void UavcanBarometerBridge::air_pressure_sub_cb(const @@ -163,7 +161,7 @@ void UavcanBarometerBridge::air_pressure_sub_cb(const baro_report report; report.timestamp = msg.getMonotonicTimestamp().toUSec(); - report.temperature = last_temperature; + report.temperature = last_temperature_kelvin - 273.15F; report.pressure = msg.static_pressure / 100.0F; // Convert to millibar report.error_count = 0; diff --git a/src/modules/uavcan/sensors/baro.hpp b/src/modules/uavcan/sensors/baro.hpp index 98dc5cf42f..4024cb156b 100644 --- a/src/modules/uavcan/sensors/baro.hpp +++ b/src/modules/uavcan/sensors/baro.hpp @@ -77,6 +77,6 @@ private: uavcan::Subscriber _sub_air_temperature_data; unsigned _msl_pressure = 101325; RingBuffer *_reports; - float last_temperature; + float last_temperature_kelvin = 0.0; }; From 9b32a55b402d9c1d9a6bce716c1a03f2b2c1e1cb Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Mon, 17 Aug 2015 14:58:26 +0300 Subject: [PATCH 06/26] Libuavcan update --- src/lib/uavcan | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/lib/uavcan b/src/lib/uavcan index 181b4f6094..bfee82f3c4 160000 --- a/src/lib/uavcan +++ b/src/lib/uavcan @@ -1 +1 @@ -Subproject commit 181b4f609491df942a2a263d4b6dac78004e2ce0 +Subproject commit bfee82f3c43262ff2a6fec83954682fd7907e848 From 4296dac884220d2a255ff02b5c1a87cd0a19c4a1 Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Tue, 18 Aug 2015 16:06:26 +0300 Subject: [PATCH 07/26] Libuavcan update --- src/lib/uavcan | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/lib/uavcan b/src/lib/uavcan index bfee82f3c4..b317b94d98 160000 --- a/src/lib/uavcan +++ b/src/lib/uavcan @@ -1 +1 @@ -Subproject commit bfee82f3c43262ff2a6fec83954682fd7907e848 +Subproject commit b317b94d98404dc303cbe2b551af18377ec1b252 From 6864779fa6bc3e895461d47e1322f4e90c642d7a Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Tue, 18 Aug 2015 16:51:36 +0300 Subject: [PATCH 08/26] Multiple stages for UAVCAN_ENABLE --- ROMFS/px4fmu_common/init.d/rc.uavcan | 30 ++++++++++++++++++++++++++++ src/modules/uavcan/uavcan_params.c | 7 +++++-- 2 files changed, 35 insertions(+), 2 deletions(-) diff --git a/ROMFS/px4fmu_common/init.d/rc.uavcan b/ROMFS/px4fmu_common/init.d/rc.uavcan index 08ba86d789..9f38ff90bc 100644 --- a/ROMFS/px4fmu_common/init.d/rc.uavcan +++ b/ROMFS/px4fmu_common/init.d/rc.uavcan @@ -3,7 +3,26 @@ # UAVCAN initialization script. # +# +# Mirriring the UAVCAN_ENABLE param value to an eponymous environment variable. +# TODO there should be a smarter way. +# +set UAVCAN_ENABLE 0 if param compare UAVCAN_ENABLE 1 +then + set UAVCAN_ENABLE 1 +fi +if param compare UAVCAN_ENABLE 2 +then + set UAVCAN_ENABLE 2 +fi + +echo "[i] UAVCAN_ENABLE is $UAVCAN_ENABLE" + +# +# Starting stuff according to UAVCAN_ENABLE value +# +if [ $UAVCAN_ENABLE -ge 1 ] then if uavcan start then @@ -16,3 +35,14 @@ then tone_alarm $TUNE_ERR fi fi + +if [ $UAVCAN_ENABLE -ge 2 ] +then + if uavcan start fw + then + echo "[i] UAVCAN servers started" + else + echo "[i] ERROR: Could not start UAVCAN servers" + tone_alarm $TUNE_ERR + fi +fi diff --git a/src/modules/uavcan/uavcan_params.c b/src/modules/uavcan/uavcan_params.c index e6ea8a8fb7..934263b6b4 100644 --- a/src/modules/uavcan/uavcan_params.c +++ b/src/modules/uavcan/uavcan_params.c @@ -41,10 +41,13 @@ /** * Enable UAVCAN. * - * Enables support for UAVCAN-interfaced actuators and sensors. + * Allowed values: + * 0 - UAVCAN disabled. + * 1 - Enabled support for UAVCAN actuators and sensors. + * 2 - Enabled support for dynamic node ID allocation and firmware update. * * @min 0 - * @max 1 + * @max 2 * @group UAVCAN */ PARAM_DEFINE_INT32(UAVCAN_ENABLE, 0); From e1f33871efd87f4b910390c853a9d301aaab8028 Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Tue, 18 Aug 2015 17:25:23 +0300 Subject: [PATCH 09/26] UAVCAN enabled by default --- src/modules/uavcan/uavcan_params.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/uavcan/uavcan_params.c b/src/modules/uavcan/uavcan_params.c index 934263b6b4..76d0e8ea09 100644 --- a/src/modules/uavcan/uavcan_params.c +++ b/src/modules/uavcan/uavcan_params.c @@ -50,7 +50,7 @@ * @max 2 * @group UAVCAN */ -PARAM_DEFINE_INT32(UAVCAN_ENABLE, 0); +PARAM_DEFINE_INT32(UAVCAN_ENABLE, 1); /** * UAVCAN Node ID. From 0e10638c7e0d43c6ed4d608ee418e759b0b77202 Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Tue, 18 Aug 2015 23:11:12 +0300 Subject: [PATCH 10/26] Temporary fix to UAVCAN sensor prioritization issue, see #2081, #2715 --- ROMFS/px4fmu_common/init.d/rc.uavcan | 25 +++++++++++++------------ ROMFS/px4fmu_common/init.d/rcS | 9 +++++---- 2 files changed, 18 insertions(+), 16 deletions(-) diff --git a/ROMFS/px4fmu_common/init.d/rc.uavcan b/ROMFS/px4fmu_common/init.d/rc.uavcan index 9f38ff90bc..3615c177de 100644 --- a/ROMFS/px4fmu_common/init.d/rc.uavcan +++ b/ROMFS/px4fmu_common/init.d/rc.uavcan @@ -10,11 +10,11 @@ set UAVCAN_ENABLE 0 if param compare UAVCAN_ENABLE 1 then - set UAVCAN_ENABLE 1 + set UAVCAN_ENABLE 1 fi if param compare UAVCAN_ENABLE 2 then - set UAVCAN_ENABLE 2 + set UAVCAN_ENABLE 2 fi echo "[i] UAVCAN_ENABLE is $UAVCAN_ENABLE" @@ -26,9 +26,6 @@ if [ $UAVCAN_ENABLE -ge 1 ] then if uavcan start then - # First sensor publisher to initialize takes lowest instance ID - # This delay ensures that UAVCAN-interfaced sensors would be allocated on lowest instance IDs - sleep 1 echo "[i] UAVCAN started" else echo "[i] ERROR: Could not start UAVCAN" @@ -38,11 +35,15 @@ fi if [ $UAVCAN_ENABLE -ge 2 ] then - if uavcan start fw - then - echo "[i] UAVCAN servers started" - else - echo "[i] ERROR: Could not start UAVCAN servers" - tone_alarm $TUNE_ERR - fi + if uavcan start fw + then + echo "[i] UAVCAN servers started" + else + echo "[i] ERROR: Could not start UAVCAN servers" + tone_alarm $TUNE_ERR + fi fi + +# First sensor publisher to initialize takes lowest instance ID +# This delay ensures that UAVCAN-interfaced sensors will be allocated on lowest instance IDs +sleep 8 diff --git a/ROMFS/px4fmu_common/init.d/rcS b/ROMFS/px4fmu_common/init.d/rcS index c1c8dc303d..b21ca9ec88 100644 --- a/ROMFS/px4fmu_common/init.d/rcS +++ b/ROMFS/px4fmu_common/init.d/rcS @@ -299,6 +299,11 @@ then then fi + # + # UAVCAN + # + sh /etc/init.d/rc.uavcan + # # Sensors System (start before Commander so Preflight checks are properly run) # @@ -491,10 +496,6 @@ then fi fi - # UAVCAN - # - sh /etc/init.d/rc.uavcan - # # Logging # From 281cf49586f28434880ba422e03ef3b3e7567c3e Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Thu, 20 Aug 2015 05:45:00 +0300 Subject: [PATCH 11/26] Libuavcan update - fixed file IO loops --- src/lib/uavcan | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/lib/uavcan b/src/lib/uavcan index b317b94d98..cf39ecf879 160000 --- a/src/lib/uavcan +++ b/src/lib/uavcan @@ -1 +1 @@ -Subproject commit b317b94d98404dc303cbe2b551af18377ec1b252 +Subproject commit cf39ecf87900be8be30911194d366a43d9b56e2e From 59fd94da3e75dc8a827979a5f19ca93f88efa4a7 Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Thu, 20 Aug 2015 12:37:51 +0300 Subject: [PATCH 12/26] Libuavcan synced with the version that has DSDL v1.0.0rc1 - no logical changes --- src/lib/uavcan | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/lib/uavcan b/src/lib/uavcan index cf39ecf879..3ae5400aa5 160000 --- a/src/lib/uavcan +++ b/src/lib/uavcan @@ -1 +1 @@ -Subproject commit cf39ecf87900be8be30911194d366a43d9b56e2e +Subproject commit 3ae5400aa5ead18139106d30f730114d5e9b65dd From 08fde4f505a8ae085fb49572add2028331dc8998 Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Fri, 21 Aug 2015 20:19:02 +0300 Subject: [PATCH 13/26] UAVCAN startup delay fix --- ROMFS/px4fmu_common/init.d/rc.uavcan | 9 ++++++--- 1 file changed, 6 insertions(+), 3 deletions(-) diff --git a/ROMFS/px4fmu_common/init.d/rc.uavcan b/ROMFS/px4fmu_common/init.d/rc.uavcan index 3615c177de..37e6893bd1 100644 --- a/ROMFS/px4fmu_common/init.d/rc.uavcan +++ b/ROMFS/px4fmu_common/init.d/rc.uavcan @@ -44,6 +44,9 @@ then fi fi -# First sensor publisher to initialize takes lowest instance ID -# This delay ensures that UAVCAN-interfaced sensors will be allocated on lowest instance IDs -sleep 8 +if [ $UAVCAN_ENABLE -ge 1 ] +then + # First sensor publisher to initialize takes lowest instance ID + # This delay ensures that UAVCAN-interfaced sensors will be allocated on lowest instance IDs + sleep 8 +fi From f371ece43b5e6b028a4be4c90c06136adf087e22 Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Fri, 21 Aug 2015 22:56:49 +0300 Subject: [PATCH 14/26] UAVCAN_ENABLE defaults to 0 --- src/modules/uavcan/uavcan_params.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/uavcan/uavcan_params.c b/src/modules/uavcan/uavcan_params.c index 76d0e8ea09..934263b6b4 100644 --- a/src/modules/uavcan/uavcan_params.c +++ b/src/modules/uavcan/uavcan_params.c @@ -50,7 +50,7 @@ * @max 2 * @group UAVCAN */ -PARAM_DEFINE_INT32(UAVCAN_ENABLE, 1); +PARAM_DEFINE_INT32(UAVCAN_ENABLE, 0); /** * UAVCAN Node ID. From 221c222ca257862aec6876ce1774cb690e13150d Mon Sep 17 00:00:00 2001 From: tumbili Date: Wed, 19 Aug 2015 22:43:13 +0200 Subject: [PATCH 15/26] comment out code which relied on old tecs logging format --- src/modules/fw_pos_control_l1/mtecs/mTecs.cpp | 22 +++++++++---------- 1 file changed, 11 insertions(+), 11 deletions(-) diff --git a/src/modules/fw_pos_control_l1/mtecs/mTecs.cpp b/src/modules/fw_pos_control_l1/mtecs/mTecs.cpp index cfba54d452..c07956fd6e 100644 --- a/src/modules/fw_pos_control_l1/mtecs/mTecs.cpp +++ b/src/modules/fw_pos_control_l1/mtecs/mTecs.cpp @@ -226,17 +226,17 @@ int mTecs::updateFlightPathAngleAcceleration(float flightPathAngle, float flight * is running) */ limitOverride.applyOverride(*outputLimiterThrottle, *outputLimiterPitch); - /* Write part of the status message */ - _status.flightPathAngleSp = flightPathAngleSp; - _status.flightPathAngle = flightPathAngle; - _status.flightPathAngleFiltered = flightPathAngleFiltered; - _status.airspeedDerivativeSp = airspeedDerivativeSp; - _status.airspeedDerivative = airspeedDerivative; - _status.totalEnergyRateSp = totalEnergyRateSp; - _status.totalEnergyRate = totalEnergyRate; - _status.energyDistributionRateSp = energyDistributionRateSp; - _status.energyDistributionRate = energyDistributionRate; - _status.mode = mode; + // /* Write part of the status message */ + // _status.flightPathAngleSp = flightPathAngleSp; + // _status.flightPathAngle = flightPathAngle; + // _status.flightPathAngleFiltered = flightPathAngleFiltered; + // _status.airspeedDerivativeSp = airspeedDerivativeSp; + // _status.airspeedDerivative = airspeedDerivative; + // _status.totalEnergyRateSp = totalEnergyRateSp; + // _status.totalEnergyRate = totalEnergyRate; + // _status.energyDistributionRateSp = energyDistributionRateSp; + // _status.energyDistributionRate = energyDistributionRate; + // _status.mode = mode; /** update control blocks **/ /* update total energy rate control block */ From 6cdceb1829d2674c27a9e53639badd9c901fe03d Mon Sep 17 00:00:00 2001 From: tumbili Date: Wed, 19 Aug 2015 22:43:42 +0200 Subject: [PATCH 16/26] fix tecs status logging --- msg/tecs_status.msg | 11 +++-- src/lib/external_lgpl/tecs/tecs.cpp | 49 +++++++++++-------- src/lib/external_lgpl/tecs/tecs.h | 41 +++++++++++----- .../fw_pos_control_l1_main.cpp | 29 ++++++----- src/modules/sdlog2/sdlog2.c | 11 +++-- src/modules/sdlog2/sdlog2_messages.h | 14 +++--- 6 files changed, 94 insertions(+), 61 deletions(-) diff --git a/msg/tecs_status.msg b/msg/tecs_status.msg index ccac921e2b..7e9d6670c9 100644 --- a/msg/tecs_status.msg +++ b/msg/tecs_status.msg @@ -18,9 +18,12 @@ float32 airspeed_filtered float32 airspeedDerivativeSp float32 airspeedDerivative -float32 totalEnergyRateSp -float32 totalEnergyRate -float32 energyDistributionRateSp -float32 energyDistributionRate +float32 totalEnergyError +float32 energyDistributionError +float32 totalEnergyRateError +float32 energyDistributionRateError + +float32 throttle_sp +float32 pitch_sp uint8 mode diff --git a/src/lib/external_lgpl/tecs/tecs.cpp b/src/lib/external_lgpl/tecs/tecs.cpp index 6f049d4f52..9e47590fd2 100644 --- a/src/lib/external_lgpl/tecs/tecs.cpp +++ b/src/lib/external_lgpl/tecs/tecs.cpp @@ -309,12 +309,12 @@ void TECS::_update_throttle(float throttle_cruise, const math::Matrix<3,3> &rotM // Calculate total energy values _STE_error = _SPE_dem - _SPE_est + _SKE_dem - _SKE_est; float STEdot_dem = constrain((_SPEdot_dem + _SKEdot_dem), _STEdot_min, _STEdot_max); - float STEdot_error = STEdot_dem - _SPEdot - _SKEdot; + _STEdot_error = STEdot_dem - _SPEdot - _SKEdot; // Apply 0.5 second first order filter to STEdot_error // This is required to remove accelerometer noise from the measurement - STEdot_error = 0.2f * STEdot_error + 0.8f * _STEdotErrLast; - _STEdotErrLast = STEdot_error; + _STEdot_error = 0.2f * _STEdot_error + 0.8f * _STEdotErrLast; + _STEdotErrLast = _STEdot_error; // Calculate throttle demand // If underspeed condition is set, then demand full throttle @@ -342,7 +342,7 @@ void TECS::_update_throttle(float throttle_cruise, const math::Matrix<3,3> &rotM } // Calculate PD + FF throttle and constrain to avoid blow-up of the integrator later - _throttle_dem = (_STE_error + STEdot_error * _thrDamp) * K_STE2Thr + ff_throttle; + _throttle_dem = (_STE_error + _STEdot_error * _thrDamp) * K_STE2Thr + ff_throttle; _throttle_dem = constrain(_throttle_dem, _THRminf, _THRmaxf); // Rate limit PD + FF throttle @@ -438,11 +438,11 @@ void TECS::_update_pitch(void) // Calculate Specific Energy Balance demand, and error float SEB_dem = _SPE_dem * SPE_weighting - _SKE_dem * SKE_weighting; float SEBdot_dem = _SPEdot_dem * SPE_weighting - _SKEdot_dem * SKE_weighting; - float SEB_error = SEB_dem - (_SPE_est * SPE_weighting - _SKE_est * SKE_weighting); - float SEBdot_error = SEBdot_dem - (_SPEdot * SPE_weighting - _SKEdot * SKE_weighting); + _SEB_error = SEB_dem - (_SPE_est * SPE_weighting - _SKE_est * SKE_weighting); + _SEBdot_error = SEBdot_dem - (_SPEdot * SPE_weighting - _SKEdot * SKE_weighting); // Calculate integrator state, constraining input if pitch limits are exceeded - float integ7_input = SEB_error * _integGain; + float integ7_input = _SEB_error * _integGain; if (_pitch_dem_unc > _PITCHmaxf) { integ7_input = min(integ7_input, _PITCHmaxf - _pitch_dem_unc); @@ -460,7 +460,7 @@ void TECS::_update_pitch(void) // demand equal to the minimum value (which is )set by the mission plan during this mode). Otherwise the // integrator has to catch up before the nose can be raised to reduce speed during climbout. float gainInv = (_integ5_state * _timeConst * CONSTANTS_ONE_G); - float temp = SEB_error + SEBdot_error * _ptchDamp + SEBdot_dem * _timeConst; + float temp = _SEB_error + _SEBdot_error * _ptchDamp + SEBdot_dem * _timeConst; if (_climbOutDem) { temp += _PITCHminf * gainInv; @@ -598,18 +598,27 @@ void TECS::update_pitch_throttle(const math::Matrix<3,3> &rotMat, float pitch, f _tecs_state.mode = ECL_TECS_MODE_NORMAL; } - _tecs_state.hgt_dem = _hgt_dem_adj; - _tecs_state.hgt = _integ3_state; - _tecs_state.dhgt_dem = _hgt_rate_dem; - _tecs_state.dhgt = _integ2_state; - _tecs_state.spd_dem = _TAS_dem_adj; - _tecs_state.spd = _integ5_state; - _tecs_state.dspd = _vel_dot; - _tecs_state.ithr = _integ6_state; - _tecs_state.iptch = _integ7_state; - _tecs_state.thr = _throttle_dem; - _tecs_state.ptch = _pitch_dem; - _tecs_state.dspd_dem = _TAS_rate_dem; + _tecs_state.altitude_sp = _hgt_dem_adj; + _tecs_state.altitude_filtered = _integ3_state; + _tecs_state.altitude_rate_sp = _hgt_rate_dem; + _tecs_state.altitude_rate = _integ2_state; + + _tecs_state.airspeed_sp = _TAS_dem_adj; + _tecs_state.airspeed_rate_sp = _TAS_rate_dem; + _tecs_state.airspeed_filtered = _integ5_state; + _tecs_state.airspeed_rate = _vel_dot; + + _tecs_state.total_energy_error = _STE_error; + _tecs_state.energy_distribution_error = _SEB_error; + _tecs_state.total_energy_rate_error = _STEdot_error; + _tecs_state.energy_distribution_error = _SEBdot_error; + + _tecs_state.energy_error_integ = _integ6_state; + _tecs_state.energy_distribution_error_integ = _integ7_state; + + + _tecs_state.throttle_sp = _throttle_dem; + _tecs_state.pitch_sp = _pitch_dem; _update_pitch_throttle_last_usec = now; diff --git a/src/lib/external_lgpl/tecs/tecs.h b/src/lib/external_lgpl/tecs/tecs.h index 81a5e2d691..0fbda0fe9a 100644 --- a/src/lib/external_lgpl/tecs/tecs.h +++ b/src/lib/external_lgpl/tecs/tecs.h @@ -79,6 +79,10 @@ public: _SKE_est(0.0f), _SPEdot(0.0f), _SKEdot(0.0f), + _STE_error(0.0f), + _STEdot_error(0.0f), + _SEB_error(0.0f), + _SEBdot_error(0.0f), _airspeed_enabled(false), _states_initalized(false), _in_air(false), @@ -137,18 +141,22 @@ public: struct tecs_state { uint64_t timestamp; - float hgt; - float dhgt; - float hgt_dem; - float dhgt_dem; - float spd_dem; - float spd; - float dspd; - float ithr; - float iptch; - float thr; - float ptch; - float dspd_dem; + float altitude_filtered; + float altitude_sp; + float altitude_rate; + float altitude_rate_sp; + float airspeed_filtered; + float airspeed_sp; + float airspeed_rate; + float airspeed_rate_sp; + float energy_error_integ; + float energy_distribution_error_integ; + float total_energy_error; + float total_energy_rate_error; + float energy_distribution_error; + float energy_distribution_rate_error; + float throttle_sp; + float pitch_sp; enum ECL_TECS_MODE mode; }; @@ -376,6 +384,15 @@ private: // Specific energy error quantities float _STE_error; + // Energy error rate + float _STEdot_error; + + // Specific energy balance error + float _SEB_error; + + // Specific energy balance error rate + float _SEBdot_error; + // Time since last update of main TECS loop (seconds) float _DT; diff --git a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp index ceea71fca9..e14b84fb39 100644 --- a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp +++ b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp @@ -1920,22 +1920,25 @@ void FixedwingPositionControl::tecs_update_pitch_throttle(float alt_sp, float v_ break; } - t.altitudeSp = s.hgt_dem; - t.altitude_filtered = s.hgt; - t.airspeedSp = s.spd_dem; - t.airspeed_filtered = s.spd; + t.altitudeSp = s.altitude_sp; + t.altitude_filtered = s.altitude_filtered; + t.airspeedSp = s.airspeed_sp; + t.airspeed_filtered = s.airspeed_filtered; - t.flightPathAngleSp = s.dhgt_dem; - t.flightPathAngle = s.dhgt; - t.flightPathAngleFiltered = s.dhgt; + t.flightPathAngleSp = s.altitude_rate_sp; + t.flightPathAngle = s.altitude_rate; + t.flightPathAngleFiltered = s.altitude_rate; - t.airspeedDerivativeSp = s.dspd_dem; - t.airspeedDerivative = s.dspd; + t.airspeedDerivativeSp = s.airspeed_rate_sp; + t.airspeedDerivative = s.airspeed_rate; - t.totalEnergyRateSp = s.thr; - t.totalEnergyRate = s.ithr; - t.energyDistributionRateSp = s.ptch; - t.energyDistributionRate = s.iptch; + t.totalEnergyError = s.total_energy_error; + t.totalEnergyRateError = s.total_energy_rate_error; + t.energyDistributionError = s.energy_distribution_error; + t.energyDistributionRateError = s.energy_distribution_rate_error; + + t.throttle_sp = s.throttle_sp; + t.pitch_sp = s.pitch_sp; if (_tecs_status_pub > 0) { orb_publish(ORB_ID(tecs_status), _tecs_status_pub, &t); diff --git a/src/modules/sdlog2/sdlog2.c b/src/modules/sdlog2/sdlog2.c index 71ac96350a..c1c01ec06e 100644 --- a/src/modules/sdlog2/sdlog2.c +++ b/src/modules/sdlog2/sdlog2.c @@ -1863,15 +1863,16 @@ int sdlog2_thread_main(int argc, char *argv[]) log_msg.body.log_TECS.altitudeFiltered = buf.tecs_status.altitude_filtered; log_msg.body.log_TECS.flightPathAngleSp = buf.tecs_status.flightPathAngleSp; log_msg.body.log_TECS.flightPathAngle = buf.tecs_status.flightPathAngle; - log_msg.body.log_TECS.flightPathAngleFiltered = buf.tecs_status.flightPathAngleFiltered; log_msg.body.log_TECS.airspeedSp = buf.tecs_status.airspeedSp; log_msg.body.log_TECS.airspeedFiltered = buf.tecs_status.airspeed_filtered; log_msg.body.log_TECS.airspeedDerivativeSp = buf.tecs_status.airspeedDerivativeSp; log_msg.body.log_TECS.airspeedDerivative = buf.tecs_status.airspeedDerivative; - log_msg.body.log_TECS.totalEnergyRateSp = buf.tecs_status.totalEnergyRateSp; - log_msg.body.log_TECS.totalEnergyRate = buf.tecs_status.totalEnergyRate; - log_msg.body.log_TECS.energyDistributionRateSp = buf.tecs_status.energyDistributionRateSp; - log_msg.body.log_TECS.energyDistributionRate = buf.tecs_status.energyDistributionRate; + log_msg.body.log_TECS.totalEnergyError = buf.tecs_status.totalEnergyError; + log_msg.body.log_TECS.energyDistributionError = buf.tecs_status.energyDistributionError; + log_msg.body.log_TECS.totalEnergyRateError = buf.tecs_status.totalEnergyRateError; + log_msg.body.log_TECS.energyDistributionRateError = buf.tecs_status.energyDistributionRateError; + log_msg.body.log_TECS.throttle_sp = buf.tecs_status.throttle_sp; + log_msg.body.log_TECS.pitch_sp = buf.tecs_status.pitch_sp; log_msg.body.log_TECS.mode = (uint8_t)buf.tecs_status.mode; LOGBUFFER_WRITE_AND_COUNT(TECS); } diff --git a/src/modules/sdlog2/sdlog2_messages.h b/src/modules/sdlog2/sdlog2_messages.h index e75b6ca256..1363b37ba7 100644 --- a/src/modules/sdlog2/sdlog2_messages.h +++ b/src/modules/sdlog2/sdlog2_messages.h @@ -360,16 +360,16 @@ struct log_TECS_s { float altitudeFiltered; float flightPathAngleSp; float flightPathAngle; - float flightPathAngleFiltered; float airspeedSp; float airspeedFiltered; float airspeedDerivativeSp; float airspeedDerivative; - - float totalEnergyRateSp; - float totalEnergyRate; - float energyDistributionRateSp; - float energyDistributionRate; + float totalEnergyError; + float totalEnergyRateError; + float energyDistributionError; + float energyDistributionRateError; + float throttle_sp; + float pitch_sp; uint8_t mode; }; @@ -527,7 +527,7 @@ static const struct log_format_s log_formats[] = { LOG_FORMAT(GS0B, "BBBBBBBBBBBBBBBB", "s0,s1,s2,s3,s4,s5,s6,s7,s8,s9,s10,s11,s12,s13,s14,s15"), LOG_FORMAT(GS1A, "BBBBBBBBBBBBBBBB", "s0,s1,s2,s3,s4,s5,s6,s7,s8,s9,s10,s11,s12,s13,s14,s15"), LOG_FORMAT(GS1B, "BBBBBBBBBBBBBBBB", "s0,s1,s2,s3,s4,s5,s6,s7,s8,s9,s10,s11,s12,s13,s14,s15"), - LOG_FORMAT(TECS, "fffffffffffffB", "ASP,AF,FSP,F,FF,AsSP,AsF,AsDSP,AsD,TERSP,TER,EDRSP,EDR,M"), + LOG_FORMAT(TECS, "ffffffffffffffB", "ASP,AF,FSP,F,AsSP,AsF,AsDSP,AsD,EE,EDE,ERE,EDRE,Thr,Ptch,M"), LOG_FORMAT(WIND, "ffff", "X,Y,CovX,CovY"), LOG_FORMAT(ENCD, "qfqf", "cnt0,vel0,cnt1,vel1"), LOG_FORMAT(TSYN, "Q", "TimeOffset"), From 8cda0ec26669302b2e06513b9595b7a78c2257db Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 27 Jul 2015 17:07:13 +0200 Subject: [PATCH 17/26] EKF memory usage fix --- .../ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp | 2 +- src/modules/ekf_att_pos_estimator/module.mk | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp b/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp index 07b8c8dfd5..4588f3c901 100644 --- a/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp +++ b/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp @@ -1115,7 +1115,7 @@ int AttitudePositionEstimatorEKF::start() _estimator_task = task_spawn_cmd("ekf_att_pos_estimator", SCHED_DEFAULT, SCHED_PRIORITY_MAX - 40, - 7500, + 4800, (main_t)&AttitudePositionEstimatorEKF::task_main_trampoline, nullptr); diff --git a/src/modules/ekf_att_pos_estimator/module.mk b/src/modules/ekf_att_pos_estimator/module.mk index f519621137..2a582ebbb8 100644 --- a/src/modules/ekf_att_pos_estimator/module.mk +++ b/src/modules/ekf_att_pos_estimator/module.mk @@ -42,5 +42,5 @@ SRCS = ekf_att_pos_estimator_main.cpp \ estimator_22states.cpp \ estimator_utilities.cpp -EXTRACXXFLAGS = -Weffc++ -Wframe-larger-than=6100 +EXTRACXXFLAGS = -Weffc++ -Wframe-larger-than=3400 From 7cb97f1d39e3e50f3d2673dc2641a6c206938aa2 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 22 Aug 2015 14:20:21 +0200 Subject: [PATCH 18/26] top: Fix code style --- src/systemcmds/top/top.c | 123 +++++++++++++++++++++++---------------- 1 file changed, 73 insertions(+), 50 deletions(-) diff --git a/src/systemcmds/top/top.c b/src/systemcmds/top/top.c index 37e913040d..f82b4bda58 100644 --- a/src/systemcmds/top/top.c +++ b/src/systemcmds/top/top.c @@ -1,7 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2012, 2013 PX4 Development Team. All rights reserved. - * Author: Lorenz Meier + * Copyright (c) 2012, 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 @@ -36,8 +35,8 @@ * @file top.c * Tool similar to UNIX top command * @see http://en.wikipedia.org/wiki/Top_unix - * - * @author Lorenz Meier + * + * @author Lorenz Meier */ #include @@ -64,23 +63,40 @@ static const char * tstate_name(const tstate_t s) { switch (s) { - case TSTATE_TASK_INVALID: return "init"; + case TSTATE_TASK_INVALID: + return "init"; - case TSTATE_TASK_PENDING: return "PEND"; - case TSTATE_TASK_READYTORUN: return "READY"; - case TSTATE_TASK_RUNNING: return "RUN"; + case TSTATE_TASK_PENDING: + return "PEND"; - case TSTATE_TASK_INACTIVE: return "inact"; - case TSTATE_WAIT_SEM: return "w:sem"; + case TSTATE_TASK_READYTORUN: + return "READY"; + + case TSTATE_TASK_RUNNING: + return "RUN"; + + case TSTATE_TASK_INACTIVE: + return "inact"; + + case TSTATE_WAIT_SEM: + return "w:sem"; #ifndef CONFIG_DISABLE_SIGNALS - case TSTATE_WAIT_SIG: return "w:sig"; + + case TSTATE_WAIT_SIG: + return "w:sig"; #endif #ifndef CONFIG_DISABLE_MQUEUE - case TSTATE_WAIT_MQNOTEMPTY: return "w:mqe"; - case TSTATE_WAIT_MQNOTFULL: return "w:mqf"; + + case TSTATE_WAIT_MQNOTEMPTY: + return "w:mqe"; + + case TSTATE_WAIT_MQNOTFULL: + return "w:mqf"; #endif #ifdef CONFIG_PAGING - case TSTATE_WAIT_PAGEFILL: return "w:pgf"; + + case TSTATE_WAIT_PAGEFILL: + return "w:pgf"; #endif default: @@ -102,8 +118,9 @@ top_main(void) uint64_t last_times[CONFIG_MAX_TASKS]; float curr_loads[CONFIG_MAX_TASKS]; - for (int t = 0; t < CONFIG_MAX_TASKS; t++) + for (int t = 0; t < CONFIG_MAX_TASKS; t++) { last_times[t] = 0; + } float interval_time_ms_inv = 0.f; @@ -118,8 +135,9 @@ top_main(void) curr_time_us = hrt_absolute_time(); idle_time_us = system_load.tasks[0].total_runtime; - if (new_time > interval_start_time) + if (new_time > interval_start_time) { interval_time_ms_inv = 1.f / ((float)((new_time - interval_start_time) / 1000)); + } running_count = 0; blocked_count = 0; @@ -155,19 +173,22 @@ top_main(void) } interval_runtime = (system_load.tasks[i].valid && last_times[i] > 0 && - system_load.tasks[i].total_runtime > last_times[i]) - ? (system_load.tasks[i].total_runtime - last_times[i]) / 1000 - : 0; + system_load.tasks[i].total_runtime > last_times[i]) + ? (system_load.tasks[i].total_runtime - last_times[i]) / 1000 + : 0; last_times[i] = system_load.tasks[i].total_runtime; if (system_load.tasks[i].valid && (new_time > interval_start_time)) { curr_loads[i] = interval_runtime * interval_time_ms_inv; - if (i > 0) + if (i > 0) { total_user_time += interval_runtime; - } else + } + + } else { curr_loads[i] = 0; + } } for (i = 0; i < CONFIG_MAX_TASKS; i++) { @@ -182,63 +203,65 @@ top_main(void) /* this can happen if one tasks total runtime was not computed correctly by the scheduler instrumentation TODO */ - if (task_load > (1.f - idle)) + if (task_load > (1.f - idle)) { task_load = (1.f - idle); + } sched_load = 1.f - idle - task_load; /* print system information */ printf("\033[H"); /* move cursor home and clear screen */ printf(CL "Processes: %d total, %d running, %d sleeping\n", - system_load.total_count, - running_count, - blocked_count); + system_load.total_count, + running_count, + blocked_count); printf(CL "CPU usage: %.2f%% tasks, %.2f%% sched, %.2f%% idle\n", - (double)(task_load * 100.f), - (double)(sched_load * 100.f), - (double)(idle * 100.f)); + (double)(task_load * 100.f), + (double)(sched_load * 100.f), + (double)(idle * 100.f)); printf(CL "Uptime: %.3fs total, %.3fs idle\n\n", - (double)curr_time_us / 1000000.d, - (double)idle_time_us / 1000000.d); + (double)curr_time_us / 1000000.d, + (double)idle_time_us / 1000000.d); /* header for task list */ printf(CL "%4s %*-s %8s %6s %11s %10s %-6s\n", - "PID", - CONFIG_TASK_NAME_SIZE, "COMMAND", - "CPU(ms)", - "CPU(%)", - "USED/STACK", - "PRIO(BASE)", + "PID", + CONFIG_TASK_NAME_SIZE, "COMMAND", + "CPU(ms)", + "CPU(%)", + "USED/STACK", + "PRIO(BASE)", #if CONFIG_RR_INTERVAL > 0 - "TSLICE" + "TSLICE" #else - "STATE" + "STATE" #endif - ); + ); } unsigned stack_size = (uintptr_t)system_load.tasks[i].tcb->adj_stack_ptr - - (uintptr_t)system_load.tasks[i].tcb->stack_alloc_ptr; + (uintptr_t)system_load.tasks[i].tcb->stack_alloc_ptr; unsigned stack_free = 0; uint8_t *stack_sweeper = (uint8_t *)system_load.tasks[i].tcb->stack_alloc_ptr; while (stack_free < stack_size) { - if (*stack_sweeper++ != 0xff) + if (*stack_sweeper++ != 0xff) { break; + } stack_free++; } printf(CL "%4d %*-s %8lld %2d.%03d %5u/%5u %3u (%3u) ", - system_load.tasks[i].tcb->pid, - CONFIG_TASK_NAME_SIZE, system_load.tasks[i].tcb->name, - (system_load.tasks[i].total_runtime / 1000), - (int)(curr_loads[i] * 100.0f), - (int)((curr_loads[i] * 100.0f - (int)(curr_loads[i] * 100.0f)) * 1000), - stack_size - stack_free, - stack_size, - system_load.tasks[i].tcb->sched_priority, - system_load.tasks[i].tcb->base_priority); + system_load.tasks[i].tcb->pid, + CONFIG_TASK_NAME_SIZE, system_load.tasks[i].tcb->name, + (system_load.tasks[i].total_runtime / 1000), + (int)(curr_loads[i] * 100.0f), + (int)((curr_loads[i] * 100.0f - (int)(curr_loads[i] * 100.0f)) * 1000), + stack_size - stack_free, + stack_size, + system_load.tasks[i].tcb->sched_priority, + system_load.tasks[i].tcb->base_priority); #if CONFIG_RR_INTERVAL > 0 /* print scheduling info with RR time slice */ From b70c9a84c621af6d0a48c0f1ba99e9385426e4ff Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 22 Aug 2015 15:22:28 +0200 Subject: [PATCH 19/26] systemlib: Add ability to dump memory usage --- src/modules/systemlib/module.mk | 1 + src/modules/systemlib/printload.c | 278 ++++++++++++++++++++++++++++++ src/modules/systemlib/printload.h | 59 +++++++ src/systemcmds/top/top.c | 230 ++---------------------- 4 files changed, 354 insertions(+), 214 deletions(-) create mode 100644 src/modules/systemlib/printload.c create mode 100644 src/modules/systemlib/printload.h diff --git a/src/modules/systemlib/module.mk b/src/modules/systemlib/module.mk index f2499bbb13..b531330ab3 100644 --- a/src/modules/systemlib/module.mk +++ b/src/modules/systemlib/module.mk @@ -42,6 +42,7 @@ SRCS = err.c \ bson/tinybson.c \ conversions.c \ cpuload.c \ + printload.c \ getopt_long.c \ up_cxxinitialize.c \ pid/pid.c \ diff --git a/src/modules/systemlib/printload.c b/src/modules/systemlib/printload.c new file mode 100644 index 0000000000..e40791fe89 --- /dev/null +++ b/src/modules/systemlib/printload.c @@ -0,0 +1,278 @@ +/**************************************************************************** + * + * Copyright (c) 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 printload.c + * + * Print the current system load. + * + * @author Lorenz Meier + */ + +#include +#include + +#include +#include +#include + +extern struct system_load_s system_load; + +#define CL "\033[K" // clear line + +void init_print_load_s(uint64_t t, struct print_load_s *s) +{ + + s->total_user_time = 0; + + s->running_count = 0; + s->blocked_count = 0; + + s->new_time = t; + s->interval_start_time = t; + + for (int i = 0; i < CONFIG_MAX_TASKS; i++) { + s->last_times[i] = 0; + } + + s->interval_time_ms_inv = 0.f; +} + +static const char * +tstate_name(const tstate_t s) +{ + switch (s) { + case TSTATE_TASK_INVALID: + return "init"; + + case TSTATE_TASK_PENDING: + return "PEND"; + + case TSTATE_TASK_READYTORUN: + return "READY"; + + case TSTATE_TASK_RUNNING: + return "RUN"; + + case TSTATE_TASK_INACTIVE: + return "inact"; + + case TSTATE_WAIT_SEM: + return "w:sem"; +#ifndef CONFIG_DISABLE_SIGNALS + + case TSTATE_WAIT_SIG: + return "w:sig"; +#endif +#ifndef CONFIG_DISABLE_MQUEUE + + case TSTATE_WAIT_MQNOTEMPTY: + return "w:mqe"; + + case TSTATE_WAIT_MQNOTFULL: + return "w:mqf"; +#endif +#ifdef CONFIG_PAGING + + case TSTATE_WAIT_PAGEFILL: + return "w:pgf"; +#endif + + default: + return "ERROR"; + } +} + +void print_load(uint64_t t, int fd, struct print_load_s *print_state) +{ + print_state->new_time = t; + + int i; + uint64_t curr_time_us; + uint64_t idle_time_us; + char *clear_line = '\0'; + + /* print system information */ + if (fd == 1) { + dprintf(fd, "\033[H"); /* move cursor home and clear screen */ + clear_line = CL; + } + + curr_time_us = t; + idle_time_us = system_load.tasks[0].total_runtime; + + if (print_state->new_time > print_state->interval_start_time) { + print_state->interval_time_ms_inv = 1.f / ((float)((print_state->new_time - print_state->interval_start_time) / 1000)); + } + + print_state->running_count = 0; + print_state->blocked_count = 0; + print_state->total_user_time = 0; + + for (i = 0; i < CONFIG_MAX_TASKS; i++) { + uint64_t interval_runtime; + + if (system_load.tasks[i].valid) { + switch (system_load.tasks[i].tcb->task_state) { + case TSTATE_TASK_PENDING: + case TSTATE_TASK_READYTORUN: + case TSTATE_TASK_RUNNING: + print_state->running_count++; + break; + + case TSTATE_TASK_INVALID: + case TSTATE_TASK_INACTIVE: + case TSTATE_WAIT_SEM: +#ifndef CONFIG_DISABLE_SIGNALS + case TSTATE_WAIT_SIG: +#endif +#ifndef CONFIG_DISABLE_MQUEUE + case TSTATE_WAIT_MQNOTEMPTY: + case TSTATE_WAIT_MQNOTFULL: +#endif +#ifdef CONFIG_PAGING + case TSTATE_WAIT_PAGEFILL: +#endif + print_state->blocked_count++; + break; + } + } + + interval_runtime = (system_load.tasks[i].valid && print_state->last_times[i] > 0 && + system_load.tasks[i].total_runtime > print_state->last_times[i]) + ? (system_load.tasks[i].total_runtime - print_state->last_times[i]) / 1000 + : 0; + + print_state->last_times[i] = system_load.tasks[i].total_runtime; + + if (system_load.tasks[i].valid && (print_state->new_time > print_state->interval_start_time)) { + print_state->curr_loads[i] = interval_runtime * print_state->interval_time_ms_inv; + + if (i > 0) { + print_state->total_user_time += interval_runtime; + } + + } else { + print_state->curr_loads[i] = 0; + } + } + + for (i = 0; i < CONFIG_MAX_TASKS; i++) { + if (system_load.tasks[i].valid && (print_state->new_time > print_state->interval_start_time)) { + if (system_load.tasks[i].tcb->pid == 0) { + float idle; + float task_load; + float sched_load; + + idle = print_state->curr_loads[0]; + task_load = (float)(print_state->total_user_time) * print_state->interval_time_ms_inv; + + /* this can happen if one tasks total runtime was not computed + correctly by the scheduler instrumentation TODO */ + if (task_load > (1.f - idle)) { + task_load = (1.f - idle); + } + + sched_load = 1.f - idle - task_load; + + dprintf(fd, "%sProcesses: %d total, %d running, %d sleeping\n", + clear_line, + system_load.total_count, + print_state->running_count, + print_state->blocked_count); + dprintf(fd, "%sCPU usage: %.2f%% tasks, %.2f%% sched, %.2f%% idle\n", + clear_line, + (double)(task_load * 100.f), + (double)(sched_load * 100.f), + (double)(idle * 100.f)); + dprintf(fd, "%sUptime: %.3fs total, %.3fs idle\n%s\n", + clear_line, + (double)curr_time_us / 1000000.d, + (double)idle_time_us / 1000000.d, + clear_line); + + /* header for task list */ + dprintf(fd, "%s%4s %*-s %8s %6s %11s %10s %-6s\n", + clear_line, + "PID", + CONFIG_TASK_NAME_SIZE, "COMMAND", + "CPU(ms)", + "CPU(%)", + "USED/STACK", + "PRIO(BASE)", +#if CONFIG_RR_INTERVAL > 0 + "TSLICE" +#else + "STATE" +#endif + ); + } + + unsigned stack_size = (uintptr_t)system_load.tasks[i].tcb->adj_stack_ptr - + (uintptr_t)system_load.tasks[i].tcb->stack_alloc_ptr; + unsigned stack_free = 0; + uint8_t *stack_sweeper = (uint8_t *)system_load.tasks[i].tcb->stack_alloc_ptr; + + while (stack_free < stack_size) { + if (*stack_sweeper++ != 0xff) { + break; + } + + stack_free++; + } + + dprintf(fd, "%s%4d %*-s %8lld %2d.%03d %5u/%5u %3u (%3u) ", + clear_line, + system_load.tasks[i].tcb->pid, + CONFIG_TASK_NAME_SIZE, system_load.tasks[i].tcb->name, + (system_load.tasks[i].total_runtime / 1000), + (int)(print_state->curr_loads[i] * 100.0f), + (int)((print_state->curr_loads[i] * 100.0f - (int)(print_state->curr_loads[i] * 100.0f)) * 1000), + stack_size - stack_free, + stack_size, + system_load.tasks[i].tcb->sched_priority, + system_load.tasks[i].tcb->base_priority); + +#if CONFIG_RR_INTERVAL > 0 + /* print scheduling info with RR time slice */ + dprintf(fd, " %6d\n", system_load.tasks[i].tcb->timeslice); +#else + // print task state instead + dprintf(fd, " %-6s\n", tstate_name(system_load.tasks[i].tcb->task_state)); +#endif + } + } + + print_state->interval_start_time = print_state->new_time; +} + diff --git a/src/modules/systemlib/printload.h b/src/modules/systemlib/printload.h new file mode 100644 index 0000000000..f0719d2106 --- /dev/null +++ b/src/modules/systemlib/printload.h @@ -0,0 +1,59 @@ +/**************************************************************************** + * + * Copyright (c) 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 printload.h + * + * Print the current system load. + * + * @author Lorenz Meier + */ + +#include + +__EXPORT struct print_load_s { + uint64_t total_user_time; + + int running_count; + int blocked_count; + + uint64_t new_time; + uint64_t interval_start_time; + uint64_t last_times[CONFIG_MAX_TASKS]; + float curr_loads[CONFIG_MAX_TASKS]; + float interval_time_ms_inv; +}; + +__EXPORT void init_print_load_s(uint64_t t, struct print_load_s *s); + +__EXPORT void print_load(uint64_t t, int fd, struct print_load_s *print_state); diff --git a/src/systemcmds/top/top.c b/src/systemcmds/top/top.c index f82b4bda58..a4846885c5 100644 --- a/src/systemcmds/top/top.c +++ b/src/systemcmds/top/top.c @@ -48,232 +48,34 @@ #include #include +#include #include -#define CL "\033[K" // clear line - /** * Start the top application. */ -__EXPORT int top_main(void); - -extern struct system_load_s system_load; - -static const char * -tstate_name(const tstate_t s) -{ - switch (s) { - case TSTATE_TASK_INVALID: - return "init"; - - case TSTATE_TASK_PENDING: - return "PEND"; - - case TSTATE_TASK_READYTORUN: - return "READY"; - - case TSTATE_TASK_RUNNING: - return "RUN"; - - case TSTATE_TASK_INACTIVE: - return "inact"; - - case TSTATE_WAIT_SEM: - return "w:sem"; -#ifndef CONFIG_DISABLE_SIGNALS - - case TSTATE_WAIT_SIG: - return "w:sig"; -#endif -#ifndef CONFIG_DISABLE_MQUEUE - - case TSTATE_WAIT_MQNOTEMPTY: - return "w:mqe"; - - case TSTATE_WAIT_MQNOTFULL: - return "w:mqf"; -#endif -#ifdef CONFIG_PAGING - - case TSTATE_WAIT_PAGEFILL: - return "w:pgf"; -#endif - - default: - return "ERROR"; - } -} +__EXPORT int top_main(int argc, char *argv[]); int -top_main(void) +top_main(int argc, char *argv[]) { - uint64_t total_user_time = 0; + hrt_abstime curr_time = hrt_absolute_time(); - int running_count = 0; - int blocked_count = 0; - - uint64_t new_time = hrt_absolute_time(); - uint64_t interval_start_time = new_time; - - uint64_t last_times[CONFIG_MAX_TASKS]; - float curr_loads[CONFIG_MAX_TASKS]; - - for (int t = 0; t < CONFIG_MAX_TASKS; t++) { - last_times[t] = 0; - } - - float interval_time_ms_inv = 0.f; + struct print_load_s load; + init_print_load_s(curr_time, &load); /* clear screen */ - printf("\033[2J"); + dprintf(1, "\033[2J\n"); + + if (argc > 1 && !strcmp(argv[1], "once")) { + print_load(curr_time, 1, &load); + sleep(1); + print_load(hrt_absolute_time(), 1, &load); + return 0; + } for (;;) { - int i; - uint64_t curr_time_us; - uint64_t idle_time_us; - - curr_time_us = hrt_absolute_time(); - idle_time_us = system_load.tasks[0].total_runtime; - - if (new_time > interval_start_time) { - interval_time_ms_inv = 1.f / ((float)((new_time - interval_start_time) / 1000)); - } - - running_count = 0; - blocked_count = 0; - total_user_time = 0; - - for (i = 0; i < CONFIG_MAX_TASKS; i++) { - uint64_t interval_runtime; - - if (system_load.tasks[i].valid) { - switch (system_load.tasks[i].tcb->task_state) { - case TSTATE_TASK_PENDING: - case TSTATE_TASK_READYTORUN: - case TSTATE_TASK_RUNNING: - running_count++; - break; - - case TSTATE_TASK_INVALID: - case TSTATE_TASK_INACTIVE: - case TSTATE_WAIT_SEM: -#ifndef CONFIG_DISABLE_SIGNALS - case TSTATE_WAIT_SIG: -#endif -#ifndef CONFIG_DISABLE_MQUEUE - case TSTATE_WAIT_MQNOTEMPTY: - case TSTATE_WAIT_MQNOTFULL: -#endif -#ifdef CONFIG_PAGING - case TSTATE_WAIT_PAGEFILL: -#endif - blocked_count++; - break; - } - } - - interval_runtime = (system_load.tasks[i].valid && last_times[i] > 0 && - system_load.tasks[i].total_runtime > last_times[i]) - ? (system_load.tasks[i].total_runtime - last_times[i]) / 1000 - : 0; - - last_times[i] = system_load.tasks[i].total_runtime; - - if (system_load.tasks[i].valid && (new_time > interval_start_time)) { - curr_loads[i] = interval_runtime * interval_time_ms_inv; - - if (i > 0) { - total_user_time += interval_runtime; - } - - } else { - curr_loads[i] = 0; - } - } - - for (i = 0; i < CONFIG_MAX_TASKS; i++) { - if (system_load.tasks[i].valid && (new_time > interval_start_time)) { - if (system_load.tasks[i].tcb->pid == 0) { - float idle; - float task_load; - float sched_load; - - idle = curr_loads[0]; - task_load = (float)(total_user_time) * interval_time_ms_inv; - - /* this can happen if one tasks total runtime was not computed - correctly by the scheduler instrumentation TODO */ - if (task_load > (1.f - idle)) { - task_load = (1.f - idle); - } - - sched_load = 1.f - idle - task_load; - - /* print system information */ - printf("\033[H"); /* move cursor home and clear screen */ - printf(CL "Processes: %d total, %d running, %d sleeping\n", - system_load.total_count, - running_count, - blocked_count); - printf(CL "CPU usage: %.2f%% tasks, %.2f%% sched, %.2f%% idle\n", - (double)(task_load * 100.f), - (double)(sched_load * 100.f), - (double)(idle * 100.f)); - printf(CL "Uptime: %.3fs total, %.3fs idle\n\n", - (double)curr_time_us / 1000000.d, - (double)idle_time_us / 1000000.d); - - /* header for task list */ - printf(CL "%4s %*-s %8s %6s %11s %10s %-6s\n", - "PID", - CONFIG_TASK_NAME_SIZE, "COMMAND", - "CPU(ms)", - "CPU(%)", - "USED/STACK", - "PRIO(BASE)", -#if CONFIG_RR_INTERVAL > 0 - "TSLICE" -#else - "STATE" -#endif - ); - } - - unsigned stack_size = (uintptr_t)system_load.tasks[i].tcb->adj_stack_ptr - - (uintptr_t)system_load.tasks[i].tcb->stack_alloc_ptr; - unsigned stack_free = 0; - uint8_t *stack_sweeper = (uint8_t *)system_load.tasks[i].tcb->stack_alloc_ptr; - - while (stack_free < stack_size) { - if (*stack_sweeper++ != 0xff) { - break; - } - - stack_free++; - } - - printf(CL "%4d %*-s %8lld %2d.%03d %5u/%5u %3u (%3u) ", - system_load.tasks[i].tcb->pid, - CONFIG_TASK_NAME_SIZE, system_load.tasks[i].tcb->name, - (system_load.tasks[i].total_runtime / 1000), - (int)(curr_loads[i] * 100.0f), - (int)((curr_loads[i] * 100.0f - (int)(curr_loads[i] * 100.0f)) * 1000), - stack_size - stack_free, - stack_size, - system_load.tasks[i].tcb->sched_priority, - system_load.tasks[i].tcb->base_priority); - -#if CONFIG_RR_INTERVAL > 0 - /* print scheduling info with RR time slice */ - printf(" %6d\n", system_load.tasks[i].tcb->timeslice); -#else - // print task state instead - printf(" %-6s\n", tstate_name(system_load.tasks[i].tcb->task_state)); -#endif - } - } - - interval_start_time = new_time; + print_load(curr_time, 1, &load); /* Sleep 200 ms waiting for user input five times ~ 1s */ for (int k = 0; k < 5; k++) { @@ -302,7 +104,7 @@ top_main(void) usleep(200000); } - new_time = hrt_absolute_time(); + curr_time = hrt_absolute_time(); } return OK; From c264d6bb99792fe166bf045dc28a79890137d226 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 22 Aug 2015 15:27:05 +0200 Subject: [PATCH 20/26] SDLOG2: Store load / mem use post-flight --- src/modules/sdlog2/sdlog2.c | 15 +++++++++++++++ 1 file changed, 15 insertions(+) diff --git a/src/modules/sdlog2/sdlog2.c b/src/modules/sdlog2/sdlog2.c index c1c01ec06e..ea843f9202 100644 --- a/src/modules/sdlog2/sdlog2.c +++ b/src/modules/sdlog2/sdlog2.c @@ -105,6 +105,7 @@ #include #include #include +#include #include #include @@ -727,9 +728,16 @@ void sdlog2_start_log() } /* write all performance counters */ + hrt_abstime curr_time = hrt_absolute_time(); + struct print_load_s load; int perf_fd = open_perf_file("preflight"); + init_print_load_s(curr_time, &load); + print_load(curr_time, perf_fd, &load); dprintf(perf_fd, "PERFORMANCE COUNTERS PRE-FLIGHT\n\n"); perf_print_all(perf_fd); + dprintf(perf_fd, "\nLOAD PRE-FLIGHT\n\n"); + usleep(500 * 1000); + print_load(hrt_absolute_time(), perf_fd, &load); close(perf_fd); /* reset performance counters to get in-flight min and max values in post flight log */ @@ -765,8 +773,15 @@ void sdlog2_stop_log() /* write all performance counters */ int perf_fd = open_perf_file("postflight"); + hrt_abstime curr_time = hrt_absolute_time(); dprintf(perf_fd, "PERFORMANCE COUNTERS POST-FLIGHT\n\n"); perf_print_all(perf_fd); + struct print_load_s load; + dprintf(perf_fd, "\nLOAD POST-FLIGHT\n\n"); + init_print_load_s(curr_time, &load); + print_load(curr_time, perf_fd, &load); + sleep(1); + print_load(hrt_absolute_time(), perf_fd, &load); close(perf_fd); /* free log writer performance counter */ From 86e2e7e2a6d18403f063685d124880112f3e551a Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 22 Aug 2015 19:21:54 +0200 Subject: [PATCH 21/26] print load: Initialize to empty string, not null pointer --- src/modules/systemlib/printload.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/systemlib/printload.c b/src/modules/systemlib/printload.c index e40791fe89..23dde1ce44 100644 --- a/src/modules/systemlib/printload.c +++ b/src/modules/systemlib/printload.c @@ -120,7 +120,7 @@ void print_load(uint64_t t, int fd, struct print_load_s *print_state) int i; uint64_t curr_time_us; uint64_t idle_time_us; - char *clear_line = '\0'; + char *clear_line = ""; /* print system information */ if (fd == 1) { From e1aaaa1f1896fad5b422b0b8fe3c8c1edad7625c Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 22 Aug 2015 20:31:45 +0200 Subject: [PATCH 22/26] MAVLink: Start app using pre-configured streams to save some script execution overhead. Frees 1K during app startup, which should help FMUv1 considerably. --- ROMFS/px4fmu_common/init.d/rc.usb | 17 +---------------- src/modules/mavlink/mavlink_main.cpp | 24 ++++++++++++++++++++++-- src/modules/mavlink/mavlink_main.h | 3 ++- 3 files changed, 25 insertions(+), 19 deletions(-) diff --git a/ROMFS/px4fmu_common/init.d/rc.usb b/ROMFS/px4fmu_common/init.d/rc.usb index 027d2aca5d..4a157a683f 100644 --- a/ROMFS/px4fmu_common/init.d/rc.usb +++ b/ROMFS/px4fmu_common/init.d/rc.usb @@ -3,22 +3,7 @@ # USB MAVLink start # -mavlink start -r 800000 -d /dev/ttyACM0 -x -# Enable a number of interesting streams we want via USB -mavlink stream -d /dev/ttyACM0 -s PARAM_VALUE -r 300 -mavlink stream -d /dev/ttyACM0 -s MISSION_ITEM -r 50 -mavlink stream -d /dev/ttyACM0 -s NAMED_VALUE_FLOAT -r 10 -mavlink stream -d /dev/ttyACM0 -s OPTICAL_FLOW_RAD -r 10 -mavlink stream -d /dev/ttyACM0 -s VFR_HUD -r 20 -mavlink stream -d /dev/ttyACM0 -s ATTITUDE -r 100 -mavlink stream -d /dev/ttyACM0 -s ACTUATOR_CONTROL_TARGET0 -r 30 -mavlink stream -d /dev/ttyACM0 -s RC_CHANNELS_RAW -r 5 -mavlink stream -d /dev/ttyACM0 -s SERVO_OUTPUT_RAW_0 -r 20 -mavlink stream -d /dev/ttyACM0 -s POSITION_TARGET_GLOBAL_INT -r 10 -mavlink stream -d /dev/ttyACM0 -s LOCAL_POSITION_NED -r 30 -mavlink stream -d /dev/ttyACM0 -s MANUAL_CONTROL -r 5 -mavlink stream -d /dev/ttyACM0 -s HIGHRES_IMU -r 100 -mavlink stream -d /dev/ttyACM0 -s GPS_RAW_INT -r 20 +mavlink start -r 800000 -d /dev/ttyACM0 -m config -x # Exit shell to make it available to MAVLink exit diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp index f2dcd33d79..f172dfbeae 100644 --- a/src/modules/mavlink/mavlink_main.cpp +++ b/src/modules/mavlink/mavlink_main.cpp @@ -1353,6 +1353,8 @@ Mavlink::task_main(int argc, char *argv[]) _mode = MAVLINK_MODE_ONBOARD; } else if (strcmp(optarg, "osd") == 0) { _mode = MAVLINK_MODE_OSD; + } else if (strcmp(optarg, "config") == 0) { + _mode = MAVLINK_MODE_CONFIG; } break; @@ -1449,8 +1451,6 @@ Mavlink::task_main(int argc, char *argv[]) /* start the MAVLink receiver */ _receive_thread = MavlinkReceiver::receive_start(this); - _task_running = true; - MavlinkOrbSubscription *param_sub = add_orb_subscription(ORB_ID(parameter_update)); uint64_t param_time = 0; MavlinkOrbSubscription *status_sub = add_orb_subscription(ORB_ID(vehicle_status)); @@ -1537,6 +1537,23 @@ Mavlink::task_main(int argc, char *argv[]) configure_stream("RC_CHANNELS_RAW", 5.0f); break; + case MAVLINK_MODE_CONFIG: + // Enable a number of interesting streams we want via USB + configure_stream("PARAM_VALUE", 300.0f); + configure_stream("MISSION_ITEM", 50.0f); + configure_stream("NAMED_VALUE_FLOAT", 10.0f); + configure_stream("OPTICAL_FLOW_RAD", 10.0f); + configure_stream("VFR_HUD", 20.0f); + configure_stream("ATTITUDE", 100.0f); + configure_stream("ACTUATOR_CONTROL_TARGET0", 30.0f); + configure_stream("RC_CHANNELS_RAW", 5.0f); + configure_stream("SERVO_OUTPUT_RAW_0", 20.0f); + configure_stream("POSITION_TARGET_GLOBAL_INT", 10.0f); + configure_stream("LOCAL_POSITION_NED", 30.0f); + configure_stream("MANUAL_CONTROL", 5.0f); + configure_stream("HIGHRES_IMU", 100.0f); + configure_stream("GPS_RAW_INT", 20.0f); + default: break; } @@ -1703,6 +1720,9 @@ Mavlink::task_main(int argc, char *argv[]) } perf_end(_loop_perf); + + /* confirm task running only once fully initialized */ + _task_running = true; } delete _subscribe_to_stream; diff --git a/src/modules/mavlink/mavlink_main.h b/src/modules/mavlink/mavlink_main.h index a484558579..455d9c796e 100644 --- a/src/modules/mavlink/mavlink_main.h +++ b/src/modules/mavlink/mavlink_main.h @@ -129,7 +129,8 @@ public: MAVLINK_MODE_NORMAL = 0, MAVLINK_MODE_CUSTOM, MAVLINK_MODE_ONBOARD, - MAVLINK_MODE_OSD + MAVLINK_MODE_OSD, + MAVLINK_MODE_CONFIG }; void set_mode(enum MAVLINK_MODE); From be638598936e7d0654a00384a53ac64c3654c685 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 22 Aug 2015 20:44:29 +0200 Subject: [PATCH 23/26] FMU driver: Remove excessive task space --- src/drivers/px4fmu/fmu.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/drivers/px4fmu/fmu.cpp b/src/drivers/px4fmu/fmu.cpp index f2ec0285be..8109ec7603 100644 --- a/src/drivers/px4fmu/fmu.cpp +++ b/src/drivers/px4fmu/fmu.cpp @@ -355,7 +355,7 @@ PX4FMU::init() _task = task_spawn_cmd("fmuservo", SCHED_DEFAULT, SCHED_PRIORITY_DEFAULT, - 1600, + 1200, (main_t)&PX4FMU::task_main_trampoline, nullptr); From 10047964e309035e6efc5c0e2fdf2d1d9fe5bdc3 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 22 Aug 2015 21:12:26 +0200 Subject: [PATCH 24/26] Fix compile error on load print struct --- src/modules/systemlib/printload.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/systemlib/printload.h b/src/modules/systemlib/printload.h index f0719d2106..2e0bfb62a8 100644 --- a/src/modules/systemlib/printload.h +++ b/src/modules/systemlib/printload.h @@ -41,7 +41,7 @@ #include -__EXPORT struct print_load_s { +struct __EXPORT print_load_s { uint64_t total_user_time; int running_count; From 808d817cfe14a34e48e69f9e140d6c45be3c2741 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 22 Aug 2015 22:10:29 +0200 Subject: [PATCH 25/26] Add missing decls to print load --- src/modules/systemlib/printload.h | 6 ++++++ 1 file changed, 6 insertions(+) diff --git a/src/modules/systemlib/printload.h b/src/modules/systemlib/printload.h index 2e0bfb62a8..3199059004 100644 --- a/src/modules/systemlib/printload.h +++ b/src/modules/systemlib/printload.h @@ -39,6 +39,10 @@ * @author Lorenz Meier */ +#pragma once + +__BEGIN_DECLS + #include struct __EXPORT print_load_s { @@ -57,3 +61,5 @@ struct __EXPORT print_load_s { __EXPORT void init_print_load_s(uint64_t t, struct print_load_s *s); __EXPORT void print_load(uint64_t t, int fd, struct print_load_s *print_state); + +__END_DECLS From 07a88e9e866d461930534ed5ad77f03d2b70dadd Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 22 Aug 2015 22:16:03 +0200 Subject: [PATCH 26/26] print load: Fix struct visibility --- src/modules/systemlib/printload.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/systemlib/printload.h b/src/modules/systemlib/printload.h index 3199059004..09f8a562b9 100644 --- a/src/modules/systemlib/printload.h +++ b/src/modules/systemlib/printload.h @@ -45,7 +45,7 @@ __BEGIN_DECLS #include -struct __EXPORT print_load_s { +struct print_load_s { uint64_t total_user_time; int running_count;