From 5b6f1b5ca554189cc25a52926066f857c543d80c Mon Sep 17 00:00:00 2001 From: Roman Date: Fri, 22 May 2015 21:12:18 +0200 Subject: [PATCH 1/7] fixed rates feedforward: plant for feedforward is given by derivative operator --- src/modules/mc_att_control/mc_att_control_main.cpp | 5 ++++- 1 file changed, 4 insertions(+), 1 deletion(-) diff --git a/src/modules/mc_att_control/mc_att_control_main.cpp b/src/modules/mc_att_control/mc_att_control_main.cpp index 34bfa46140..97d8625b02 100644 --- a/src/modules/mc_att_control/mc_att_control_main.cpp +++ b/src/modules/mc_att_control/mc_att_control_main.cpp @@ -158,6 +158,7 @@ private: perf_counter_t _controller_latency_perf; math::Vector<3> _rates_prev; /**< angular rates on previous step */ + math::Vector<3> _rates_sp_prev; /**< previous rates setpoint */ math::Vector<3> _rates_sp; /**< angular rates setpoint */ math::Vector<3> _rates_int; /**< angular rates integral error */ float _thrust_sp; /**< thrust setpoint */ @@ -353,6 +354,7 @@ MulticopterAttitudeControl::MulticopterAttitudeControl() : _rates_prev.zero(); _rates_sp.zero(); + _rates_sp_prev.zero(); _rates_int.zero(); _thrust_sp = 0.0f; _att_control.zero(); @@ -712,7 +714,8 @@ MulticopterAttitudeControl::control_attitude_rates(float dt) /* angular rates error */ math::Vector<3> rates_err = _rates_sp - rates; - _att_control = _params.rate_p.emult(rates_err) + _params.rate_d.emult(_rates_prev - rates) / dt + _rates_int + _params.rate_ff.emult(_rates_sp); + _att_control = _params.rate_p.emult(rates_err) + _params.rate_d.emult(_rates_prev - rates) / dt + _rates_int + _params.rate_ff.emult(_rates_sp - _rates_sp_prev) / dt; + _rates_sp_prev = _rates_sp; _rates_prev = rates; /* update integral only if not saturated on low limit and if motor commands are not saturated */ From 335b212f9238265c48a5d947819bc599af519a93 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 24 May 2015 11:43:18 +0200 Subject: [PATCH 2/7] commander: Better update logic, better feedback text for landings --- src/modules/commander/commander.cpp | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index f752a37b98..a9b2926d2a 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -1478,20 +1478,20 @@ int commander_thread_main(int argc, char *argv[]) /* Update land detector */ orb_check(land_detector_sub, &updated); - if(updated) { + if (updated) { orb_copy(ORB_ID(vehicle_land_detected), land_detector_sub, &land_detector); } - if (status.condition_local_altitude_valid) { + if (updated && status.condition_local_altitude_valid) { if (status.condition_landed != land_detector.landed) { status.condition_landed = land_detector.landed; status_changed = true; if (status.condition_landed) { - mavlink_log_critical(mavlink_fd, "LANDED MODE"); + mavlink_log_critical(mavlink_fd, "LANDING DETECTED"); } else { - mavlink_log_critical(mavlink_fd, "IN AIR MODE"); + mavlink_log_critical(mavlink_fd, "TAKEOFF DETECTED"); } } } From 945aa4789bb9015a75a95181c8a0938dd60c5454 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 24 May 2015 11:45:42 +0200 Subject: [PATCH 3/7] Land detector: Make fixed wing detections more stable with better param --- src/modules/land_detector/land_detector_params.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/land_detector/land_detector_params.c b/src/modules/land_detector/land_detector_params.c index e3070c115e..9cf57b5fc9 100644 --- a/src/modules/land_detector/land_detector_params.c +++ b/src/modules/land_detector/land_detector_params.c @@ -83,7 +83,7 @@ PARAM_DEFINE_FLOAT(LNDMC_THR_MAX, 0.20f); * * @group Land Detector */ -PARAM_DEFINE_FLOAT(LNDFW_VEL_XY_MAX, 0.20f); +PARAM_DEFINE_FLOAT(LNDFW_VEL_XY_MAX, 0.40f); /** * Fixedwing max climb rate From 073f10fe4f119c115a503d62a596f0f89183ce8b Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 24 May 2015 11:46:25 +0200 Subject: [PATCH 4/7] Land detector: Better docs and status feedback --- src/modules/land_detector/LandDetector.h | 19 ++++++++++++------- .../land_detector/land_detector_main.cpp | 2 +- 2 files changed, 13 insertions(+), 8 deletions(-) diff --git a/src/modules/land_detector/LandDetector.h b/src/modules/land_detector/LandDetector.h index 09db6e4746..b2984003b9 100644 --- a/src/modules/land_detector/LandDetector.h +++ b/src/modules/land_detector/LandDetector.h @@ -52,19 +52,24 @@ public: virtual ~LandDetector(); /** - * @return true if this task is currently running - **/ + * @return true if this task is currently running + **/ inline bool isRunning() const {return _taskIsRunning;} /** - * @brief Tells the Land Detector task that it should exit - **/ + * @return the current landed state + */ + bool isLanded() { return _landDetected.landed; } + + /** + * @brief Tells the Land Detector task that it should exit + **/ void shutdown(); /** - * @brief Blocking function that should be called from it's own task thread. This method will - * run the underlying algorithm at the desired update rate and publish if the landing state changes. - **/ + * @brief Blocking function that should be called from it's own task thread. This method will + * run the underlying algorithm at the desired update rate and publish if the landing state changes. + **/ void start(); protected: diff --git a/src/modules/land_detector/land_detector_main.cpp b/src/modules/land_detector/land_detector_main.cpp index b4b7c33a20..c3829bb70d 100644 --- a/src/modules/land_detector/land_detector_main.cpp +++ b/src/modules/land_detector/land_detector_main.cpp @@ -195,7 +195,7 @@ int land_detector_main(int argc, char *argv[]) if (land_detector_task) { if (land_detector_task->isRunning()) { - warnx("running (%s)", _currentMode); + warnx("running (%s): %s", _currentMode, (land_detector_task->isLanded()) ? "LANDED" : "IN AIR"); } else { errx(1, "exists, but not running (%s)", _currentMode); From ae50328646e705ce5292fb895f99bbf9adb28d62 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 24 May 2015 12:47:21 +0200 Subject: [PATCH 5/7] Properly define global limits for RC input in driver --- src/drivers/drv_rc_input.h | 15 +++++++++++++++ 1 file changed, 15 insertions(+) diff --git a/src/drivers/drv_rc_input.h b/src/drivers/drv_rc_input.h index a24d8814fe..a8a76c3eff 100644 --- a/src/drivers/drv_rc_input.h +++ b/src/drivers/drv_rc_input.h @@ -67,6 +67,21 @@ */ #define RC_INPUT_RSSI_MAX 100 +/** + * Minimum value + */ +#define RC_INPUT_LOWEST_MIN_US 500 + +/** + * Maximum value + */ +#define RC_INPUT_HIGHEST_MAX_US 2500 + +/** + * Maximum deadzone value + */ +#define RC_INPUT_MAX_DEADZONE_US 500 + /** * @addtogroup topics * @{ From 8f70ebecc8d3d604d2ef8338cc9d28515fc201d1 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 24 May 2015 12:49:00 +0200 Subject: [PATCH 6/7] RC check: Cleanup, removal of magic numbers and addition of mandatory mapping parameters --- src/modules/systemlib/rc_check.c | 108 +++++++++++++++++++++---------- 1 file changed, 75 insertions(+), 33 deletions(-) diff --git a/src/modules/systemlib/rc_check.c b/src/modules/systemlib/rc_check.c index b35b333af2..ad3be18d47 100644 --- a/src/modules/systemlib/rc_check.c +++ b/src/modules/systemlib/rc_check.c @@ -48,29 +48,72 @@ #include #include -int rc_calibration_check(int mavlink_fd) { +#define RC_INPUT_MAP_UNMAPPED 0 + +int rc_calibration_check(int mavlink_fd) +{ char nbuf[20]; param_t _parameter_handles_min, _parameter_handles_trim, _parameter_handles_max, - _parameter_handles_rev, _parameter_handles_dz; + _parameter_handles_rev, _parameter_handles_dz; + + unsigned map_fail_count = 0; - float param_min, param_max, param_trim, param_rev, param_dz; + const char *rc_map_mandatory[] = { "RC_MAP_MODE_SW", + /* needs discussion if this should be mandatory "RC_MAP_POSCTL_SW"*/ + 0 /* end marker */ + }; + + unsigned j = 0; /* first check channel mappings */ - /* check which map param applies */ - // if (map_by_channel[i] >= MAX_CONTROL_CHANNELS) { - // mavlink_log_critical(mavlink_fd, "ERR: RC_%d_MAP >= # CHANS", i+1); - // /* give system time to flush error message in case there are more */ - // usleep(100000); - // count++; - // } + while (rc_map_mandatory[j] != 0) { + + param_t map_parm = param_find(rc_map_mandatory[j]); - int channel_fail_count = 0; + if (map_parm == PARAM_INVALID) { + mavlink_log_critical(mavlink_fd, "RC ERR: PARAM %s MISSING", rc_map_mandatory[j]); + /* give system time to flush error message in case there are more */ + usleep(100000); + map_fail_count++; + j++; + continue; + } - for (int i = 0; i < RC_INPUT_MAX_CHANNELS; i++) { + int32_t mapping; + param_get(map_parm, &mapping); + + if (mapping > RC_INPUT_MAX_CHANNELS) { + mavlink_log_critical(mavlink_fd, "RC ERR: %s >= # CHANS", rc_map_mandatory[j]); + /* give system time to flush error message in case there are more */ + usleep(100000); + map_fail_count++; + } + + if (mapping == 0) { + mavlink_log_critical(mavlink_fd, "RC ERR: Mandatory %s is unmapped", rc_map_mandatory[j]); + /* give system time to flush error message in case there are more */ + usleep(100000); + map_fail_count++; + } + + j++; + } + + unsigned total_fail_count = 0; + unsigned channels_failed = 0; + + for (unsigned i = 0; i < RC_INPUT_MAX_CHANNELS; i++) { /* should the channel be enabled? */ uint8_t count = 0; + /* initialize values to values failing the check */ + float param_min = 0.0f; + float param_max = 0.0f; + float param_trim = 0.0f; + float param_rev = 0.0f; + float param_dz = RC_INPUT_MAX_DEADZONE_US * 2.0f; + /* min values */ sprintf(nbuf, "RC%d_MIN", i + 1); _parameter_handles_min = param_find(nbuf); @@ -97,56 +140,55 @@ int rc_calibration_check(int mavlink_fd) { param_get(_parameter_handles_dz, ¶m_dz); /* assert min..center..max ordering */ - if (param_min < 500) { + if (param_min < RC_INPUT_LOWEST_MIN_US) { count++; - mavlink_log_critical(mavlink_fd, "#audio ERR: RC_%d_MIN < 500", i+1); + mavlink_log_critical(mavlink_fd, "RC ERR: RC_%d_MIN < %u", i + 1, RC_INPUT_LOWEST_MIN_US); /* give system time to flush error message in case there are more */ usleep(100000); } - if (param_max > 2500) { + + if (param_max > RC_INPUT_HIGHEST_MAX_US) { count++; - mavlink_log_critical(mavlink_fd, "#audio ERR: RC_%d_MAX > 2500", i+1); + mavlink_log_critical(mavlink_fd, "RC ERR: RC_%d_MAX > %u", i + 1, RC_INPUT_HIGHEST_MAX_US); /* give system time to flush error message in case there are more */ usleep(100000); } + if (param_trim < param_min) { count++; - mavlink_log_critical(mavlink_fd, "#audio ERR: RC_%d_TRIM < MIN (%d/%d)", i+1, (int)param_trim, (int)param_min); + mavlink_log_critical(mavlink_fd, "RC ERR: RC_%d_TRIM < MIN (%d/%d)", i + 1, (int)param_trim, (int)param_min); /* give system time to flush error message in case there are more */ usleep(100000); } + if (param_trim > param_max) { count++; - mavlink_log_critical(mavlink_fd, "#audio ERR: RC_%d_TRIM > MAX (%d/%d)", i+1, (int)param_trim, (int)param_max); + mavlink_log_critical(mavlink_fd, "RC ERR: RC_%d_TRIM > MAX (%d/%d)", i + 1, (int)param_trim, (int)param_max); /* give system time to flush error message in case there are more */ usleep(100000); } /* assert deadzone is sane */ - if (param_dz > 500) { - mavlink_log_critical(mavlink_fd, "#audio ERR: RC_%d_DZ > 500", i+1); + if (param_dz > RC_INPUT_MAX_DEADZONE_US) { + mavlink_log_critical(mavlink_fd, "RC ERR: RC_%d_DZ > %u", i + 1, RC_INPUT_MAX_DEADZONE_US); /* give system time to flush error message in case there are more */ usleep(100000); count++; } - /* check which map param applies */ - // if (map_by_channel[i] >= MAX_CONTROL_CHANNELS) { - // mavlink_log_critical(mavlink_fd, "ERR: RC_%d_MAP >= # CHANS", i+1); - // /* give system time to flush error message in case there are more */ - // usleep(100000); - // count++; - // } + total_fail_count += count; - /* sanity checks pass, enable channel */ if (count) { - mavlink_log_critical(mavlink_fd, "#audio ERROR: %d config error(s) for RC channel %d.", count, (i + 1)); - warnx("ERROR: %d config error(s) for RC channel %d.", count, (i + 1)); - usleep(100000); + channels_failed++; } + } - channel_fail_count += count; + if (channels_failed) { + sleep(2); + mavlink_and_console_log_critical(mavlink_fd, "%d config error%s for %d RC channel%s.", total_fail_count, + (total_fail_count > 1) ? "s" : "", channels_failed, (channels_failed > 1) ? "s" : ""); + usleep(100000); } - return channel_fail_count; + return total_fail_count + map_fail_count; } From 77bd36aa334a602ed7e4cd225a8e4c7598048a64 Mon Sep 17 00:00:00 2001 From: Don Gagne Date: Sun, 24 May 2015 10:40:52 -0700 Subject: [PATCH 7/7] Fix opendir failure handling --- src/modules/mavlink/mavlink_ftp.cpp | 10 ++-------- 1 file changed, 2 insertions(+), 8 deletions(-) diff --git a/src/modules/mavlink/mavlink_ftp.cpp b/src/modules/mavlink/mavlink_ftp.cpp index 233bc2d323..42cabb4ba6 100644 --- a/src/modules/mavlink/mavlink_ftp.cpp +++ b/src/modules/mavlink/mavlink_ftp.cpp @@ -316,14 +316,8 @@ MavlinkFTP::_workList(PayloadHeader* payload) _mavlink->send_statustext_critical("FTP: can't open path (file system corrupted?)"); _mavlink->send_statustext_critical(dirPath); #endif - // this is not an FTP error, abort directory read and continue - - payload->data[offset++] = kDirentSkip; - *((char *)&payload->data[offset]) = '\0'; - offset++; - payload->size = offset; - - return errorCode; + // this is not an FTP error, abort directory by simulating eof + return kErrEOF; } #ifdef MAVLINK_FTP_DEBUG