From b7847e13a689440515c80d7ee1b197a58021ca48 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Sat, 8 Aug 2015 09:25:01 +0200 Subject: [PATCH 01/44] mtecs: quick hack: only write tecs topic on nuttx, need to solve this cleanly with inheritance. Introduced own mode enum. --- src/modules/fw_pos_control_l1/mtecs/mTecs.cpp | 24 +++++++++++++------ src/modules/fw_pos_control_l1/mtecs/mTecs.h | 16 +++++++++++++ 2 files changed, 33 insertions(+), 7 deletions(-) diff --git a/src/modules/fw_pos_control_l1/mtecs/mTecs.cpp b/src/modules/fw_pos_control_l1/mtecs/mTecs.cpp index c07956fd6e..36ce58f2ff 100644 --- a/src/modules/fw_pos_control_l1/mtecs/mTecs.cpp +++ b/src/modules/fw_pos_control_l1/mtecs/mTecs.cpp @@ -52,7 +52,9 @@ mTecs::mTecs() : _mTecsEnabled(this, "ENABLED"), _airspeedMin(this, "FW_AIRSPD_MIN", false), /* Publications */ +#if defined(__PX4_NUTTX) _status(ORB_ID(tecs_status), &getPublications()), +#endif // defined(__PX4_NUTTX) /* control blocks */ _controlTotalEnergy(this, "THR"), _controlEnergyDistribution(this, "PIT", true), @@ -107,9 +109,11 @@ int mTecs::updateAltitudeSpeed(float flightPathAngle, float altitude, float alti debug("updateAltitudeSpeed: altitudeSp %.4f, altitude %.4f, altitude filtered %.4f, flightPathAngleSp %.4f", (double)altitudeSp, (double)altitude, (double)altitudeFiltered, (double)flightPathAngleSp); } +#if defined(__PX4_NUTTX) /* Write part of the status message */ _status.altitudeSp = altitudeSp; _status.altitude_filtered = altitudeFiltered; +#endif // defined(__PX4_NUTTX) /* use flightpath angle setpoint for total energy control */ @@ -143,9 +147,11 @@ int mTecs::updateFlightPathAngleSpeed(float flightPathAngle, float flightPathAng (double)airspeedFiltered, (double)accelerationLongitudinalSp); } +#if defined(__PX4_NUTTX) /* Write part of the status message */ _status.airspeedSp = airspeedSp; _status.airspeed_filtered = airspeedFiltered; +#endif // defined(__PX4_NUTTX) /* use longitudinal acceleration setpoint for total energy control */ @@ -200,23 +206,23 @@ int mTecs::updateFlightPathAngleAcceleration(float flightPathAngle, float flight } /* Check airspeed: if below safe value switch to underspeed mode (if not in land or takeoff mode) */ - if (mode != tecs_status_s::TECS_MODE_LAND && mode != tecs_status_s::TECS_MODE_TAKEOFF && airspeedFiltered < _airspeedMin.get()) { - mode = tecs_status_s::TECS_MODE_UNDERSPEED; + if (mode != MTECS_MODE_LAND && mode != MTECS_MODE_TAKEOFF && airspeedFiltered < _airspeedMin.get()) { + mode = MTECS_MODE_UNDERSPEED; } - /* Set special output limiters if we are not in TECS_MODE_NORMAL */ + /* Set special output limiters if we are not in MTECS_MODE_NORMAL */ BlockOutputLimiter *outputLimiterThrottle = &_controlTotalEnergy.getOutputLimiter(); BlockOutputLimiter *outputLimiterPitch = &_controlEnergyDistribution.getOutputLimiter(); - if (mode == tecs_status_s::TECS_MODE_TAKEOFF) { + if (mode == MTECS_MODE_TAKEOFF) { outputLimiterThrottle = &_BlockOutputLimiterTakeoffThrottle; outputLimiterPitch = &_BlockOutputLimiterTakeoffPitch; - } else if (mode == tecs_status_s::TECS_MODE_LAND) { + } else if (mode == MTECS_MODE_LAND) { // only limit pitch but do not limit throttle outputLimiterPitch = &_BlockOutputLimiterLandPitch; - } else if (mode == tecs_status_s::TECS_MODE_LAND_THROTTLELIM) { + } else if (mode == MTECS_MODE_LAND_THROTTLELIM) { outputLimiterThrottle = &_BlockOutputLimiterLandThrottle; outputLimiterPitch = &_BlockOutputLimiterLandPitch; - } else if (mode == tecs_status_s::TECS_MODE_UNDERSPEED) { + } else if (mode == MTECS_MODE_UNDERSPEED) { outputLimiterThrottle = &_BlockOutputLimiterUnderspeedThrottle; outputLimiterPitch = &_BlockOutputLimiterUnderspeedPitch; } @@ -226,6 +232,7 @@ int mTecs::updateFlightPathAngleAcceleration(float flightPathAngle, float flight * is running) */ limitOverride.applyOverride(*outputLimiterThrottle, *outputLimiterPitch); +// #if defined(__PX4_NUTTX) // /* Write part of the status message */ // _status.flightPathAngleSp = flightPathAngleSp; // _status.flightPathAngle = flightPathAngle; @@ -237,6 +244,7 @@ int mTecs::updateFlightPathAngleAcceleration(float flightPathAngle, float flight // _status.energyDistributionRateSp = energyDistributionRateSp; // _status.energyDistributionRate = energyDistributionRate; // _status.mode = mode; +// #endif // defined(__PX4_NUTTX) /** update control blocks **/ /* update total energy rate control block */ @@ -253,8 +261,10 @@ int mTecs::updateFlightPathAngleAcceleration(float flightPathAngle, float flight (double)accelerationLongitudinalSp, (double)airspeedDerivative); } +#if defined(__PX4_NUTTX) /* publish status message */ _status.update(); +#endif // defined(__PX4_NUTTX) /* clean up */ _firstIterationAfterReset = false; diff --git a/src/modules/fw_pos_control_l1/mtecs/mTecs.h b/src/modules/fw_pos_control_l1/mtecs/mTecs.h index e114cc3ae0..990cb62e2d 100644 --- a/src/modules/fw_pos_control_l1/mtecs/mTecs.h +++ b/src/modules/fw_pos_control_l1/mtecs/mTecs.h @@ -49,11 +49,25 @@ #include #include #include + +#if defined(__PX4_NUTTX) #include +#endif // defined(__PX4_NUTTX) namespace fwPosctrl { +/* corresponds to TECS_MODE in tecs_status.msg */ +enum MTECS_MODE { + MTECS_MODE_NORMAL = 0, + MTECS_MODE_UNDERSPEED = 1, + MTECS_MODE_TAKEOFF = 2, + MTECS_MODE_LAND = 3, + MTECS_MODE_LAND_THROTTLELIM = 4, + MTECS_MODE_BAD_DESCENT = 5, + MTECS_MODE_CLIMBOUT = 6 +}; + /* Main class of the mTecs */ class mTecs : public control::SuperBlock { @@ -101,7 +115,9 @@ protected: control::BlockParamFloat _airspeedMin; /**< minimal airspeed */ /* Publications */ +#if defined(__PX4_NUTTX) uORB::Publication _status; /**< publish internal values for logging */ +#endif // defined(__PX4_NUTTX) /* control blocks */ BlockFFPILimitedCustom _controlTotalEnergy; /**< FFPI controller for total energy control: output From 040d1da8a146727cf9e581583845cb45e18a582f Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Sun, 6 Sep 2015 15:46:59 +0200 Subject: [PATCH 02/44] add 2nd order low pass block --- src/modules/controllib/blocks.cpp | 8 ++++++++ src/modules/controllib/blocks.hpp | 28 ++++++++++++++++++++++++++++ 2 files changed, 36 insertions(+) diff --git a/src/modules/controllib/blocks.cpp b/src/modules/controllib/blocks.cpp index f739446fa7..ed6bfb0f85 100644 --- a/src/modules/controllib/blocks.cpp +++ b/src/modules/controllib/blocks.cpp @@ -198,6 +198,14 @@ int blockHighPassTest() return 0; } +float BlockLowPass2::update(float input) +{ + if (_lp.get_cutoff_freq() != getFCutParam()) { + _lp.set_cutoff_frequency(_fs, getFCutParam()); + } + return _lp.apply(input); +} + float BlockIntegral::update(float input) { // trapezoidal integration diff --git a/src/modules/controllib/blocks.hpp b/src/modules/controllib/blocks.hpp index 979b9541be..ded54e9988 100644 --- a/src/modules/controllib/blocks.hpp +++ b/src/modules/controllib/blocks.hpp @@ -45,6 +45,7 @@ #include #include #include +#include #include "block/Block.hpp" #include "block/BlockParam.hpp" @@ -163,6 +164,33 @@ protected: int __EXPORT blockHighPassTest(); +/** + * A 2nd order low pass filter block which uses the 2nd order low pass filter used by px4 + */ +class __EXPORT BlockLowPass2 : public Block +{ +public: +// methods + BlockLowPass2(SuperBlock *parent, const char *name, float sample_freq) : + Block(parent, name), + _fCut(this, ""), // only one parameter, no need to name + _fs(sample_freq), + _lp(_fs, _fCut.get()) + {}; + virtual ~BlockLowPass2() {}; + float update(float input); +// accessors + float getFCutParam() { return _fCut.get(); } + void setState(float state) { _lp.reset(state); } +protected: +// attributes + control::BlockParamFloat _fCut; + float _fs; + math::LowPassFilter2p _lp; +}; + +// XXX missing test function for BlockLowPass2 + /** * A rectangular integrator. * A limiter is built into the class to bound the From 2c5d810b065e9fc93be5a93109c527c733e0ab7d Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Sun, 6 Sep 2015 15:47:16 +0200 Subject: [PATCH 03/44] make mtecs use 2nd order low pass block --- src/modules/fw_pos_control_l1/mtecs/mTecs.cpp | 6 +++--- src/modules/fw_pos_control_l1/mtecs/mTecs.h | 6 +++--- 2 files changed, 6 insertions(+), 6 deletions(-) diff --git a/src/modules/fw_pos_control_l1/mtecs/mTecs.cpp b/src/modules/fw_pos_control_l1/mtecs/mTecs.cpp index 36ce58f2ff..49e1d735e0 100644 --- a/src/modules/fw_pos_control_l1/mtecs/mTecs.cpp +++ b/src/modules/fw_pos_control_l1/mtecs/mTecs.cpp @@ -60,9 +60,9 @@ mTecs::mTecs() : _controlEnergyDistribution(this, "PIT", true), _controlAltitude(this, "FPA", true), _controlAirSpeed(this, "ACC"), - _flightPathAngleLowpass(this, "FPA_LP"), - _altitudeLowpass(this, "ALT_LP"), - _airspeedLowpass(this, "A_LP"), + _flightPathAngleLowpass(this, "FPA_LP", 50), + _altitudeLowpass(this, "ALT_LP", 50), + _airspeedLowpass(this, "A_LP", 50), _airspeedDerivative(this, "AD"), _throttleSp(0.0f), _pitchSp(0.0f), diff --git a/src/modules/fw_pos_control_l1/mtecs/mTecs.h b/src/modules/fw_pos_control_l1/mtecs/mTecs.h index 990cb62e2d..0d6f2613a6 100644 --- a/src/modules/fw_pos_control_l1/mtecs/mTecs.h +++ b/src/modules/fw_pos_control_l1/mtecs/mTecs.h @@ -130,9 +130,9 @@ protected: setpoint */ /* Other calculation Blocks */ - control::BlockLowPass _flightPathAngleLowpass; /**< low pass filter for the flight path angle */ - control::BlockLowPass _altitudeLowpass; /**< low pass filter for altitude */ - control::BlockLowPass _airspeedLowpass; /**< low pass filter for airspeed */ + control::BlockLowPass2 _flightPathAngleLowpass; /**< low pass filter for the flight path angle */ + control::BlockLowPass2 _altitudeLowpass; /**< low pass filter for altitude */ + control::BlockLowPass2 _airspeedLowpass; /**< low pass filter for airspeed */ control::BlockDerivative _airspeedDerivative; /**< airspeed derivative calulation */ /* Output setpoints */ From b4ee05da03043aed4e6d165eb77b3e02933f3127 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Wed, 9 Sep 2015 06:24:00 +0200 Subject: [PATCH 04/44] add more accessors for 2nd order lp block and derivative block --- src/modules/controllib/blocks.cpp | 7 ++++++- src/modules/controllib/blocks.hpp | 6 +++++- 2 files changed, 11 insertions(+), 2 deletions(-) diff --git a/src/modules/controllib/blocks.cpp b/src/modules/controllib/blocks.cpp index ed6bfb0f85..d97fa76974 100644 --- a/src/modules/controllib/blocks.cpp +++ b/src/modules/controllib/blocks.cpp @@ -200,10 +200,15 @@ int blockHighPassTest() float BlockLowPass2::update(float input) { + if (!isfinite(getState())) { + setState(input); + } + if (_lp.get_cutoff_freq() != getFCutParam()) { _lp.set_cutoff_frequency(_fs, getFCutParam()); } - return _lp.apply(input); + _state = _lp.apply(input); + return _state; } float BlockIntegral::update(float input) diff --git a/src/modules/controllib/blocks.hpp b/src/modules/controllib/blocks.hpp index ded54e9988..7084e69e0d 100644 --- a/src/modules/controllib/blocks.hpp +++ b/src/modules/controllib/blocks.hpp @@ -173,6 +173,7 @@ public: // methods BlockLowPass2(SuperBlock *parent, const char *name, float sample_freq) : Block(parent, name), + _state(0.0 / 0.0 /* initialize to invalid val, force into is_finite() check on first call */), _fCut(this, ""), // only one parameter, no need to name _fs(sample_freq), _lp(_fs, _fCut.get()) @@ -180,10 +181,12 @@ public: virtual ~BlockLowPass2() {}; float update(float input); // accessors + float getState() { return _state; } float getFCutParam() { return _fCut.get(); } - void setState(float state) { _lp.reset(state); } + void setState(float state) { _state = _lp.reset(state); } protected: // attributes + float _state; control::BlockParamFloat _fCut; float _fs; math::LowPassFilter2p _lp; @@ -291,6 +294,7 @@ public: void setU(float u) {_u = u;} float getU() {return _u;} float getLP() {return _lowPass.getFCut();} + float getO() { return _lowPass.getState(); } protected: // attributes float _u; /**< previous input */ From bd2ed0c93bb4d496fe37a669f08c9b0f70aa6266 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Wed, 9 Sep 2015 06:25:00 +0200 Subject: [PATCH 05/44] mtecs: add getters for some internal values --- src/modules/fw_pos_control_l1/mtecs/mTecs.h | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/src/modules/fw_pos_control_l1/mtecs/mTecs.h b/src/modules/fw_pos_control_l1/mtecs/mTecs.h index 0d6f2613a6..09d9eec1d5 100644 --- a/src/modules/fw_pos_control_l1/mtecs/mTecs.h +++ b/src/modules/fw_pos_control_l1/mtecs/mTecs.h @@ -108,6 +108,10 @@ public: float getThrottleSetpoint() { return _throttleSp; } float getPitchSetpoint() { return _pitchSp; } float airspeedLowpassUpdate(float input) { return _airspeedLowpass.update(input); } + float getFlightPathAngleLowpassState() { return _flightPathAngleLowpass.getState(); } + float getAltitudeLowpassState() { return _altitudeLowpass.getState(); } + float getAirspeedLowpassState() { return _airspeedLowpass.getState(); } + float getAirspeedDerivativeLowpassState() { return _airspeedDerivative.getO(); } protected: /* parameters */ From ee001597ff86e5bed90a99eaf7a7d030964dab76 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Sun, 11 Oct 2015 15:20:40 +0200 Subject: [PATCH 06/44] fix posix build --- src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp | 1 + 1 file changed, 1 insertion(+) 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 c3816f5374..d8bc71c463 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 @@ -81,6 +81,7 @@ #include #include #include +#include #include #include #include From a87776eb81b0517053d847dbe1c5fd4e5843add9 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Sun, 11 Oct 2015 16:06:41 +0200 Subject: [PATCH 07/44] add test for BlockLowPass2 --- src/modules/controllib/blocks.cpp | 32 ++++++++++++++++++++++++++++++- src/modules/controllib/blocks.hpp | 4 ++-- 2 files changed, 33 insertions(+), 3 deletions(-) diff --git a/src/modules/controllib/blocks.cpp b/src/modules/controllib/blocks.cpp index d97fa76974..6a0b615910 100644 --- a/src/modules/controllib/blocks.cpp +++ b/src/modules/controllib/blocks.cpp @@ -39,6 +39,7 @@ #include #include +#include #include "blocks.hpp" @@ -51,6 +52,7 @@ int basicBlocksTest() blockLimitSymTest(); blockLowPassTest(); blockHighPassTest(); + blockLowPass2Test(); blockIntegralTest(); blockIntegralTrapTest(); blockDerivativeTest(); @@ -204,13 +206,41 @@ float BlockLowPass2::update(float input) setState(input); } - if (_lp.get_cutoff_freq() != getFCutParam()) { + if (fabsf(_lp.get_cutoff_freq() - getFCutParam()) > FLT_EPSILON) { _lp.set_cutoff_frequency(_fs, getFCutParam()); } _state = _lp.apply(input); return _state; } +int blockLowPass2Test() +{ + printf("Test BlockLowPass2\t\t: "); + BlockLowPass2 lowPass(NULL, "TEST_LP", 100); + // test initial state + ASSERT(equal(10.0f, lowPass.getFCutParam())); + ASSERT(equal(0.0f, lowPass.getState())); + ASSERT(equal(0.0f, lowPass.getDt())); + // set dt + lowPass.setDt(0.1f); + ASSERT(equal(0.1f, lowPass.getDt())); + // set state + lowPass.setState(1.0f); + ASSERT(equal(1.0f, lowPass.getState())); + // test update + ASSERT(equal(1.06745527f, lowPass.update(2.0f))); + + // test end condition + for (int i = 0; i < 100; i++) { + lowPass.update(2.0f); + } + + ASSERT(equal(2.0f, lowPass.getState())); + ASSERT(equal(2.0f, lowPass.update(2.0f))); + printf("PASS\n"); + return 0; +}; + float BlockIntegral::update(float input) { // trapezoidal integration diff --git a/src/modules/controllib/blocks.hpp b/src/modules/controllib/blocks.hpp index 7084e69e0d..786bfc06d3 100644 --- a/src/modules/controllib/blocks.hpp +++ b/src/modules/controllib/blocks.hpp @@ -165,7 +165,7 @@ protected: int __EXPORT blockHighPassTest(); /** - * A 2nd order low pass filter block which uses the 2nd order low pass filter used by px4 + * A 2nd order low pass filter block which uses the default px4 2nd order low pass filter */ class __EXPORT BlockLowPass2 : public Block { @@ -192,7 +192,7 @@ protected: math::LowPassFilter2p _lp; }; -// XXX missing test function for BlockLowPass2 +int __EXPORT blockLowPass2Test(); /** * A rectangular integrator. From d7274ac5f0f5429a5d937365c41cdda3484770c4 Mon Sep 17 00:00:00 2001 From: Nate Weibley Date: Mon, 12 Oct 2015 13:36:37 -0400 Subject: [PATCH 08/44] Enable hash check of used parameters to verify integrity of GCS local copy --- src/modules/mavlink/mavlink_parameters.cpp | 30 +++++++++++++++++----- src/modules/systemlib/param/param.c | 24 +++++++++++++++++ src/modules/systemlib/param/param.h | 7 +++++ 3 files changed, 54 insertions(+), 7 deletions(-) diff --git a/src/modules/mavlink/mavlink_parameters.cpp b/src/modules/mavlink/mavlink_parameters.cpp index 414b7ee96f..b005f3d80d 100644 --- a/src/modules/mavlink/mavlink_parameters.cpp +++ b/src/modules/mavlink/mavlink_parameters.cpp @@ -44,6 +44,8 @@ #include "mavlink_parameters.h" #include "mavlink_main.h" +#define HASH_PARAM "_HASH_CHECK" + MavlinkParametersManager::MavlinkParametersManager(Mavlink *mavlink) : MavlinkStream(mavlink), _send_all_index(-1), _rc_param_map_pub(nullptr), @@ -121,13 +123,27 @@ MavlinkParametersManager::handle_message(const mavlink_message_t *msg) /* when no index is given, loop through string ids and compare them */ if (req_read.param_index < 0) { - /* local name buffer to enforce null-terminated string */ - char name[MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN + 1]; - strncpy(name, req_read.param_id, MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN); - /* enforce null termination */ - name[MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN] = '\0'; - /* attempt to find parameter and send it */ - send_param(param_find_no_notification(name)); + if (strncmp(req_read.param_id, HASH_PARAM, MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN) == 0) { + /* return hash check for cached params */ + uint32_t hash = param_hash_check(); + + /* build the one-off response message */ + mavlink_param_value_t msg; + msg.param_count = param_count_used(); + msg.param_index = -1; + strncpy(msg.param_id, HASH_PARAM, MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN); + msg.param_type = MAV_PARAM_TYPE_UINT32; + memcpy(&msg.param_value, &hash, sizeof(hash)); + _mavlink->send_message(MAVLINK_MSG_ID_PARAM_VALUE, &msg); + } else { + /* local name buffer to enforce null-terminated string */ + char name[MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN + 1]; + strncpy(name, req_read.param_id, MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN); + /* enforce null termination */ + name[MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN] = '\0'; + /* attempt to find parameter and send it */ + send_param(param_find_no_notification(name)); + } } else { /* when index is >= 0, send this parameter again */ diff --git a/src/modules/systemlib/param/param.c b/src/modules/systemlib/param/param.c index 097e61e350..d9976ccdaa 100644 --- a/src/modules/systemlib/param/param.c +++ b/src/modules/systemlib/param/param.c @@ -65,6 +65,8 @@ #include "uORB/topics/parameter_update.h" #include "px4_parameters.h" +#include + #if 0 # define debug(fmt, args...) do { warnx(fmt, ##args); } while(0) #else @@ -1035,3 +1037,25 @@ param_foreach(void (*func)(void *arg, param_t param), void *arg, bool only_chang func(arg, param); } } + +uint32_t param_hash_check(void) +{ + uint32_t param_hash = 0; + + param_lock(); + + /* compute the CRC32 over all string param names and 4 byte values */ + for (param_t param = 0; handle_in_range(param); param++) { + if (!param_used(param)) { + continue; + } + const char *name = param_name(param); + const void *val = param_get_value_ptr(param); + param_hash = crc32part((const uint8_t*)name, strlen(name), param_hash); + param_hash = crc32part(val, sizeof(union param_value_u), param_hash); + } + + param_unlock(); + + return param_hash; +} diff --git a/src/modules/systemlib/param/param.h b/src/modules/systemlib/param/param.h index d7cf666a71..f8334361f1 100644 --- a/src/modules/systemlib/param/param.h +++ b/src/modules/systemlib/param/param.h @@ -333,6 +333,13 @@ __EXPORT int param_save_default(void); */ __EXPORT int param_load_default(void); +/** + * Generate the hash of all parameters and their values + * + * @return CRC32 hash of all param_ids and values + */ +__EXPORT uint32_t param_hash_check(void); + /* * Macros creating static parameter definitions. * From 8a4699c65641388c7dee5aeb3adc6e90b33869ff Mon Sep 17 00:00:00 2001 From: Nate Weibley Date: Mon, 12 Oct 2015 15:26:11 -0400 Subject: [PATCH 09/44] Fix style --- src/modules/systemlib/param/param.c | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/src/modules/systemlib/param/param.c b/src/modules/systemlib/param/param.c index d9976ccdaa..f606f20702 100644 --- a/src/modules/systemlib/param/param.c +++ b/src/modules/systemlib/param/param.c @@ -1049,9 +1049,10 @@ uint32_t param_hash_check(void) if (!param_used(param)) { continue; } + const char *name = param_name(param); const void *val = param_get_value_ptr(param); - param_hash = crc32part((const uint8_t*)name, strlen(name), param_hash); + param_hash = crc32part((const uint8_t *)name, strlen(name), param_hash); param_hash = crc32part(val, sizeof(union param_value_u), param_hash); } From 51e9f686e340b130d03adba859d25ef6b2a837ed Mon Sep 17 00:00:00 2001 From: Don Gagne Date: Mon, 12 Oct 2015 15:06:20 -0700 Subject: [PATCH 10/44] Fix MAV_FRAME_MISSION usage --- src/modules/mavlink/mavlink_mission.cpp | 9 ++++----- 1 file changed, 4 insertions(+), 5 deletions(-) diff --git a/src/modules/mavlink/mavlink_mission.cpp b/src/modules/mavlink/mavlink_mission.cpp index 883fcd0f34..7e9f546d26 100644 --- a/src/modules/mavlink/mavlink_mission.cpp +++ b/src/modules/mavlink/mavlink_mission.cpp @@ -779,13 +779,12 @@ MavlinkMissionManager::parse_mavlink_mission_item(const mavlink_mission_item_t * mission_item->altitude_is_relative = true; break; - case MAV_FRAME_LOCAL_NED: - case MAV_FRAME_LOCAL_ENU: - return MAV_MISSION_UNSUPPORTED_FRAME; - case MAV_FRAME_MISSION: + // This is a mission item with no coordinate + break; + default: - return MAV_MISSION_ERROR; + return MAV_MISSION_UNSUPPORTED_FRAME; } switch (mavlink_mission_item->command) { From 76dcd8b7173df2ac5cd086028e1ce51c35eeec3c Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 13 Oct 2015 18:56:33 +0200 Subject: [PATCH 11/44] EKF: Better output --- .../ekf_att_pos_estimator_main.cpp | 33 ++++++++++++------- 1 file changed, 21 insertions(+), 12 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 295a597fcd..587d22bac4 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 @@ -1180,11 +1180,12 @@ void AttitudePositionEstimatorEKF::print_status() PX4_INFO("states (terrain) [22]: %8.4f", (double)_ekf->states[22]); } else { - PX4_INFO("states (wind) [13-14]: %8.4f, %8.4f", (double)_ekf->states[13], (double)_ekf->states[14]); - PX4_INFO("states (earth mag) [15-17]: %8.4f, %8.4f, %8.4f", (double)_ekf->states[15], (double)_ekf->states[16], - (double)_ekf->states[17]); - PX4_INFO("states (body mag) [18-20]: %8.4f, %8.4f, %8.4f", (double)_ekf->states[18], (double)_ekf->states[19], - (double)_ekf->states[20]); + PX4_INFO("states (accel offs) [13]: %8.4f", (double)_ekf->states[13]); + PX4_INFO("states (wind) [14-15]: %8.4f, %8.4f", (double)_ekf->states[14], (double)_ekf->states[15]); + PX4_INFO("states (earth mag) [16-18]: %8.4f, %8.4f, %8.4f", (double)_ekf->states[16], (double)_ekf->states[17], + (double)_ekf->states[18]); + PX4_INFO("states (mag bias) [19-21]: %8.4f, %8.4f, %8.4f", (double)_ekf->states[19], (double)_ekf->states[20], + (double)_ekf->states[21]); } PX4_INFO("states: %s %s %s %s %s %s %s %s %s %s", @@ -1263,7 +1264,7 @@ void AttitudePositionEstimatorEKF::pollData() } else { _ekf->dVelIMU.x = 0.5f * (_ekf->accel.x + _sensor_combined.accelerometer_m_s2[_accel_main * 3 + 0]); _ekf->dVelIMU.y = 0.5f * (_ekf->accel.y + _sensor_combined.accelerometer_m_s2[_accel_main * 3 + 1]); - _ekf->dVelIMU.z = 0.5f * (_ekf->accel.z + _sensor_combined.accelerometer_m_s2[_accel_main * 3 + 2]); + _ekf->dVelIMU.z = 0.5f * (_ekf->accel.z + _sensor_combined.accelerometer_m_s2[_accel_main * 3 + 2]) + 9.80665f; } _ekf->accel.x = _sensor_combined.accelerometer_m_s2[_accel_main * 3 + 0]; @@ -1331,8 +1332,8 @@ void AttitudePositionEstimatorEKF::pollData() // leave this in as long as larger improvements are still being made. #if 0 - float deltaTIntegral = (_sensor_combined.gyro_integral_dt) / 1e6f; - float deltaTIntAcc = (_sensor_combined.accelerometer_integral_dt) / 1e6f; + float deltaTIntegral = (_sensor_combined.gyro_integral_dt[0]) / 1e6f; + float deltaTIntAcc = (_sensor_combined.accelerometer_integral_dt[0]) / 1e6f; static unsigned dtoverflow5 = 0; static unsigned dtoverflow10 = 0; @@ -1343,13 +1344,21 @@ void AttitudePositionEstimatorEKF::pollData() (double)deltaT, (double)deltaTIntegral, (double)deltaTIntAcc, dtoverflow5, dtoverflow10); - warnx("DRV: dang: %8.4f %8.4f dvel: %8.4f %8.4f", + warnx("EKF: dang: %8.4f %8.4f dvel: %8.4f %8.4f %8.4f", (double)_ekf->dAngIMU.x, (double)_ekf->dAngIMU.z, - (double)_ekf->dVelIMU.x, (double)_ekf->dVelIMU.z); + (double)_ekf->dVelIMU.x, (double)_ekf->dVelIMU.y, (double)_ekf->dVelIMU.z); - warnx("EKF: dang: %8.4f %8.4f dvel: %8.4f %8.4f", + warnx("INT: dang: %8.4f %8.4f dvel: %8.4f %8.4f %8.4f", + (double)(_sensor_combined.gyro_integral_rad[0]), (double)(_sensor_combined.gyro_integral_rad[2]), + (double)(_sensor_combined.accelerometer_integral_m_s[0]), + (double)(_sensor_combined.accelerometer_integral_m_s[1]), + (double)(_sensor_combined.accelerometer_integral_m_s[2])); + + warnx("DRV: dang: %8.4f %8.4f dvel: %8.4f %8.4f %8.4f", (double)(_sensor_combined.gyro_rad_s[0] * deltaT), (double)(_sensor_combined.gyro_rad_s[2] * deltaT), - (double)(_sensor_combined.accelerometer_m_s2[0] * deltaT), (double)(_sensor_combined.accelerometer_m_s2[2] * deltaT)); + (double)(_sensor_combined.accelerometer_m_s2[0] * deltaT), + (double)(_sensor_combined.accelerometer_m_s2[1] * deltaT), + (double)((_sensor_combined.accelerometer_m_s2[2] + 9.8665f) * deltaT)); lastprint = hrt_absolute_time(); } From 2c1e999e1a5859a31199e0fe4cb6a8e3f6e20111 Mon Sep 17 00:00:00 2001 From: Daniel Agar Date: Sat, 10 Oct 2015 23:36:40 -0400 Subject: [PATCH 12/44] cmake remove rsync dep --- cmake/nuttx/px4_impl_nuttx.cmake | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/cmake/nuttx/px4_impl_nuttx.cmake b/cmake/nuttx/px4_impl_nuttx.cmake index 13d120af26..5995a64157 100644 --- a/cmake/nuttx/px4_impl_nuttx.cmake +++ b/cmake/nuttx/px4_impl_nuttx.cmake @@ -213,7 +213,9 @@ function(px4_nuttx_add_export) # copy add_custom_command(OUTPUT nuttx_copy_${CONFIG}.stamp COMMAND ${MKDIR} -p ${CMAKE_BINARY_DIR}/${CONFIG} - COMMAND rsync -a --exclude=.git ${CMAKE_SOURCE_DIR}/NuttX/ ${nuttx_src}/ + COMMAND ${RM} -rf ${nuttx_src} + COMMAND ${CP} -r ${CMAKE_SOURCE_DIR}/NuttX ${nuttx_src} + COMMAND ${RM} -rf ${nuttx_src}/.git COMMAND ${TOUCH} nuttx_copy_${CONFIG}.stamp DEPENDS ${DEPENDS}) add_custom_target(__nuttx_copy_${CONFIG} From e8fd711a5c721230ac829db0904bc3d4f00ab455 Mon Sep 17 00:00:00 2001 From: Daniel Agar Date: Mon, 12 Oct 2015 10:47:09 -0400 Subject: [PATCH 13/44] uorb generate interally set required paths --- Tools/px_generate_uorb_topic_headers.py | 5 +++++ cmake/common/px4_base.cmake | 7 +++---- 2 files changed, 8 insertions(+), 4 deletions(-) diff --git a/Tools/px_generate_uorb_topic_headers.py b/Tools/px_generate_uorb_topic_headers.py index 3c9f5379c6..7ed0b3af19 100755 --- a/Tools/px_generate_uorb_topic_headers.py +++ b/Tools/px_generate_uorb_topic_headers.py @@ -43,6 +43,11 @@ import shutil import filecmp import argparse +import sys +px4_tools_dir = os.path.dirname(os.path.abspath(__file__)) +sys.path.append(px4_tools_dir + "/genmsg/src") +sys.path.append(px4_tools_dir + "/gencpp/src") + try: import genmsg.template_tools except ImportError as e: diff --git a/cmake/common/px4_base.cmake b/cmake/common/px4_base.cmake index 257254ba43..65e96055a7 100644 --- a/cmake/common/px4_base.cmake +++ b/cmake/common/px4_base.cmake @@ -330,7 +330,6 @@ function(px4_generate_messages) if(NOT VERBOSE) set(QUIET "-q") endif() - set(PYTHONPATH "${CMAKE_SOURCE_DIR}/Tools/genmsg/src:${CMAKE_SOURCE_DIR}/Tools/gencpp/src:$ENV{PYTHONPATH}") set(msg_out_path ${CMAKE_BINARY_DIR}/src/modules/uORB/topics) set(msg_list) foreach(msg_file ${MSG_FILES}) @@ -342,7 +341,7 @@ function(px4_generate_messages) list(APPEND msg_files_out ${msg_out_path}/${msg}.h) endforeach() add_custom_command(OUTPUT ${msg_files_out} - COMMAND PYTHONPATH=${PYTHONPATH} ${PYTHON_EXECUTABLE} + COMMAND ${PYTHON_EXECUTABLE} Tools/px_generate_uorb_topic_headers.py ${QUIET} -d msg @@ -363,7 +362,7 @@ function(px4_generate_messages) list(APPEND msg_multi_files_out ${msg_multi_out_path}/px4_${msg}.h) endforeach() add_custom_command(OUTPUT ${msg_multi_files_out} - COMMAND PYTHONPATH=${PYTHONPATH} ${PYTHON_EXECUTABLE} + COMMAND ${PYTHON_EXECUTABLE} Tools/px_generate_uorb_topic_headers.py ${QUIET} -d msg @@ -425,7 +424,7 @@ function(px4_add_upload) endif() px4_join(OUT serial_ports LIST "${serial_ports}" GLUE ",") add_custom_target(${OUT} - COMMAND PYTHONPATH=${PYTHONPATH} ${PYTHON_EXECUTABLE} + COMMAND ${PYTHON_EXECUTABLE} ${CMAKE_SOURCE_DIR}/Tools/px_uploader.py --port ${serial_ports} ${BUNDLE} DEPENDS ${BUNDLE} WORKING_DIRECTORY ${CMAKE_BINARY_DIR} From 1bfc919c83e93eddcd333a606a3da6df3b0f0dbd Mon Sep 17 00:00:00 2001 From: Daniel Agar Date: Mon, 12 Oct 2015 11:10:13 -0400 Subject: [PATCH 14/44] cmake generate MSYS Makefiles on windows --- Makefile | 8 +++++++- 1 file changed, 7 insertions(+), 1 deletion(-) diff --git a/Makefile b/Makefile index 2473eeff43..2505c75711 100644 --- a/Makefile +++ b/Makefile @@ -74,7 +74,13 @@ ifdef NINJA_BUILD PX4_MAKE = ninja PX4_MAKE_ARGS = else - PX4_CMAKE_GENERATOR ?= "Unix Makefiles" + +ifdef SYSTEMROOT + # Windows + PX4_CMAKE_GENERATOR ?= "MSYS Makefiles" +else + PX4_CMAKE_GENERATOR ?= "Unix Makefiles" +endif PX4_MAKE = make PX4_MAKE_ARGS = -j$(j) --no-print-directory endif From 4e4d10d8b9b1b814729e529a461a37e284f8f0f2 Mon Sep 17 00:00:00 2001 From: Daniel Agar Date: Mon, 12 Oct 2015 20:51:38 -0400 Subject: [PATCH 15/44] generate_listener.py don't depend on platform dependant slash --- Tools/generate_listener.py | 5 ++++- 1 file changed, 4 insertions(+), 1 deletion(-) diff --git a/Tools/generate_listener.py b/Tools/generate_listener.py index 1ebb316baf..d32bbc0f9c 100755 --- a/Tools/generate_listener.py +++ b/Tools/generate_listener.py @@ -1,6 +1,7 @@ #!/usr/bin/python import glob +import os import sys # This script is run from Build/_default.build/$(PX4_BASE)/Firmware/src/systemcmds/topic_listener @@ -48,7 +49,9 @@ for index,m in enumerate(raw_messages): temp_list.append(("int8",line.split(' ')[1].split('\t')[0].split('\n')[0])) f.close() - messages.append(m.split('/')[-1].split('.')[0]) + (m_head, m_tail) = os.path.split(m) + messages.append(m_tail.split('.')[0]) + #messages.append(m.split('/')[-1].split('.')[0]) message_elements.append(temp_list) num_messages = len(messages); From 3f0653e824949f5e991bc279d9f1f4749a7914c0 Mon Sep 17 00:00:00 2001 From: Daniel Agar Date: Tue, 13 Oct 2015 00:47:58 -0400 Subject: [PATCH 16/44] bin_to_obj.py don't use full path to visibility.h --- cmake/common/px4_base.cmake | 4 +++- cmake/nuttx/bin_to_obj.py | 4 +++- cmake/nuttx/px4_impl_nuttx.cmake | 1 + 3 files changed, 7 insertions(+), 2 deletions(-) diff --git a/cmake/common/px4_base.cmake b/cmake/common/px4_base.cmake index 65e96055a7..fe07a67666 100644 --- a/cmake/common/px4_base.cmake +++ b/cmake/common/px4_base.cmake @@ -535,6 +535,7 @@ function(px4_add_common_flags) -ffunction-sections -fdata-sections ) + if (NOT ${CMAKE_C_COMPILER_ID} STREQUAL "Clang") list(APPEND optimization_flags -fno-strength-reduce @@ -565,6 +566,7 @@ function(px4_add_common_flags) set(cxx_warnings -Wno-missing-field-initializers ) + set(cxx_compile_flags -g -fno-exceptions @@ -577,7 +579,7 @@ function(px4_add_common_flags) set(visibility_flags -fvisibility=hidden - "-include ${CMAKE_SOURCE_DIR}/src/include/visibility.h" + -include visibility.h ) set(added_c_flags diff --git a/cmake/nuttx/bin_to_obj.py b/cmake/nuttx/bin_to_obj.py index 64ba864295..4b99a6c29b 100755 --- a/cmake/nuttx/bin_to_obj.py +++ b/cmake/nuttx/bin_to_obj.py @@ -12,6 +12,7 @@ from subprocess import PIPE parser = argparse.ArgumentParser(description='Convert bin to obj.') parser.add_argument('--c_flags', required=True) parser.add_argument('--c_compiler', required=True) +parser.add_argument('--include_path', required=True) parser.add_argument('--nm', required=True) parser.add_argument('--ld', required=True) parser.add_argument('--objcopy', required=True) @@ -23,6 +24,7 @@ args = parser.parse_args() in_bin = args.bin c_flags = args.c_flags c_compiler = args.c_compiler +include_path = args.include_path nm = args.nm ld = args.ld obj = args.obj @@ -46,7 +48,7 @@ def run_cmd(cmd, d): return stdout # do compile -run_cmd("{c_compiler:s} {c_flags:s} -c {obj:s}.c -o {obj:s}.c.o", +run_cmd("{c_compiler:s} -I{include_path:s} {c_flags:s} -c {obj:s}.c -o {obj:s}.c.o", locals()) # link diff --git a/cmake/nuttx/px4_impl_nuttx.cmake b/cmake/nuttx/px4_impl_nuttx.cmake index 5995a64157..2586478383 100644 --- a/cmake/nuttx/px4_impl_nuttx.cmake +++ b/cmake/nuttx/px4_impl_nuttx.cmake @@ -349,6 +349,7 @@ function(px4_nuttx_add_romfs) #COMMAND cmake -E remove_directory ${romfs_temp_dir} COMMAND ${PYTHON_EXECUTABLE} ${bin_to_obj} --ld ${LD} --c_flags ${CMAKE_C_FLAGS} + --include_path "${CMAKE_SOURCE_DIR}/src/include" --c_compiler ${CMAKE_C_COMPILER} --nm ${NM} --objcopy ${OBJCOPY} --obj romfs.o From a45fe6343007dc7e0161f4038e460f9c74011757 Mon Sep 17 00:00:00 2001 From: Daniel Agar Date: Tue, 13 Oct 2015 01:07:37 -0400 Subject: [PATCH 17/44] bin_to_obj.py fix windows regex size match --- cmake/nuttx/bin_to_obj.py | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) diff --git a/cmake/nuttx/bin_to_obj.py b/cmake/nuttx/bin_to_obj.py index 4b99a6c29b..80ce80d741 100755 --- a/cmake/nuttx/bin_to_obj.py +++ b/cmake/nuttx/bin_to_obj.py @@ -57,9 +57,10 @@ run_cmd("{ld:s} -r -o {obj:s}.bin.o {obj:s}.c.o -b binary {in_bin:s}", # get size of image stdout = run_cmd("{nm:s} -p --radix=x {obj:s}.bin.o", locals()) -re_string = r"^([0-9A-F-a-f]+) .*{sym:s}_size\n".format(**locals()) +re_string = r"^([0-9A-Fa-f]+) .*{sym:s}_size".format(**locals()) re_size = re.compile(re_string, re.MULTILINE) size_match = re.search(re_size, stdout.decode()) + try: size = size_match.group(1) except AttributeError as e: @@ -78,7 +79,7 @@ with open('{obj:s}.c'.format(**locals()), 'w') as f: **locals())) # do compile -run_cmd("{c_compiler:s} {c_flags:s} -c {obj:s}.c -o {obj:s}.c.o", +run_cmd("{c_compiler:s} -I{include_path:s} {c_flags:s} -c {obj:s}.c -o {obj:s}.c.o", locals()) # link From cc3695d0b73be26bf7b4f151b48736671eb7f513 Mon Sep 17 00:00:00 2001 From: Daniel Agar Date: Tue, 13 Oct 2015 01:18:33 -0400 Subject: [PATCH 18/44] uavcan manually add src/include to include path --- src/modules/uavcan/CMakeLists.txt | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/uavcan/CMakeLists.txt b/src/modules/uavcan/CMakeLists.txt index 38bf0bc111..476acc5dd4 100644 --- a/src/modules/uavcan/CMakeLists.txt +++ b/src/modules/uavcan/CMakeLists.txt @@ -53,7 +53,7 @@ set(uavcan_definitions -DUAVCAN_STM32_${OS_UPPER}=1 -DUAVCAN_CPP_VERSION=UAVCAN_CPP03 ) -set(uavcan_extra_flags ${uavcan_definitions}) +set(uavcan_extra_flags ${uavcan_definitions} -I${CMAKE_SOURCE_DIR}/src/include) if (${OS} STREQUAL "nuttx") set(uavcan_platform stm32) From 5865dc64348b9b2b7bc514965ba275937bc28263 Mon Sep 17 00:00:00 2001 From: Daniel Agar Date: Wed, 14 Oct 2015 01:33:08 -0400 Subject: [PATCH 19/44] cmake add libuavcan as subdirectory -move uavcan submodule to src/modules/uavcan/libuavcan -adding libuavcan as a subdirectory simplifies inheriting all compile flags and include paths --- .gitmodules | 4 +- CMakeLists.txt | 2 +- Makefile | 4 +- Tools/check_code_style.sh | 2 +- cmake/common/px4_base.cmake | 4 +- cmake/configs/nuttx_px4fmu-v2_default.cmake | 4 +- nuttx-configs/px4fmu-v2/nsh/defconfig | 2 + src/lib/uavcan | 1 - src/modules/uavcan/CMakeLists.txt | 76 ++++++--------------- src/modules/uavcan/libuavcan | 1 + 10 files changed, 32 insertions(+), 68 deletions(-) delete mode 160000 src/lib/uavcan create mode 160000 src/modules/uavcan/libuavcan diff --git a/.gitmodules b/.gitmodules index cb9a6ccf05..2820e68539 100644 --- a/.gitmodules +++ b/.gitmodules @@ -4,8 +4,8 @@ [submodule "NuttX"] path = NuttX url = git://github.com/PX4/NuttX.git -[submodule "src/lib/uavcan"] - path = src/lib/uavcan +[submodule "src/modules/uavcan/libuavcan"] + path = src/modules/uavcan/libuavcan url = git://github.com/UAVCAN/libuavcan.git [submodule "Tools/genmsg"] path = Tools/genmsg diff --git a/CMakeLists.txt b/CMakeLists.txt index 948959ca32..6aec7f2e0c 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -224,7 +224,7 @@ px4_add_git_submodule(TARGET git_genmsg PATH "Tools/genmsg") px4_add_git_submodule(TARGET git_gencpp PATH "Tools/gencpp") px4_add_git_submodule(TARGET git_mavlink PATH "mavlink/include/mavlink/v1.0") px4_add_git_submodule(TARGET git_gtest PATH "unittets/gtest") -px4_add_git_submodule(TARGET git_uavcan PATH "src/lib/uavcan") +px4_add_git_submodule(TARGET git_uavcan PATH "src/modules/uavcan/libuavcan") px4_add_git_submodule(TARGET git_eigen PATH "src/lib/eigen") px4_add_git_submodule(TARGET git_nuttx PATH "NuttX") px4_add_git_submodule(TARGET git_dspal PATH "src/lib/dspal") diff --git a/Makefile b/Makefile index 2505c75711..a507be0826 100644 --- a/Makefile +++ b/Makefile @@ -177,9 +177,9 @@ distclean: clean @cd NuttX @git clean -d -f -x @cd .. - @cd src/lib/uavcan + @cd src/modules/uavcan/libuavcan @git clean -d -f -x - @cd ../../.. + @cd ../../../.. # targets handled by cmake cmake_targets = test upload package package_source debug debug_tui debug_ddd debug_io debug_io_tui debug_io_ddd check_weak libuavcan diff --git a/Tools/check_code_style.sh b/Tools/check_code_style.sh index db140fca45..bc59128317 100755 --- a/Tools/check_code_style.sh +++ b/Tools/check_code_style.sh @@ -25,7 +25,7 @@ for fn in $(find src/examples \ -path './NuttX' -prune -o \ -path './src/lib/eigen' -prune -o \ -path './src/lib/mathlib/CMSIS' -prune -o \ - -path './src/lib/uavcan' -prune -o \ + -path './src/modules/uavcan/libuavcan' -prune -o \ -path './src/modules/attitude_estimator_ekf/codegen' -prune -o \ -path './src/modules/ekf_att_pos_estimator' -prune -o \ -path './src/modules/sdlog2' -prune -o \ diff --git a/cmake/common/px4_base.cmake b/cmake/common/px4_base.cmake index fe07a67666..9a0b17b7a2 100644 --- a/cmake/common/px4_base.cmake +++ b/cmake/common/px4_base.cmake @@ -138,8 +138,7 @@ function(px4_add_git_submodule) string(REPLACE "/" "_" NAME ${PATH}) add_custom_command(OUTPUT ${CMAKE_BINARY_DIR}/git_${NAME}.stamp WORKING_DIRECTORY ${CMAKE_SOURCE_DIR} - COMMAND git submodule init ${PATH} - COMMAND git submodule update -f ${PATH} + COMMAND git submodule update --init --recursive -f ${PATH} COMMAND touch ${CMAKE_BINARY_DIR}/git_${NAME}.stamp ) add_custom_target(${TARGET} @@ -544,7 +543,6 @@ function(px4_add_common_flags) endif() set(c_warnings - -Wbad-function-cast -Wstrict-prototypes -Wmissing-prototypes -Wnested-externs diff --git a/cmake/configs/nuttx_px4fmu-v2_default.cmake b/cmake/configs/nuttx_px4fmu-v2_default.cmake index 89910bbd95..a242f4f736 100644 --- a/cmake/configs/nuttx_px4fmu-v2_default.cmake +++ b/cmake/configs/nuttx_px4fmu-v2_default.cmake @@ -168,8 +168,8 @@ set(config_io_board set(config_extra_libs ${CMAKE_SOURCE_DIR}/src/lib/mathlib/CMSIS/libarm_cortexM4lf_math.a - libuavcan.a - libuavcan_stm32_driver.a + uavcan + uavcan_stm32_driver ) set(config_io_extra_libs diff --git a/nuttx-configs/px4fmu-v2/nsh/defconfig b/nuttx-configs/px4fmu-v2/nsh/defconfig index db7c41dae0..871b214f49 100644 --- a/nuttx-configs/px4fmu-v2/nsh/defconfig +++ b/nuttx-configs/px4fmu-v2/nsh/defconfig @@ -1064,3 +1064,5 @@ CONFIG_SYSTEM_SYSINFO=y # # USB Monitor # + +CONFIG_NSOCKET_DESCRIPTORS=0 diff --git a/src/lib/uavcan b/src/lib/uavcan deleted file mode 160000 index 86c3397b1b..0000000000 --- a/src/lib/uavcan +++ /dev/null @@ -1 +0,0 @@ -Subproject commit 86c3397b1b423eeed86f21c8b08c8acfbd5add96 diff --git a/src/modules/uavcan/CMakeLists.txt b/src/modules/uavcan/CMakeLists.txt index 476acc5dd4..3c7ad32998 100644 --- a/src/modules/uavcan/CMakeLists.txt +++ b/src/modules/uavcan/CMakeLists.txt @@ -31,65 +31,30 @@ # ############################################################################ -# uavcan project -set(uavcan_c_flags ${c_flags}) -list(REMOVE_ITEM uavcan_c_flags -std=gnu++0x -D__CUSTOM_FILE_IO__) -set(uavcan_cxx_flags ${cxx_flags}) -list(REMOVE_ITEM uavcan_cxx_flags -std=gnu++0x -std=c++11 -Wundef -Werror -D__CUSTOM_FILE_IO__) -set(uavcan_deps git_uavcan) -set(uavcan_platform generic) - -set(nuttx_export_dir ${CMAKE_BINARY_DIR}/${BOARD}/NuttX/nuttx-export) +set(UAVCAN_USE_CPP03 ON CACHE BOOL "uavcan cpp03") +set(UAVCAN_PLATFORM stm32 CACHE STRING "uavcan platform") string(TOUPPER "${OS}" OS_UPPER) - -set(uavcan_definitions - -DUAVCAN_NO_ASSERTIONS - -DUAVCAN_STM32_NUM_IFACES=2 - -DUAVCAN_USE_EXTERNAL_SNPRINT - -DUAVCAN_MEM_POOL_BLOCK_SIZE=48 - -DUAVCAN_MAX_NETWORK_SIZE_HINT=16 - -DUAVCAN_STM32_TIMER_NUMBER=5 - -DUAVCAN_STM32_${OS_UPPER}=1 - -DUAVCAN_CPP_VERSION=UAVCAN_CPP03 - ) -set(uavcan_extra_flags ${uavcan_definitions} -I${CMAKE_SOURCE_DIR}/src/include) - -if (${OS} STREQUAL "nuttx") - set(uavcan_platform stm32) - list(APPEND uavcan_extra_flags - -I${nuttx_export_dir}/include - -I${nuttx_export_dir}/include/cxx - -I${nuttx_export_dir}/arch/chip - -I${nuttx_export_dir}/arch/common - ) - list(APPEND uavcan_deps nuttx_export_${BOARD}) -endif() - -list(APPEND uavcan_c_flags ${uavcan_extra_flags}) -list(APPEND uavcan_cxx_flags ${uavcan_extra_flags}) - -px4_join(OUT uavcan_c_flags LIST "${uavcan_c_flags}" GLUE " ") -px4_join(OUT uavcan_cxx_flags LIST "${uavcan_cxx_flags}" GLUE " ") - -externalproject_add(libuavcan - DEPENDS ${uavcan_deps} - DOWNLOAD_COMMAND "" - UPDATE_COMMAND git submodule update --init - LOG_INSTALL 1 - SOURCE_DIR ${CMAKE_SOURCE_DIR}/src/lib/uavcan - CMAKE_ARGS -DCMAKE_TOOLCHAIN_FILE=${CMAKE_TOOLCHAIN_FILE} +add_definitions( + -DUAVCAN_CPP_VERSION=UAVCAN_CPP03 + -DUAVCAN_MAX_NETWORK_SIZE_HINT=16 + -DUAVCAN_MEM_POOL_BLOCK_SIZE=48 + -DUAVCAN_NO_ASSERTIONS + -DUAVCAN_PLATFORM=stm32 + -DUAVCAN_STM32_${OS_UPPER}=1 + -DUAVCAN_STM32_NUM_IFACES=2 + -DUAVCAN_STM32_TIMER_NUMBER=5 -DUAVCAN_USE_CPP03=ON - -DUAVCAN_PLATFORM=${uavcan_platform} - -DUAVCAN_OS=${OS} - -DCMAKE_CXX_FLAGS=${uavcan_cxx_flags} - -DCMAKE_C_FLAGS=${uavcan_c_flags} - -DCMAKE_INSTALL_PREFIX=${ep_base}/Install -) + -DUAVCAN_USE_EXTERNAL_SNPRINT + ) -string(TOUPPER ${OS} OS_UPPER) +add_subdirectory(libuavcan EXCLUDE_FROM_ALL) +add_dependencies(uavcan platforms__nuttx) -add_definitions(${uavcan_definitions}) +include_directories(libuavcan/libuavcan/include) +include_directories(libuavcan/libuavcan/include/dsdlc_generated) +include_directories(libuavcan/libuavcan_drivers/posix/include) +include_directories(libuavcan/libuavcan_drivers/stm32/driver/include) px4_add_module( MODULE modules__uavcan @@ -100,7 +65,6 @@ px4_add_module( -Wno-deprecated-declarations -O3 SRCS - # Main uavcan_main.cpp uavcan_servers.cpp @@ -117,7 +81,7 @@ px4_add_module( DEPENDS platforms__common - libuavcan + uavcan ) ## vim: set noet ft=cmake fenc=utf-8 ff=unix : diff --git a/src/modules/uavcan/libuavcan b/src/modules/uavcan/libuavcan new file mode 160000 index 0000000000..0643879922 --- /dev/null +++ b/src/modules/uavcan/libuavcan @@ -0,0 +1 @@ +Subproject commit 0643879922239930cf7482777356f06891c26616 From 4d7cc41921101cf9ee91793cf12e21c3d94a7c19 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Wed, 14 Oct 2015 12:12:58 +0200 Subject: [PATCH 20/44] EKF: Retain minimum Kalman update rate --- .../ekf_att_pos_estimator/AttitudePositionEstimatorEKF.h | 1 + .../ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp | 4 +++- 2 files changed, 4 insertions(+), 1 deletion(-) diff --git a/src/modules/ekf_att_pos_estimator/AttitudePositionEstimatorEKF.h b/src/modules/ekf_att_pos_estimator/AttitudePositionEstimatorEKF.h index 57df913069..421e109eb3 100644 --- a/src/modules/ekf_att_pos_estimator/AttitudePositionEstimatorEKF.h +++ b/src/modules/ekf_att_pos_estimator/AttitudePositionEstimatorEKF.h @@ -177,6 +177,7 @@ private: hrt_abstime _last_accel; hrt_abstime _last_mag; unsigned _prediction_steps; + uint64_t _prediction_last; struct sensor_combined_s _sensor_combined; 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 587d22bac4..afb7cfbac5 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 @@ -157,6 +157,7 @@ AttitudePositionEstimatorEKF::AttitudePositionEstimatorEKF() : _last_accel(0), _last_mag(0), _prediction_steps(0), + _prediction_last(0), _sensor_combined{}, @@ -997,11 +998,12 @@ void AttitudePositionEstimatorEKF::updateSensorFusion(const bool fuseGPS, const _covariancePredictionDt += _ekf->dtIMU; // only fuse every few steps - if (_prediction_steps < MAX_PREDICITION_STEPS) { + if (_prediction_steps < MAX_PREDICITION_STEPS && ((hrt_absolute_time() - _prediction_last) < 20 * 1000)) { _prediction_steps++; return; } else { _prediction_steps = 0; + _prediction_last = hrt_absolute_time(); } // perform a covariance prediction if the total delta angle has exceeded the limit From 51707245bb643a79bec6a75e3a151549ad1093c1 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Wed, 14 Oct 2015 12:13:20 +0200 Subject: [PATCH 21/44] Makefile: Use blunt force to ensure uavcan submodule is up to date --- Makefile | 11 ++++++++--- 1 file changed, 8 insertions(+), 3 deletions(-) diff --git a/Makefile b/Makefile index a507be0826..2b9a34553f 100644 --- a/Makefile +++ b/Makefile @@ -104,13 +104,13 @@ endef # -------------------------------------------------------------------- # Do not put any spaces between function arguments. -px4fmu-v1_default: +px4fmu-v1_default: git-init $(call cmake-build,nuttx_px4fmu-v1_default) -px4fmu-v2_default: +px4fmu-v2_default: git-init $(call cmake-build,nuttx_px4fmu-v2_default) -px4fmu-v2_simple: +px4fmu-v2_simple: git-init $(call cmake-build,nuttx_px4fmu-v2_simple) nuttx_sim_simple: @@ -181,6 +181,11 @@ distclean: clean @git clean -d -f -x @cd ../../../.. +# XXX this is not the right way to fix it, but we need a temporary solution +# for average joe +git-init: + @git submodule update --init --recursive + # targets handled by cmake cmake_targets = test upload package package_source debug debug_tui debug_ddd debug_io debug_io_tui debug_io_ddd check_weak libuavcan $(foreach targ,$(cmake_targets),$(eval $(call cmake-targ,$(targ)))) From 92ae41c1cf970b5ec9fd7b1a9f919c305fcf2133 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Wed, 14 Oct 2015 13:58:36 +0200 Subject: [PATCH 22/44] PX4 log: reduce output verbosity to save flash --- src/platforms/px4_log.h | 10 +++++----- 1 file changed, 5 insertions(+), 5 deletions(-) diff --git a/src/platforms/px4_log.h b/src/platforms/px4_log.h index 5b7a65eeb6..2a0ff33773 100644 --- a/src/platforms/px4_log.h +++ b/src/platforms/px4_log.h @@ -357,8 +357,8 @@ __END_DECLS /**************************************************************************** * Non-verbose settings for a Release build to minimize strings in build ****************************************************************************/ -#define PX4_PANIC(FMT, ...) __px4_log_file_and_line(_PX4_LOG_LEVEL_PANIC, FMT, ##__VA_ARGS__) -#define PX4_ERR(FMT, ...) __px4_log_file_and_line(_PX4_LOG_LEVEL_ERROR, FMT, ##__VA_ARGS__) +#define PX4_PANIC(FMT, ...) __px4_log(_PX4_LOG_LEVEL_PANIC, FMT, ##__VA_ARGS__) +#define PX4_ERR(FMT, ...) __px4_log(_PX4_LOG_LEVEL_ERROR, FMT, ##__VA_ARGS__) #define PX4_WARN(FMT, ...) __px4_log_omit(_PX4_LOG_LEVEL_WARN, FMT, ##__VA_ARGS__) #define PX4_DEBUG(FMT, ...) __px4_log_omit(_PX4_LOG_LEVEL_DEBUG, FMT, ##__VA_ARGS__) @@ -366,9 +366,9 @@ __END_DECLS /**************************************************************************** * Medium verbose settings for a default build ****************************************************************************/ -#define PX4_PANIC(FMT, ...) __px4_log_file_and_line(_PX4_LOG_LEVEL_PANIC, FMT, ##__VA_ARGS__) -#define PX4_ERR(FMT, ...) __px4_log_file_and_line(_PX4_LOG_LEVEL_ERROR, FMT, ##__VA_ARGS__) -#define PX4_WARN(FMT, ...) __px4_log_file_and_line(_PX4_LOG_LEVEL_WARN, FMT, ##__VA_ARGS__) +#define PX4_PANIC(FMT, ...) __px4_log(_PX4_LOG_LEVEL_PANIC, FMT, ##__VA_ARGS__) +#define PX4_ERR(FMT, ...) __px4_log(_PX4_LOG_LEVEL_ERROR, FMT, ##__VA_ARGS__) +#define PX4_WARN(FMT, ...) __px4_log(_PX4_LOG_LEVEL_WARN, FMT, ##__VA_ARGS__) #define PX4_DEBUG(FMT, ...) __px4_log_omit(_PX4_LOG_LEVEL_DEBUG, FMT, ##__VA_ARGS__) #endif From ae65269de9f0ea1e63ba5addce1e1788c62c93ee Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Wed, 14 Oct 2015 14:06:42 +0200 Subject: [PATCH 23/44] UAVCAN: Increase stack since its hitting almost the limit --- src/modules/uavcan/uavcan_main.hpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/uavcan/uavcan_main.hpp b/src/modules/uavcan/uavcan_main.hpp index 2b5e01e246..6a1d2f391d 100644 --- a/src/modules/uavcan/uavcan_main.hpp +++ b/src/modules/uavcan/uavcan_main.hpp @@ -94,7 +94,7 @@ class UavcanNode : public device::CDev */ static constexpr unsigned RxQueueLenPerIface = FramePerMSecond * PollTimeoutMs; // At - static constexpr unsigned StackSize = 1600; + static constexpr unsigned StackSize = 1800; public: typedef uavcan::Node Node; From cff6cde47b65d3470f286a0a3099da7b2f8935f4 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Wed, 14 Oct 2015 14:07:17 +0200 Subject: [PATCH 24/44] UAVCAN: Set default optimization level to -Os because comm protocols should not hog the flash or RAM. --- src/modules/uavcan/CMakeLists.txt | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/uavcan/CMakeLists.txt b/src/modules/uavcan/CMakeLists.txt index 3c7ad32998..c2bf52247d 100644 --- a/src/modules/uavcan/CMakeLists.txt +++ b/src/modules/uavcan/CMakeLists.txt @@ -63,7 +63,7 @@ px4_add_module( COMPILE_FLAGS -Wframe-larger-than=1500 -Wno-deprecated-declarations - -O3 + -Os SRCS # Main uavcan_main.cpp From 21431cf237a3da38004251d66f49082eb15bb60b Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Wed, 14 Oct 2015 14:08:03 +0200 Subject: [PATCH 25/44] EKF: Fix output commands, fix stack size for commandline tool so we do not run out of space during debug printing --- src/modules/ekf_att_pos_estimator/CMakeLists.txt | 1 + .../ekf_att_pos_estimator_main.cpp | 13 ++++++------- 2 files changed, 7 insertions(+), 7 deletions(-) diff --git a/src/modules/ekf_att_pos_estimator/CMakeLists.txt b/src/modules/ekf_att_pos_estimator/CMakeLists.txt index 6f109def1f..fc7fefd2bf 100644 --- a/src/modules/ekf_att_pos_estimator/CMakeLists.txt +++ b/src/modules/ekf_att_pos_estimator/CMakeLists.txt @@ -39,6 +39,7 @@ endif() px4_add_module( MODULE modules__ekf_att_pos_estimator MAIN ekf_att_pos_estimator + STACK 3000 COMPILE_FLAGS ${MODULE_CFLAGS} SRCS ekf_att_pos_estimator_main.cpp 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 afb7cfbac5..48d30ceac8 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 @@ -1163,6 +1163,7 @@ void AttitudePositionEstimatorEKF::print_status() PX4_INFO("dang: %8.4f %8.4f %8.4f dang corr: %8.4f %8.4f %8.4f" , (double)_ekf->dAngIMU.x, (double)_ekf->dAngIMU.y, (double)_ekf->dAngIMU.z, (double)_ekf->correctedDelAng.x, (double)_ekf->correctedDelAng.y, (double)_ekf->correctedDelAng.z); + PX4_INFO("states (quat) [0-3]: %8.4f, %8.4f, %8.4f, %8.4f", (double)_ekf->states[0], (double)_ekf->states[1], (double)_ekf->states[2], (double)_ekf->states[3]); PX4_INFO("states (vel m/s) [4-6]: %8.4f, %8.4f, %8.4f", (double)_ekf->states[4], (double)_ekf->states[5], @@ -1342,21 +1343,21 @@ void AttitudePositionEstimatorEKF::pollData() static hrt_abstime lastprint = 0; if (hrt_elapsed_time(&lastprint) > 1000000) { - warnx("dt: %8.6f, dtg: %8.6f, dta: %8.6f, dt > 5 ms: %u, dt > 10 ms: %u", + PX4_WARN("dt: %8.6f, dtg: %8.6f, dta: %8.6f, dt > 5 ms: %u, dt > 10 ms: %u", (double)deltaT, (double)deltaTIntegral, (double)deltaTIntAcc, dtoverflow5, dtoverflow10); - warnx("EKF: dang: %8.4f %8.4f dvel: %8.4f %8.4f %8.4f", + PX4_WARN("EKF: dang: %8.4f %8.4f dvel: %8.4f %8.4f %8.4f", (double)_ekf->dAngIMU.x, (double)_ekf->dAngIMU.z, (double)_ekf->dVelIMU.x, (double)_ekf->dVelIMU.y, (double)_ekf->dVelIMU.z); - warnx("INT: dang: %8.4f %8.4f dvel: %8.4f %8.4f %8.4f", + PX4_WARN("INT: dang: %8.4f %8.4f dvel: %8.4f %8.4f %8.4f", (double)(_sensor_combined.gyro_integral_rad[0]), (double)(_sensor_combined.gyro_integral_rad[2]), (double)(_sensor_combined.accelerometer_integral_m_s[0]), (double)(_sensor_combined.accelerometer_integral_m_s[1]), (double)(_sensor_combined.accelerometer_integral_m_s[2])); - warnx("DRV: dang: %8.4f %8.4f dvel: %8.4f %8.4f %8.4f", + PX4_WARN("DRV: dang: %8.4f %8.4f dvel: %8.4f %8.4f %8.4f", (double)(_sensor_combined.gyro_rad_s[0] * deltaT), (double)(_sensor_combined.gyro_rad_s[2] * deltaT), (double)(_sensor_combined.accelerometer_m_s2[0] * deltaT), (double)(_sensor_combined.accelerometer_m_s2[1] * deltaT), @@ -1666,8 +1667,6 @@ int ekf_att_pos_estimator_main(int argc, char *argv[]) } if (!strcmp(argv[1], "status")) { - PX4_INFO("running"); - estimator::g_estimator->print_status(); return 0; @@ -1692,6 +1691,6 @@ int ekf_att_pos_estimator_main(int argc, char *argv[]) return ret; } - PX4_ERR("unrecognized command"); + PX4_WARN("unrecognized command"); return 1; } From 88a4d0deca60d52d02daea09ab508eb03e793956 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Wed, 14 Oct 2015 14:27:49 +0200 Subject: [PATCH 26/44] EKF: Reduce excessive stack size --- .../ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) 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 48d30ceac8..615ad143aa 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 @@ -1122,7 +1122,7 @@ int AttitudePositionEstimatorEKF::start() _estimator_task = px4_task_spawn_cmd("ekf_att_pos_estimator", SCHED_DEFAULT, SCHED_PRIORITY_MAX - 20, - 4200, + 3900, (px4_main_t)&AttitudePositionEstimatorEKF::task_main_trampoline, nullptr); From cde947a1d88bdad89210891c1153d828073eca39 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Wed, 14 Oct 2015 14:44:37 +0200 Subject: [PATCH 27/44] FMUv2: Reduce excessive work task stack sizes --- nuttx-configs/px4fmu-v2/nsh/defconfig | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/nuttx-configs/px4fmu-v2/nsh/defconfig b/nuttx-configs/px4fmu-v2/nsh/defconfig index 871b214f49..3340fdd0c9 100644 --- a/nuttx-configs/px4fmu-v2/nsh/defconfig +++ b/nuttx-configs/px4fmu-v2/nsh/defconfig @@ -801,11 +801,11 @@ CONFIG_SCHED_WORKQUEUE=y CONFIG_SCHED_HPWORK=y CONFIG_SCHED_WORKPRIORITY=192 CONFIG_SCHED_WORKPERIOD=5000 -CONFIG_SCHED_WORKSTACKSIZE=1800 +CONFIG_SCHED_WORKSTACKSIZE=1600 CONFIG_SCHED_LPWORK=y CONFIG_SCHED_LPWORKPRIORITY=50 CONFIG_SCHED_LPWORKPERIOD=50000 -CONFIG_SCHED_LPWORKSTACKSIZE=1800 +CONFIG_SCHED_LPWORKSTACKSIZE=1600 # CONFIG_LIB_KBDCODEC is not set # CONFIG_LIB_SLCDCODEC is not set From b83f56bc1d77cd67499c60d5e86f0d4435282a99 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Wed, 14 Oct 2015 17:31:09 +0200 Subject: [PATCH 28/44] sdlog2: Allocate log buffer as needed, not on start. This allows the system to leverage the log buffer RAM for UAVCAN first --- src/modules/sdlog2/logbuffer.c | 14 ++++++++++++-- 1 file changed, 12 insertions(+), 2 deletions(-) diff --git a/src/modules/sdlog2/logbuffer.c b/src/modules/sdlog2/logbuffer.c index 9e25dd2f23..2768994dde 100644 --- a/src/modules/sdlog2/logbuffer.c +++ b/src/modules/sdlog2/logbuffer.c @@ -50,8 +50,8 @@ int logbuffer_init(struct logbuffer_s *lb, int size) lb->size = size; lb->write_ptr = 0; lb->read_ptr = 0; - lb->data = malloc(lb->size); - return (lb->data == 0) ? PX4_ERROR : PX4_OK; + lb->data = NULL; + return PX4_OK; } int logbuffer_count(struct logbuffer_s *lb) @@ -72,6 +72,16 @@ int logbuffer_is_empty(struct logbuffer_s *lb) bool logbuffer_write(struct logbuffer_s *lb, void *ptr, int size) { + // allocate buffer if not yet present + if (lb->data == NULL) { + lb->data = malloc(lb->size); + } + + // allocation failed, bail out + if (lb->data == NULL) { + return false; + } + // bytes available to write int available = lb->read_ptr - lb->write_ptr - 1; From 110a630cbd01b18163dfca9c53361711584c2a49 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Wed, 14 Oct 2015 18:06:28 +0200 Subject: [PATCH 29/44] Fix PX4 log to show error --- src/platforms/px4_log.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/platforms/px4_log.h b/src/platforms/px4_log.h index 2a0ff33773..55bf3a55bc 100644 --- a/src/platforms/px4_log.h +++ b/src/platforms/px4_log.h @@ -130,7 +130,7 @@ __EXPORT extern int __px4_log_level_current; __END_DECLS // __px4_log_level_current will be initialized to PX4_LOG_LEVEL_AT_RUN_TIME -#define PX4_LOG_LEVEL_AT_RUN_TIME _PX4_LOG_LEVEL_WARN +#define PX4_LOG_LEVEL_AT_RUN_TIME _PX4_LOG_LEVEL_ERROR /**************************************************************************** * Implementation of log section formatting based on printf From 626edc6a8d593a9f1a1d28e836cb6e72e3410fd8 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Wed, 14 Oct 2015 18:06:41 +0200 Subject: [PATCH 30/44] EKF: Add error macro when running into errors --- .../ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) 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 615ad143aa..861b56c60f 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 @@ -1691,6 +1691,6 @@ int ekf_att_pos_estimator_main(int argc, char *argv[]) return ret; } - PX4_WARN("unrecognized command"); + PX4_ERR("unrecognized command"); return 1; } From 75b356478927c0aeb8cd0ac71f451f1e763856cf Mon Sep 17 00:00:00 2001 From: Daniel Agar Date: Wed, 14 Oct 2015 10:52:09 -0400 Subject: [PATCH 31/44] cmake better nuttx copy --- cmake/nuttx/px4_impl_nuttx.cmake | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/cmake/nuttx/px4_impl_nuttx.cmake b/cmake/nuttx/px4_impl_nuttx.cmake index 2586478383..db8f11ef00 100644 --- a/cmake/nuttx/px4_impl_nuttx.cmake +++ b/cmake/nuttx/px4_impl_nuttx.cmake @@ -213,8 +213,8 @@ function(px4_nuttx_add_export) # copy add_custom_command(OUTPUT nuttx_copy_${CONFIG}.stamp COMMAND ${MKDIR} -p ${CMAKE_BINARY_DIR}/${CONFIG} - COMMAND ${RM} -rf ${nuttx_src} - COMMAND ${CP} -r ${CMAKE_SOURCE_DIR}/NuttX ${nuttx_src} + COMMAND ${MKDIR} -p ${nuttx_src} + COMMAND ${CP} -a ${CMAKE_SOURCE_DIR}/NuttX/. ${nuttx_src}/ COMMAND ${RM} -rf ${nuttx_src}/.git COMMAND ${TOUCH} nuttx_copy_${CONFIG}.stamp DEPENDS ${DEPENDS}) From 8e039c40790ff18c41d4bd0d3529f51407c7f673 Mon Sep 17 00:00:00 2001 From: Daniel Agar Date: Wed, 14 Oct 2015 10:52:45 -0400 Subject: [PATCH 32/44] cmake uavcan remove whitespace --- src/modules/uavcan/CMakeLists.txt | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/uavcan/CMakeLists.txt b/src/modules/uavcan/CMakeLists.txt index c2bf52247d..b8c899840a 100644 --- a/src/modules/uavcan/CMakeLists.txt +++ b/src/modules/uavcan/CMakeLists.txt @@ -81,7 +81,7 @@ px4_add_module( DEPENDS platforms__common - uavcan + uavcan ) ## vim: set noet ft=cmake fenc=utf-8 ff=unix : From 77f2295b62513d77878c6f1726e0e86b807ed3c9 Mon Sep 17 00:00:00 2001 From: Daniel Agar Date: Wed, 14 Oct 2015 10:53:55 -0400 Subject: [PATCH 33/44] cmake restore -Wbad-function-cast --- cmake/common/px4_base.cmake | 1 + 1 file changed, 1 insertion(+) diff --git a/cmake/common/px4_base.cmake b/cmake/common/px4_base.cmake index 9a0b17b7a2..6853b4f161 100644 --- a/cmake/common/px4_base.cmake +++ b/cmake/common/px4_base.cmake @@ -543,6 +543,7 @@ function(px4_add_common_flags) endif() set(c_warnings + -Wbad-function-cast -Wstrict-prototypes -Wmissing-prototypes -Wnested-externs From adc59b9e7186100022c3f177cc9895872bde59a9 Mon Sep 17 00:00:00 2001 From: Daniel Agar Date: Wed, 14 Oct 2015 11:05:51 -0400 Subject: [PATCH 34/44] cmake improve submodule handling --- Makefile | 23 ++---- Tools/check_submodules.sh | 144 ------------------------------------ cmake/common/px4_base.cmake | 13 ++-- 3 files changed, 14 insertions(+), 166 deletions(-) delete mode 100755 Tools/check_submodules.sh diff --git a/Makefile b/Makefile index 2b9a34553f..e2f1814f0c 100644 --- a/Makefile +++ b/Makefile @@ -89,7 +89,7 @@ endif # -------------------------------------------------------------------- # describe how to build a cmake config define cmake-build -+@if [ ! -e $(PWD)/build_$@/CMakeCache.txt ]; then mkdir -p $(PWD)/build_$@ && cd $(PWD)/build_$@ && cmake .. -G$(PX4_CMAKE_GENERATOR) -DCONFIG=$(1); fi ++@if [ ! -e $(PWD)/build_$@/CMakeCache.txt ]; then git submodule update --init --recursive --force && mkdir -p $(PWD)/build_$@ && cd $(PWD)/build_$@ && cmake .. -G$(PX4_CMAKE_GENERATOR) -DCONFIG=$(1); fi +$(PX4_MAKE) -C $(PWD)/build_$@ $(PX4_MAKE_ARGS) $(ARGS) endef @@ -104,13 +104,13 @@ endef # -------------------------------------------------------------------- # Do not put any spaces between function arguments. -px4fmu-v1_default: git-init +px4fmu-v1_default: $(call cmake-build,nuttx_px4fmu-v1_default) -px4fmu-v2_default: git-init +px4fmu-v2_default: $(call cmake-build,nuttx_px4fmu-v2_default) -px4fmu-v2_simple: git-init +px4fmu-v2_simple: $(call cmake-build,nuttx_px4fmu-v2_simple) nuttx_sim_simple: @@ -172,19 +172,8 @@ check_format: clean: @rm -rf build_*/ - -distclean: clean - @cd NuttX - @git clean -d -f -x - @cd .. - @cd src/modules/uavcan/libuavcan - @git clean -d -f -x - @cd ../../../.. - -# XXX this is not the right way to fix it, but we need a temporary solution -# for average joe -git-init: - @git submodule update --init --recursive + @(cd NuttX && git clean -d -f -x) + @(cd src/modules/uavcan/libuavcan && git clean -d -f -x) # targets handled by cmake cmake_targets = test upload package package_source debug debug_tui debug_ddd debug_io debug_io_tui debug_io_ddd check_weak libuavcan diff --git a/Tools/check_submodules.sh b/Tools/check_submodules.sh deleted file mode 100755 index 5fa0831438..0000000000 --- a/Tools/check_submodules.sh +++ /dev/null @@ -1,144 +0,0 @@ -#!/bin/sh - -[ -n "$GIT_SUBMODULES_ARE_EVIL" ] && { - # GIT_SUBMODULES_ARE_EVIL is set, meaning user doesn't want submodules - echo "Skipping submodules. NUTTX_SRC is set to $NUTTX_SRC" - exit 0 -} - -if [ -d NuttX/nuttx ]; - then - STATUSRETVAL=$(git submodule summary | grep -A20 -i "NuttX" | grep "<") - if [ -z "$STATUSRETVAL" ]; then - echo "Checked NuttX submodule, correct version found" - else - echo "" - echo "" - echo "New commits required:" - echo "$(git submodule summary)" - echo "" - echo "" - echo " NuttX sub repo not at correct version. Try 'git submodule update'" - echo " or follow instructions on http://pixhawk.org/dev/git/submodules" - echo "" - echo " DO NOT FORGET TO RUN 'make distclean && make archives' AFTER EACH NUTTX UPDATE!" - exit 1 - fi -else - git submodule update --init --recursive -fi - - -if [ -d mavlink/include/mavlink/v1.0 ]; - then - STATUSRETVAL=$(git submodule summary | grep -A20 -i "mavlink/include/mavlink/v1.0" | grep "<") - if [ -z "$STATUSRETVAL" ]; then - echo "Checked mavlink submodule, correct version found" - else - echo "" - echo "" - echo "New commits required:" - echo "$(git submodule summary)" - echo "" - echo "" - echo "mavlink sub repo not at correct version. Try 'git submodule update'" - echo "or follow instructions on http://pixhawk.org/dev/git/submodules" - exit 1 - fi -else - git submodule update --init --recursive -fi - - -if [ -d uavcan ] -then - STATUSRETVAL=$(git submodule summary | grep -A20 -i uavcan | grep "<") - if [ -z "$STATUSRETVAL" ] - then - echo "Checked uavcan submodule, correct version found" - else - echo "" - echo "" - echo "New commits required:" - echo "$(git submodule summary)" - echo "" - echo "" - echo "uavcan sub repo not at correct version. Try 'git submodule update'" - echo "or follow instructions on http://pixhawk.org/dev/git/submodules" - exit 1 - fi -else - git submodule update --init --recursive -fi - -if [ -d src/lib/eigen ] -then - echo "ARG = $1" - if [ $1 = "qurt" ] - then - # QuRT needs to use Eigen 3.2 because the toolchain doews not support C++11 - STATUSRETVAL=$(true) - else - STATUSRETVAL=$(git submodule summary | grep -A20 -i eigen | grep "<") - if [ -z "$STATUSRETVAL" ] - then - echo "Checked Eigen submodule, correct version found" - else - echo "" - echo "" - echo "New commits required:" - echo "$(git submodule summary)" - echo "" - echo "" - echo "eigen sub repo not at correct version. Try 'git submodule update'" - echo "or follow instructions on http://pixhawk.org/dev/git/submodules" - exit 1 - fi - fi -else - git submodule update --init --recursive -fi - -if [ -d Tools/gencpp ] -then - STATUSRETVAL=$(git submodule summary | grep -A20 -i gencpp | grep "<") - if [ -z "$STATUSRETVAL" ] - then - echo "Checked gencpp submodule, correct version found" - else - echo "" - echo "" - echo "New commits required:" - echo "$(git submodule summary)" - echo "" - echo "" - echo "gencpp sub repo not at correct version. Try 'git submodule update'" - echo "or follow instructions on http://pixhawk.org/dev/git/submodules" - exit 1 - fi -else - git submodule update --init --recursive -fi - -if [ -d Tools/genmsg ] -then - STATUSRETVAL=$(git submodule summary | grep -A20 -i genmsg | grep "<") - if [ -z "$STATUSRETVAL" ] - then - echo "Checked genmsg submodule, correct version found" - else - echo "" - echo "" - echo "New commits required:" - echo "$(git submodule summary)" - echo "" - echo "" - echo "genmsg sub repo not at correct version. Try 'git submodule update'" - echo "or follow instructions on http://pixhawk.org/dev/git/submodules" - exit 1 - fi -else - git submodule update --init --recursive -fi - -exit 0 diff --git a/cmake/common/px4_base.cmake b/cmake/common/px4_base.cmake index 6853b4f161..086f77070f 100644 --- a/cmake/common/px4_base.cmake +++ b/cmake/common/px4_base.cmake @@ -136,14 +136,17 @@ function(px4_add_git_submodule) REQUIRED TARGET PATH ARGN ${ARGN}) string(REPLACE "/" "_" NAME ${PATH}) - add_custom_command(OUTPUT ${CMAKE_BINARY_DIR}/git_${NAME}.stamp + add_custom_command(OUTPUT ${CMAKE_BINARY_DIR}/git_init_${NAME}.stamp WORKING_DIRECTORY ${CMAKE_SOURCE_DIR} - COMMAND git submodule update --init --recursive -f ${PATH} - COMMAND touch ${CMAKE_BINARY_DIR}/git_${NAME}.stamp + COMMAND git submodule init ${PATH} + COMMAND touch ${CMAKE_BINARY_DIR}/git_init_${NAME}.stamp + DEPENDS ${CMAKE_SOURCE_DIR}/.gitmodules ) add_custom_target(${TARGET} - DEPENDS git_${NAME}.stamp - ) + WORKING_DIRECTORY ${CMAKE_SOURCE_DIR} + COMMAND git submodule update --recursive ${PATH} + DEPENDS ${CMAKE_BINARY_DIR}/git_init_${NAME}.stamp + ) endfunction() #============================================================================= From e2fda0154540f5f1d87287dd5861a2335c114eab Mon Sep 17 00:00:00 2001 From: Daniel Agar Date: Wed, 14 Oct 2015 12:19:39 -0400 Subject: [PATCH 35/44] nuttx only show stderr -failures will still be displayed, but we don't need to see normal successful compile output --- cmake/nuttx/px4_impl_nuttx.cmake | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/cmake/nuttx/px4_impl_nuttx.cmake b/cmake/nuttx/px4_impl_nuttx.cmake index db8f11ef00..af7391f45c 100644 --- a/cmake/nuttx/px4_impl_nuttx.cmake +++ b/cmake/nuttx/px4_impl_nuttx.cmake @@ -229,7 +229,7 @@ function(px4_nuttx_add_export) COMMAND ${CP} -r ${CMAKE_SOURCE_DIR}/nuttx-configs/${CONFIG} ${nuttx_src}/nuttx/configs COMMAND cd ${nuttx_src}/nuttx/tools && ./configure.sh ${CONFIG}/nsh COMMAND ${ECHO} Exporting NuttX for ${CONFIG} - COMMAND ${MAKE} --no-print-directory --quiet -C ${nuttx_src}/nuttx -j${THREADS} -r CONFIG_ARCH_BOARD=${CONFIG} export + COMMAND ${MAKE} --no-print-directory --quiet -C ${nuttx_src}/nuttx -j${THREADS} -r CONFIG_ARCH_BOARD=${CONFIG} export > /dev/null COMMAND ${CP} -r ${nuttx_src}/nuttx/nuttx-export.zip ${CMAKE_BINARY_DIR}/${CONFIG}.export DEPENDS ${config_files} ${DEPENDS} __nuttx_copy_${CONFIG}) From ca5e1bd62b2a15fe202ae914a42bb35d115b6c56 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 27 Sep 2015 13:36:55 +0200 Subject: [PATCH 36/44] Drivers: Add calibration check IOCTL, not implemented yet in sensor drivers. --- src/drivers/drv_sensor.h | 5 +++++ 1 file changed, 5 insertions(+) diff --git a/src/drivers/drv_sensor.h b/src/drivers/drv_sensor.h index a402f327b7..0c1ab48267 100644 --- a/src/drivers/drv_sensor.h +++ b/src/drivers/drv_sensor.h @@ -121,5 +121,10 @@ */ #define SENSORIOCGROTATION _SENSORIOC(6) +/** + * Test the sensor calibration + */ +#define SENSORIOCCALTEST _SENSORIOC(7) + #endif /* _DRV_SENSOR_H */ From 7238916d035d6550d8a614d865da90e97cb2c706 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 27 Sep 2015 13:37:18 +0200 Subject: [PATCH 37/44] Sensors: Reload calibration whenever a new sensor instance is detected --- src/modules/sensors/sensors.cpp | 14 ++++++++++++++ 1 file changed, 14 insertions(+) diff --git a/src/modules/sensors/sensors.cpp b/src/modules/sensors/sensors.cpp index 60e1a9aabb..61eab348d7 100644 --- a/src/modules/sensors/sensors.cpp +++ b/src/modules/sensors/sensors.cpp @@ -2020,6 +2020,11 @@ Sensors::task_main() * do subscriptions */ + int gcount_prev = _gyro_count; + int mcount_prev = _mag_count; + int acount_prev = _accel_count; + int bcount_prev = _baro_count; + _gyro_count = init_sensor_class(ORB_ID(sensor_gyro), &_gyro_sub[0], &raw.gyro_priority[0], &raw.gyro_errcount[0]); @@ -2032,6 +2037,15 @@ Sensors::task_main() _baro_count = init_sensor_class(ORB_ID(sensor_baro), &_baro_sub[0], &raw.baro_priority[0], &raw.baro_errcount[0]); + if (gcount_prev != _gyro_count || + mcount_prev != _mag_count || + acount_prev != _accel_count || + bcount_prev != _baro_count) { + + /* reload calibration params */ + parameter_update_poll(true); + } + _rc_sub = orb_subscribe(ORB_ID(input_rc)); _diff_pres_sub = orb_subscribe(ORB_ID(differential_pressure)); _vcontrol_mode_sub = orb_subscribe(ORB_ID(vehicle_control_mode)); From a7c6a343c6ea0d27f007ee250415ea3bef81514e Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 27 Sep 2015 13:37:56 +0200 Subject: [PATCH 38/44] Commander: Do not enforce sensor order, only enforce that all present sensors need to be calibrated. --- src/modules/commander/PreflightCheck.cpp | 63 +++++++++++++++++------- 1 file changed, 45 insertions(+), 18 deletions(-) diff --git a/src/modules/commander/PreflightCheck.cpp b/src/modules/commander/PreflightCheck.cpp index bbf5f8ec6b..883a851f26 100644 --- a/src/modules/commander/PreflightCheck.cpp +++ b/src/modules/commander/PreflightCheck.cpp @@ -71,6 +71,45 @@ namespace Commander { + +static int check_calibration(int fd, const char* param_template); + +int check_calibration(int fd, const char* param_template) +{ + bool calibration_found; + + /* new style: ask device for calibration state */ + ret = px4_ioctl(fd, SENSORIOCCALTEST, 0); + + calibration_found = (ret == OK); + + /* old style transition: check param values */ + int devid = px4_ioctl(fd, DEVIOCGDEVICEID, 0); + + while (!calibration_found) { + sprintf(s, param_template, instance); + param_t parm = param_find(s); + + /* if the calibration param is not present, abort */ + if (parm == PARAM_INVALID) { + break; + } + + /* if param get succeeds */ + int calibration_devid; + if (!param_get(parm, &(calibration_devid))) { + + /* if the devid matches, exit early */ + if (devid == calibration_devid) { + calibration_found = true; + break; + } + } + } + + return !calibration_found; +} + static bool magnometerCheck(int mavlink_fd, unsigned instance, bool optional) { bool success = true; @@ -88,13 +127,9 @@ static bool magnometerCheck(int mavlink_fd, unsigned instance, bool optional) return false; } - int calibration_devid; - int ret; - int devid = px4_ioctl(fd, DEVIOCGDEVICEID, 0); - sprintf(s, "CAL_MAG%u_ID", instance); - param_get(param_find(s), &(calibration_devid)); + int ret = check_calibration(fd, "CAL_MAG%u_ID"); - if (devid != calibration_devid) { + if (ret) { mavlink_and_console_log_critical(mavlink_fd, "PREFLIGHT FAIL: MAG #%u UNCALIBRATED", instance); success = false; @@ -132,13 +167,9 @@ static bool accelerometerCheck(int mavlink_fd, unsigned instance, bool optional, return false; } - int calibration_devid; - int ret; - int devid = px4_ioctl(fd, DEVIOCGDEVICEID, 0); - sprintf(s, "CAL_ACC%u_ID", instance); - param_get(param_find(s), &(calibration_devid)); + int ret = check_calibration(fd, "CAL_ACC%u_ID"); - if (devid != calibration_devid) { + if (ret) { mavlink_and_console_log_critical(mavlink_fd, "PREFLIGHT FAIL: ACCEL #%u UNCALIBRATED", instance); success = false; @@ -201,13 +232,9 @@ static bool gyroCheck(int mavlink_fd, unsigned instance, bool optional) return false; } - int calibration_devid; - int ret; - int devid = px4_ioctl(fd, DEVIOCGDEVICEID, 0); - sprintf(s, "CAL_GYRO%u_ID", instance); - param_get(param_find(s), &(calibration_devid)); + int ret = check_calibration(fd, "CAL_GYRO%u_ID"); - if (devid != calibration_devid) { + if (ret) { mavlink_and_console_log_critical(mavlink_fd, "PREFLIGHT FAIL: GYRO #%u UNCALIBRATED", instance); success = false; From e5bb1cff91cb7537c3ca99739a406a65a5fb6948 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 27 Sep 2015 14:13:39 +0200 Subject: [PATCH 39/44] Store primary sensor ID to allow cross-check of calibration on next boot --- src/modules/commander/PreflightCheck.cpp | 67 +++++++++++++++++-- .../commander/accelerometer_calibration.cpp | 12 ++++ src/modules/commander/gyro_calibration.cpp | 14 ++++ src/modules/commander/mag_calibration.cpp | 13 ++++ 4 files changed, 101 insertions(+), 5 deletions(-) diff --git a/src/modules/commander/PreflightCheck.cpp b/src/modules/commander/PreflightCheck.cpp index 883a851f26..61aa461a91 100644 --- a/src/modules/commander/PreflightCheck.cpp +++ b/src/modules/commander/PreflightCheck.cpp @@ -110,7 +110,7 @@ int check_calibration(int fd, const char* param_template) return !calibration_found; } -static bool magnometerCheck(int mavlink_fd, unsigned instance, bool optional) +static bool magnometerCheck(int mavlink_fd, unsigned instance, bool optional, int &device_id) { bool success = true; @@ -338,50 +338,107 @@ bool preflightCheck(int mavlink_fd, bool checkMag, bool checkAcc, bool checkGyro /* ---- MAG ---- */ if (checkMag) { + bool prime_found = false; + int32_t prime_id = 0; + param_get(param_find("CAL_MAG_PRIME"), &prime_id); + /* check all sensors, but fail only for mandatory ones */ for (unsigned i = 0; i < max_optional_mag_count; i++) { bool required = (i < max_mandatory_mag_count); + int device_id; - if (!magnometerCheck(mavlink_fd, i, !required) && required) { + if (!magnometerCheck(mavlink_fd, i, !required, device_id) && required) { failed = true; } + + if (device_id == prime_id) { + prime_found = true; + } + } + + /* check if the primary device is present */ + if (!prime_found) { + //failed = true; } } /* ---- ACCEL ---- */ if (checkAcc) { + bool prime_found = false; + int32_t prime_id = 0; + param_get(param_find("CAL_ACC_PRIME"), &prime_id); + /* check all sensors, but fail only for mandatory ones */ for (unsigned i = 0; i < max_optional_accel_count; i++) { bool required = (i < max_mandatory_accel_count); + int device_id - if (!accelerometerCheck(mavlink_fd, i, !required, checkDynamic) && required) { + if (!accelerometerCheck(mavlink_fd, i, !required, checkDynamic, device_id) && required) { failed = true; } + + if (device_id == prime_id) { + prime_found = true; + } + } + + /* check if the primary device is present */ + if (!prime_found) { + //failed = true; } } /* ---- GYRO ---- */ if (checkGyro) { + bool prime_found = false; + int32_t prime_id = 0; + param_get(param_find("CAL_GYRO_PRIME"), &prime_id); + /* check all sensors, but fail only for mandatory ones */ for (unsigned i = 0; i < max_optional_gyro_count; i++) { bool required = (i < max_mandatory_gyro_count); + int device_id; - if (!gyroCheck(mavlink_fd, i, !required) && required) { + if (!gyroCheck(mavlink_fd, i, !required, device_id) && required) { failed = true; } + + if (device_id == prime_id) { + prime_found = true; + } + } + + /* check if the primary device is present */ + if (!prime_found) { + //failed = true; } } /* ---- BARO ---- */ if (checkBaro) { + bool prime_found = false; + int32_t prime_id = 0; + param_get(param_find("CAL_BARO_PRIME"), &prime_id); + /* check all sensors, but fail only for mandatory ones */ for (unsigned i = 0; i < max_optional_baro_count; i++) { bool required = (i < max_mandatory_baro_count); + int device_id; - if (!baroCheck(mavlink_fd, i, !required) && required) { + if (!baroCheck(mavlink_fd, i, !required, device_id) && required) { failed = true; } + + if (device_id == prime_id) { + prime_found = true; + } } + + // TODO there is no logic in place to calibrate the primary baro yet + // // check if the primary device is present + // if (!prime_found) { + // failed = true; + // } } /* ---- AIRSPEED ---- */ diff --git a/src/modules/commander/accelerometer_calibration.cpp b/src/modules/commander/accelerometer_calibration.cpp index 6a38d54cb6..2c52489fdf 100644 --- a/src/modules/commander/accelerometer_calibration.cpp +++ b/src/modules/commander/accelerometer_calibration.cpp @@ -173,6 +173,8 @@ int do_accel_calibration(int mavlink_fd) { int fd; int32_t device_id[max_accel_sens]; + int device_prio_max = 0; + int32_t device_id_primary = 0; mavlink_and_console_log_info(mavlink_fd, CAL_QGC_STARTED_MSG, sensor_name); @@ -259,6 +261,8 @@ int do_accel_calibration(int mavlink_fd) bool failed = false; + failed = failed || (OK != param_set_no_notification("CAL_ACC_PRIME", &(device_id_primary))); + /* set parameters */ (void)sprintf(str, "CAL_ACC%u_XOFF", i); failed |= (OK != param_set_no_notification(param_find(str), &(accel_scale.x_offset))); @@ -370,6 +374,14 @@ calibrate_return do_accel_calibration_measurements(int mavlink_fd, float (&accel struct accel_report arp = {}; (void)orb_copy(ORB_ID(sensor_accel), worker_data.subs[i], &arp); timestamps[i] = arp.timestamp; + + // Get priority + int32_t prio = orb_priority(work_data.subs[i]); + + if (prio > device_prio_max) { + device_prio_max = prio; + device_id_primary = device_id[i]; + } } if (result == calibrate_return_ok) { diff --git a/src/modules/commander/gyro_calibration.cpp b/src/modules/commander/gyro_calibration.cpp index 817cbcdb0e..6c8c840713 100644 --- a/src/modules/commander/gyro_calibration.cpp +++ b/src/modules/commander/gyro_calibration.cpp @@ -168,6 +168,9 @@ int do_gyro_calibration(int mavlink_fd) 1.0f, // z scale }; + int device_prio_max = 0; + int32_t device_id_primary = 0; + for (unsigned s = 0; s < max_gyros; s++) { char str[30]; @@ -199,6 +202,14 @@ int do_gyro_calibration(int mavlink_fd) for (unsigned s = 0; s < max_gyros; s++) { worker_data.gyro_sensor_sub[s] = orb_subscribe_multi(ORB_ID(sensor_gyro), s); + + // Get priority + int32_t prio = orb_priority(work_data.gyro_sensor_subs[s]); + + if (prio > device_prio_max) { + device_prio_max = prio; + device_id_primary = worker_data.device_id[s]; + } } int cancel_sub = calibrate_cancel_subscribe(); @@ -258,9 +269,12 @@ int do_gyro_calibration(int mavlink_fd) } if (res == OK) { + /* set offset parameters to new values */ bool failed = false; + failed = failed || (OK != param_set_no_notification("CAL_GYRO_PRIME", &(device_id_primary))); + for (unsigned s = 0; s < max_gyros; s++) { if (worker_data.device_id[s] != 0) { char str[30]; diff --git a/src/modules/commander/mag_calibration.cpp b/src/modules/commander/mag_calibration.cpp index 4511882c7b..c9bd48a5cc 100644 --- a/src/modules/commander/mag_calibration.cpp +++ b/src/modules/commander/mag_calibration.cpp @@ -109,6 +109,8 @@ int do_mag_calibration(int mavlink_fd) // Determine which mags are available and reset each int32_t device_ids[max_mags]; + int device_prio_max = 0; + int32_t device_id_primary = 0; char str[30]; for (size_t i=0; i device_prio_max) { + device_prio_max = prio; + device_id_primary = device_ids[cur_mag]; + } } } } @@ -550,6 +560,9 @@ calibrate_return mag_calibrate_all(int mavlink_fd, int32_t (&device_ids)[max_mag } if (result == calibrate_return_ok) { + + (void)param_set_no_notification("CAL_MAG_PRIME", &(device_id_primary)); + for (unsigned cur_mag=0; cur_mag Date: Tue, 6 Oct 2015 19:44:17 +0200 Subject: [PATCH 40/44] Commander: Finish preflight update for prime sensor IDs --- src/modules/commander/PreflightCheck.cpp | 30 ++++++++++++++----- .../commander/accelerometer_calibration.cpp | 12 ++++---- src/modules/commander/gyro_calibration.cpp | 5 ++-- src/modules/commander/mag_calibration.cpp | 12 ++++---- 4 files changed, 40 insertions(+), 19 deletions(-) diff --git a/src/modules/commander/PreflightCheck.cpp b/src/modules/commander/PreflightCheck.cpp index 61aa461a91..cdddd3b637 100644 --- a/src/modules/commander/PreflightCheck.cpp +++ b/src/modules/commander/PreflightCheck.cpp @@ -79,13 +79,16 @@ int check_calibration(int fd, const char* param_template) bool calibration_found; /* new style: ask device for calibration state */ - ret = px4_ioctl(fd, SENSORIOCCALTEST, 0); + int ret = px4_ioctl(fd, SENSORIOCCALTEST, 0); calibration_found = (ret == OK); /* old style transition: check param values */ int devid = px4_ioctl(fd, DEVIOCGDEVICEID, 0); + char s[20]; + int instance = 0; + while (!calibration_found) { sprintf(s, param_template, instance); param_t parm = param_find(s); @@ -105,6 +108,7 @@ int check_calibration(int fd, const char* param_template) break; } } + instance++; } return !calibration_found; @@ -150,7 +154,7 @@ out: return success; } -static bool accelerometerCheck(int mavlink_fd, unsigned instance, bool optional, bool dynamic) +static bool accelerometerCheck(int mavlink_fd, unsigned instance, bool optional, bool dynamic, int &device_id) { bool success = true; @@ -215,7 +219,7 @@ out: return success; } -static bool gyroCheck(int mavlink_fd, unsigned instance, bool optional) +static bool gyroCheck(int mavlink_fd, unsigned instance, bool optional, int &device_id) { bool success = true; @@ -255,7 +259,7 @@ out: return success; } -static bool baroCheck(int mavlink_fd, unsigned instance, bool optional) +static bool baroCheck(int mavlink_fd, unsigned instance, bool optional, int &device_id) { bool success = true; @@ -272,6 +276,18 @@ static bool baroCheck(int mavlink_fd, unsigned instance, bool optional) return false; } + // TODO: There is no baro calibration yet, since no external baros exist + // int ret = check_calibration(fd, "CAL_BARO%u_ID"); + + // if (ret) { + // mavlink_and_console_log_critical(mavlink_fd, + // "PREFLIGHT FAIL: BARO #%u UNCALIBRATED", instance); + // success = false; + // goto out; + // } + +//out: + px4_close(fd); return success; } @@ -371,7 +387,7 @@ bool preflightCheck(int mavlink_fd, bool checkMag, bool checkAcc, bool checkGyro /* check all sensors, but fail only for mandatory ones */ for (unsigned i = 0; i < max_optional_accel_count; i++) { bool required = (i < max_mandatory_accel_count); - int device_id + int device_id; if (!accelerometerCheck(mavlink_fd, i, !required, checkDynamic, device_id) && required) { failed = true; @@ -436,9 +452,9 @@ bool preflightCheck(int mavlink_fd, bool checkMag, bool checkAcc, bool checkGyro // TODO there is no logic in place to calibrate the primary baro yet // // check if the primary device is present - // if (!prime_found) { + if (!prime_found) { // failed = true; - // } + } } /* ---- AIRSPEED ---- */ diff --git a/src/modules/commander/accelerometer_calibration.cpp b/src/modules/commander/accelerometer_calibration.cpp index 2c52489fdf..d7a58e6254 100644 --- a/src/modules/commander/accelerometer_calibration.cpp +++ b/src/modules/commander/accelerometer_calibration.cpp @@ -156,6 +156,10 @@ static const int ERROR = -1; static const char *sensor_name = "accel"; +static int32_t device_id[max_accel_sens]; +static int device_prio_max = 0; +static int32_t device_id_primary = 0; + calibrate_return do_accel_calibration_measurements(int mavlink_fd, float (&accel_offs)[max_accel_sens][3], float (&accel_T)[max_accel_sens][3][3], unsigned *active_sensors); calibrate_return read_accelerometer_avg(int (&subs)[max_accel_sens], float (&accel_avg)[max_accel_sens][detect_orientation_side_count][3], unsigned orient, unsigned samples_num); int mat_invert3(float src[3][3], float dst[3][3]); @@ -172,9 +176,6 @@ typedef struct { int do_accel_calibration(int mavlink_fd) { int fd; - int32_t device_id[max_accel_sens]; - int device_prio_max = 0; - int32_t device_id_primary = 0; mavlink_and_console_log_info(mavlink_fd, CAL_QGC_STARTED_MSG, sensor_name); @@ -261,7 +262,7 @@ int do_accel_calibration(int mavlink_fd) bool failed = false; - failed = failed || (OK != param_set_no_notification("CAL_ACC_PRIME", &(device_id_primary))); + failed = failed || (OK != param_set_no_notification(param_find("CAL_ACC_PRIME"), &(device_id_primary))); /* set parameters */ (void)sprintf(str, "CAL_ACC%u_XOFF", i); @@ -376,7 +377,8 @@ calibrate_return do_accel_calibration_measurements(int mavlink_fd, float (&accel timestamps[i] = arp.timestamp; // Get priority - int32_t prio = orb_priority(work_data.subs[i]); + int32_t prio; + orb_priority(worker_data.subs[i], &prio); if (prio > device_prio_max) { device_prio_max = prio; diff --git a/src/modules/commander/gyro_calibration.cpp b/src/modules/commander/gyro_calibration.cpp index 6c8c840713..54866d8cb3 100644 --- a/src/modules/commander/gyro_calibration.cpp +++ b/src/modules/commander/gyro_calibration.cpp @@ -204,7 +204,8 @@ int do_gyro_calibration(int mavlink_fd) worker_data.gyro_sensor_sub[s] = orb_subscribe_multi(ORB_ID(sensor_gyro), s); // Get priority - int32_t prio = orb_priority(work_data.gyro_sensor_subs[s]); + int32_t prio; + orb_priority(worker_data.gyro_sensor_sub[s], &prio); if (prio > device_prio_max) { device_prio_max = prio; @@ -273,7 +274,7 @@ int do_gyro_calibration(int mavlink_fd) /* set offset parameters to new values */ bool failed = false; - failed = failed || (OK != param_set_no_notification("CAL_GYRO_PRIME", &(device_id_primary))); + failed = failed || (OK != param_set_no_notification(param_find("CAL_GYRO_PRIME"), &(device_id_primary))); for (unsigned s = 0; s < max_gyros; s++) { if (worker_data.device_id[s] != 0) { diff --git a/src/modules/commander/mag_calibration.cpp b/src/modules/commander/mag_calibration.cpp index c9bd48a5cc..eba8feec99 100644 --- a/src/modules/commander/mag_calibration.cpp +++ b/src/modules/commander/mag_calibration.cpp @@ -73,6 +73,10 @@ static constexpr unsigned int calibration_sides = 6; ///< The total number of static constexpr unsigned int calibration_total_points = 240; ///< The total points per magnetometer static constexpr unsigned int calibraton_duration_seconds = 42; ///< The total duration the routine is allowed to take +int32_t device_ids[max_mags]; +int device_prio_max = 0; +int32_t device_id_primary = 0; + calibrate_return mag_calibrate_all(int mavlink_fd, int32_t (&device_ids)[max_mags]); /// Data passed to calibration worker routine @@ -108,9 +112,6 @@ int do_mag_calibration(int mavlink_fd) // Determine which mags are available and reset each - int32_t device_ids[max_mags]; - int device_prio_max = 0; - int32_t device_id_primary = 0; char str[30]; for (size_t i=0; i device_prio_max) { device_prio_max = prio; @@ -561,7 +563,7 @@ calibrate_return mag_calibrate_all(int mavlink_fd, int32_t (&device_ids)[max_mag if (result == calibrate_return_ok) { - (void)param_set_no_notification("CAL_MAG_PRIME", &(device_id_primary)); + (void)param_set_no_notification(param_find("CAL_MAG_PRIME"), &(device_id_primary)); for (unsigned cur_mag=0; cur_mag Date: Tue, 6 Oct 2015 19:44:50 +0200 Subject: [PATCH 41/44] sensors app: Finish preflight update --- src/modules/sensors/CMakeLists.txt | 1 - src/modules/sensors/sensor_params.c | 28 ++++++++++++++++++++++++++++ 2 files changed, 28 insertions(+), 1 deletion(-) diff --git a/src/modules/sensors/CMakeLists.txt b/src/modules/sensors/CMakeLists.txt index fdfc2dd774..e296ebe103 100644 --- a/src/modules/sensors/CMakeLists.txt +++ b/src/modules/sensors/CMakeLists.txt @@ -40,7 +40,6 @@ px4_add_module( -O3 SRCS sensors.cpp - sensor_params.c DEPENDS platforms__common ) diff --git a/src/modules/sensors/sensor_params.c b/src/modules/sensors/sensor_params.c index 1db6ed1b77..393cd63a06 100644 --- a/src/modules/sensors/sensor_params.c +++ b/src/modules/sensors/sensor_params.c @@ -585,6 +585,34 @@ PARAM_DEFINE_FLOAT(CAL_ACC2_YSCALE, 1.0f); */ PARAM_DEFINE_FLOAT(CAL_ACC2_ZSCALE, 1.0f); +/** + * Primary accel ID + * + * @group Sensor Calibration + */ +PARAM_DEFINE_INT32(CAL_ACC_PRIME, 0); + +/** + * Primary gyro ID + * + * @group Sensor Calibration + */ +PARAM_DEFINE_INT32(CAL_GYRO_PRIME, 0); + +/** + * Primary mag ID + * + * @group Sensor Calibration + */ +PARAM_DEFINE_INT32(CAL_MAG_PRIME, 0); + +/** + * Primary baro ID + * + * @group Sensor Calibration + */ +PARAM_DEFINE_INT32(CAL_BARO_PRIME, 0); + /** * Differential pressure sensor offset * From 336ca86117f981377879189265ca93ca7aaa8437 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Wed, 14 Oct 2015 20:02:02 +0200 Subject: [PATCH 42/44] Commander: Ensure primary sensor is present if configured --- src/modules/commander/PreflightCheck.cpp | 49 ++++++++++++++---------- 1 file changed, 28 insertions(+), 21 deletions(-) diff --git a/src/modules/commander/PreflightCheck.cpp b/src/modules/commander/PreflightCheck.cpp index cdddd3b637..2a8a0695cf 100644 --- a/src/modules/commander/PreflightCheck.cpp +++ b/src/modules/commander/PreflightCheck.cpp @@ -72,9 +72,9 @@ namespace Commander { -static int check_calibration(int fd, const char* param_template); +static int check_calibration(int fd, const char* param_template, int &devid); -int check_calibration(int fd, const char* param_template) +int check_calibration(int fd, const char* param_template, int &devid) { bool calibration_found; @@ -82,13 +82,13 @@ int check_calibration(int fd, const char* param_template) int ret = px4_ioctl(fd, SENSORIOCCALTEST, 0); calibration_found = (ret == OK); - - /* old style transition: check param values */ - int devid = px4_ioctl(fd, DEVIOCGDEVICEID, 0); + + devid = px4_ioctl(fd, DEVIOCGDEVICEID, 0); char s[20]; int instance = 0; + /* old style transition: check param values */ while (!calibration_found) { sprintf(s, param_template, instance); param_t parm = param_find(s); @@ -131,7 +131,7 @@ static bool magnometerCheck(int mavlink_fd, unsigned instance, bool optional, in return false; } - int ret = check_calibration(fd, "CAL_MAG%u_ID"); + int ret = check_calibration(fd, "CAL_MAG%u_ID", device_id); if (ret) { mavlink_and_console_log_critical(mavlink_fd, @@ -171,7 +171,7 @@ static bool accelerometerCheck(int mavlink_fd, unsigned instance, bool optional, return false; } - int ret = check_calibration(fd, "CAL_ACC%u_ID"); + int ret = check_calibration(fd, "CAL_ACC%u_ID", device_id); if (ret) { mavlink_and_console_log_critical(mavlink_fd, @@ -236,7 +236,7 @@ static bool gyroCheck(int mavlink_fd, unsigned instance, bool optional, int &dev return false; } - int ret = check_calibration(fd, "CAL_GYRO%u_ID"); + int ret = check_calibration(fd, "CAL_GYRO%u_ID", device_id); if (ret) { mavlink_and_console_log_critical(mavlink_fd, @@ -276,6 +276,8 @@ static bool baroCheck(int mavlink_fd, unsigned instance, bool optional, int &dev return false; } + device_id = -1000; + // TODO: There is no baro calibration yet, since no external baros exist // int ret = check_calibration(fd, "CAL_BARO%u_ID"); @@ -361,7 +363,7 @@ bool preflightCheck(int mavlink_fd, bool checkMag, bool checkAcc, bool checkGyro /* check all sensors, but fail only for mandatory ones */ for (unsigned i = 0; i < max_optional_mag_count; i++) { bool required = (i < max_mandatory_mag_count); - int device_id; + int device_id = -1; if (!magnometerCheck(mavlink_fd, i, !required, device_id) && required) { failed = true; @@ -373,8 +375,9 @@ bool preflightCheck(int mavlink_fd, bool checkMag, bool checkAcc, bool checkGyro } /* check if the primary device is present */ - if (!prime_found) { - //failed = true; + if (!prime_found && prime_id != 0) { + mavlink_log_critical(mavlink_fd, "warning: primary compass not operational"); + failed = true; } } @@ -387,7 +390,7 @@ bool preflightCheck(int mavlink_fd, bool checkMag, bool checkAcc, bool checkGyro /* check all sensors, but fail only for mandatory ones */ for (unsigned i = 0; i < max_optional_accel_count; i++) { bool required = (i < max_mandatory_accel_count); - int device_id; + int device_id = -1; if (!accelerometerCheck(mavlink_fd, i, !required, checkDynamic, device_id) && required) { failed = true; @@ -399,8 +402,9 @@ bool preflightCheck(int mavlink_fd, bool checkMag, bool checkAcc, bool checkGyro } /* check if the primary device is present */ - if (!prime_found) { - //failed = true; + if (!prime_found && prime_id != 0) { + mavlink_log_critical(mavlink_fd, "warning: primary accelerometer not operational"); + failed = true; } } @@ -413,7 +417,7 @@ bool preflightCheck(int mavlink_fd, bool checkMag, bool checkAcc, bool checkGyro /* check all sensors, but fail only for mandatory ones */ for (unsigned i = 0; i < max_optional_gyro_count; i++) { bool required = (i < max_mandatory_gyro_count); - int device_id; + int device_id = -1; if (!gyroCheck(mavlink_fd, i, !required, device_id) && required) { failed = true; @@ -425,8 +429,9 @@ bool preflightCheck(int mavlink_fd, bool checkMag, bool checkAcc, bool checkGyro } /* check if the primary device is present */ - if (!prime_found) { - //failed = true; + if (!prime_found && prime_id != 0) { + mavlink_log_critical(mavlink_fd, "warning: primary gyro not operational"); + failed = true; } } @@ -439,7 +444,7 @@ bool preflightCheck(int mavlink_fd, bool checkMag, bool checkAcc, bool checkGyro /* check all sensors, but fail only for mandatory ones */ for (unsigned i = 0; i < max_optional_baro_count; i++) { bool required = (i < max_mandatory_baro_count); - int device_id; + int device_id = -1; if (!baroCheck(mavlink_fd, i, !required, device_id) && required) { failed = true; @@ -452,8 +457,9 @@ bool preflightCheck(int mavlink_fd, bool checkMag, bool checkAcc, bool checkGyro // TODO there is no logic in place to calibrate the primary baro yet // // check if the primary device is present - if (!prime_found) { - // failed = true; + if (!prime_found && prime_id != 0) { + mavlink_log_critical(mavlink_fd, "warning: primary barometer not operational"); + failed = true; } } @@ -467,12 +473,13 @@ bool preflightCheck(int mavlink_fd, bool checkMag, bool checkAcc, bool checkGyro /* ---- RC CALIBRATION ---- */ if (checkRC) { if (rc_calibration_check(mavlink_fd) != OK) { + mavlink_log_critical(mavlink_fd, "RC calibration check failed"); failed = true; } } /* ---- Global Navigation Satellite System receiver ---- */ - if(checkGNSS) { + if (checkGNSS) { if(!gnssCheck(mavlink_fd)) { failed = true; } From 53bbe1920d635918f50985ccd5a2fea4482fc28a Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Wed, 14 Oct 2015 20:02:27 +0200 Subject: [PATCH 43/44] Navigator: Make error message less cryptic --- src/modules/navigator/mission_feasibility_checker.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/navigator/mission_feasibility_checker.cpp b/src/modules/navigator/mission_feasibility_checker.cpp index a31aa25805..880d4bd98e 100644 --- a/src/modules/navigator/mission_feasibility_checker.cpp +++ b/src/modules/navigator/mission_feasibility_checker.cpp @@ -336,7 +336,7 @@ MissionFeasibilityChecker::check_dist_1wp(dm_item_t dm_current, size_t nMissionI } else { /* item is too far from home */ - mavlink_log_critical(_mavlink_fd, "Waypoint too far: %d m,[MIS_DIST_1WP=%d]", (int)dist_to_1wp, (int)dist_first_wp); + mavlink_log_critical(_mavlink_fd, "First waypoint too far: %d m,refusing mission", (int)dist_to_1wp, (int)dist_first_wp); warning_issued = true; return false; } From dbcb226de6ffd41882342eeb65f9bb1745641751 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Wed, 14 Oct 2015 20:11:00 +0200 Subject: [PATCH 44/44] Sensors: Ensure all mag rotations are visible to the GCS --- src/modules/sensors/sensors.cpp | 2 ++ 1 file changed, 2 insertions(+) diff --git a/src/modules/sensors/sensors.cpp b/src/modules/sensors/sensors.cpp index 61eab348d7..6009e2dd5c 100644 --- a/src/modules/sensors/sensors.cpp +++ b/src/modules/sensors/sensors.cpp @@ -1412,6 +1412,8 @@ Sensors::parameter_update_poll(bool forced) (void)sprintf(str, "CAL_MAG%u_ID", i); int device_id; failed = failed || (OK != param_get(param_find(str), &device_id)); + (void)sprintf(str, "CAL_MAG%u_ROT", i); + (void)param_find(str); if (failed) { px4_close(fd);