From ab022d51338c30207379048c913715a0b8d601c3 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Sun, 31 Aug 2014 16:23:37 +0200 Subject: [PATCH 1/8] disable underspeed protection when landing also in tecs --- src/lib/external_lgpl/tecs/tecs.cpp | 5 +++++ src/lib/external_lgpl/tecs/tecs.h | 10 ++++++++++ .../fw_pos_control_l1/fw_pos_control_l1_main.cpp | 3 +++ 3 files changed, 18 insertions(+) diff --git a/src/lib/external_lgpl/tecs/tecs.cpp b/src/lib/external_lgpl/tecs/tecs.cpp index a57a0481a6..d27bf776fb 100644 --- a/src/lib/external_lgpl/tecs/tecs.cpp +++ b/src/lib/external_lgpl/tecs/tecs.cpp @@ -252,6 +252,11 @@ void TECS::_update_height_demand(float demand, float state) void TECS::_detect_underspeed(void) { + if (!_detect_underspeed_enabled) { + _underspeed = false; + return; + } + if (((_integ5_state < _TASmin * 0.9f) && (_throttle_dem >= _THRmaxf * 0.95f)) || ((_integ3_state < _hgt_dem_adj) && _underspeed)) { _underspeed = true; diff --git a/src/lib/external_lgpl/tecs/tecs.h b/src/lib/external_lgpl/tecs/tecs.h index bcc2d90e5c..36ae4ecaf2 100644 --- a/src/lib/external_lgpl/tecs/tecs.h +++ b/src/lib/external_lgpl/tecs/tecs.h @@ -66,6 +66,9 @@ public: _hgt_dem_prev(0.0f), _TAS_dem_adj(0.0f), _STEdotErrLast(0.0f), + _underspeed(false), + _detect_underspeed_enabled(true), + _badDescent(false), _climbOutDem(false), _SPE_dem(0.0f), _SKE_dem(0.0f), @@ -221,6 +224,10 @@ public: _speedrate_p = speedrate_p; } + void set_detect_underspeed_enabled(bool enabled) { + _detect_underspeed_enabled = enabled; + } + private: struct tecs_state _tecs_state; @@ -323,6 +330,9 @@ private: // Underspeed condition bool _underspeed; + // Underspeed detection enabled + bool _detect_underspeed_enabled; + // Bad descent condition caused by unachievable airspeed demand bool _badDescent; 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 350ce6dec1..522f5caca0 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 @@ -1380,6 +1380,9 @@ void FixedwingPositionControl::tecs_update_pitch_throttle(float alt_sp, float v_ _mTecs.updateAltitudeSpeed(flightPathAngle, altitude, alt_sp, _airspeed.true_airspeed_m_s, v_sp, mode, limitOverride); } else { + /* No underspeed protection in landing mode */ + _tecs.set_detect_underspeed_enabled(!(mode == TECS_MODE_LAND || mode == TECS_MODE_LAND_THROTTLELIM)); + /* Using tecs library */ _tecs.update_pitch_throttle(_R_nb, _att.pitch, altitude, alt_sp, v_sp, _airspeed.indicated_airspeed_m_s, eas2tas, From 0e954b30c2ef3e31d60e5ef7a5fa985882baef8f Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 31 Aug 2014 17:58:43 +0200 Subject: [PATCH 2/8] Rely on laser altitude once valid --- src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) 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 522f5caca0..c983ac6f9c 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 @@ -823,7 +823,7 @@ float FixedwingPositionControl::get_relative_landingalt(float land_setpoint_alt, * the measurement is valid * the estimated relative altitude (from global altitude estimate and landing waypoint) <= range_finder_use_relative_alt */ - if (range_finder_use_relative_alt < 0 || !range_finder.valid || rel_alt_estimated > range_finder_use_relative_alt ) { + if (range_finder_use_relative_alt < 0 || !range_finder.valid || range_finder.distance > range_finder_use_relative_alt ) { return rel_alt_estimated; } From 7a253d50f8da7a6f8176f784c196b85bcae3c8f1 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 31 Aug 2014 17:59:21 +0200 Subject: [PATCH 3/8] Add more paranoid checks to sf0x altitude parsing --- src/drivers/sf0x/sf0x.cpp | 8 +++++++- 1 file changed, 7 insertions(+), 1 deletion(-) diff --git a/src/drivers/sf0x/sf0x.cpp b/src/drivers/sf0x/sf0x.cpp index 80ecab2ee1..d7780e9e81 100644 --- a/src/drivers/sf0x/sf0x.cpp +++ b/src/drivers/sf0x/sf0x.cpp @@ -593,13 +593,19 @@ SF0X::collect() /* wipe out partially read content from last cycle(s), check for dot */ for (unsigned i = 0; i < (lend - 2); i++) { if (_linebuf[i] == '\n') { + /* allocate temporary buffer */ char buf[sizeof(_linebuf)]; + /* copy remainder of buffer (2nd measurement) to temporary buffer */ memcpy(buf, &_linebuf[i+1], (lend + 1) - (i + 1)); + /* copy temporary buffer to beginning of line buffer, + * effectively overwriting a previous temporary + * measurement + */ memcpy(_linebuf, buf, (lend + 1) - (i + 1)); } /* we need a digit before the dot and a dot for a valid number */ - if (i > 0 && _linebuf[i] == '.') { + if (i > 0 && ((_linebuf[i - 1] >= '0') && (_linebuf[i - 1] <= '9')) && (_linebuf[i] == '.')) { valid = true; } } From ee913202991f2a015904b1570bd9f5a62865e5c7 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 31 Aug 2014 19:53:01 +0200 Subject: [PATCH 4/8] SF02/F driver: Improve parsing --- src/drivers/sf0x/sf0x.cpp | 40 ++++++++++++++++----------------------- 1 file changed, 16 insertions(+), 24 deletions(-) diff --git a/src/drivers/sf0x/sf0x.cpp b/src/drivers/sf0x/sf0x.cpp index d7780e9e81..cf04190756 100644 --- a/src/drivers/sf0x/sf0x.cpp +++ b/src/drivers/sf0x/sf0x.cpp @@ -520,11 +520,9 @@ SF0X::collect() /* clear buffer if last read was too long ago */ uint64_t read_elapsed = hrt_elapsed_time(&_last_read); + /* timed out - retry */ if (read_elapsed > (SF0X_CONVERSION_INTERVAL * 2)) { _linebuf_index = 0; - } else if (_linebuf_index > 0) { - /* increment to next read position */ - _linebuf_index++; } /* the buffer for read chars is buflen minus null termination */ @@ -550,18 +548,19 @@ SF0X::collect() return -EAGAIN; } - /* we did increment the index to the next position already, so just add the additional fields */ - _linebuf_index += (ret - 1); + /* let the write pointer point to the next free entry */ + _linebuf_index += ret; _last_read = hrt_absolute_time(); - if (_linebuf_index < 1) { - /* we need at least the two end bytes to make sense of this string */ + /* require a reasonable amount of minimum bytes */ + if (_linebuf_index < 6) { + /* we need at this format: x.xx\r\n */ return -EAGAIN; - } else if (_linebuf[_linebuf_index - 1] != '\r' || _linebuf[_linebuf_index] != '\n') { + } else if (_linebuf[_linebuf_index - 2] != '\r' || _linebuf[_linebuf_index - 1] != '\n') { - if (_linebuf_index >= readlen - 1) { + if (_linebuf_index == readlen) { /* we have a full buffer, but no line ending - abort */ _linebuf_index = 0; perf_count(_comms_errors); @@ -577,9 +576,7 @@ SF0X::collect() bool valid; /* enforce line ending */ - unsigned lend = (_linebuf_index < (sizeof(_linebuf) - 1)) ? _linebuf_index : (sizeof(_linebuf) - 1); - - _linebuf[lend] = '\0'; + _linebuf[_linebuf_index] = '\0'; if (_linebuf[0] == '-' && _linebuf[1] == '-' && _linebuf[2] == '.') { si_units = -1.0f; @@ -591,17 +588,12 @@ SF0X::collect() valid = false; /* wipe out partially read content from last cycle(s), check for dot */ - for (unsigned i = 0; i < (lend - 2); i++) { + for (unsigned i = 0; i < (_linebuf_index - 2); i++) { if (_linebuf[i] == '\n') { - /* allocate temporary buffer */ - char buf[sizeof(_linebuf)]; - /* copy remainder of buffer (2nd measurement) to temporary buffer */ - memcpy(buf, &_linebuf[i+1], (lend + 1) - (i + 1)); - /* copy temporary buffer to beginning of line buffer, - * effectively overwriting a previous temporary - * measurement - */ - memcpy(_linebuf, buf, (lend + 1) - (i + 1)); + /* wipe out any partial measurements */ + for (unsigned j = 0; j <= i; j++) { + _linebuf[j] = ' '; + } } /* we need a digit before the dot and a dot for a valid number */ @@ -613,8 +605,8 @@ SF0X::collect() if (valid) { si_units = strtod(_linebuf, &end); - /* we require at least 3 characters for a valid number */ - if (end > _linebuf + 3) { + /* we require at least four characters for a valid number */ + if (end > _linebuf + 4) { valid = true; } else { si_units = -1.0f; From a46e1aa60efcaf8b52db02f783fc4c0a9dc05878 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 31 Aug 2014 20:55:29 +0200 Subject: [PATCH 5/8] Fix reschedule logic for SF02/F driver --- src/drivers/sf0x/sf0x.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/drivers/sf0x/sf0x.cpp b/src/drivers/sf0x/sf0x.cpp index cf04190756..b690938e71 100644 --- a/src/drivers/sf0x/sf0x.cpp +++ b/src/drivers/sf0x/sf0x.cpp @@ -707,12 +707,12 @@ SF0X::cycle() int collect_ret = collect(); if (collect_ret == -EAGAIN) { - /* reschedule to grab the missing bits, time to transmit 10 bytes @9600 bps */ + /* reschedule to grab the missing bits, time to transmit 8 bytes @ 9600 bps */ work_queue(HPWORK, &_work, (worker_t)&SF0X::cycle_trampoline, this, - USEC2TICK(1100)); + USEC2TICK(1042 * 8)); return; } From af3b3680e090ad80ffeb6581f1f58f2dd4ae5195 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 31 Aug 2014 21:10:04 +0200 Subject: [PATCH 6/8] Fixed SF02/F driver, tested ok --- src/drivers/sf0x/sf0x.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/drivers/sf0x/sf0x.cpp b/src/drivers/sf0x/sf0x.cpp index b690938e71..d382d08d09 100644 --- a/src/drivers/sf0x/sf0x.cpp +++ b/src/drivers/sf0x/sf0x.cpp @@ -606,7 +606,7 @@ SF0X::collect() si_units = strtod(_linebuf, &end); /* we require at least four characters for a valid number */ - if (end > _linebuf + 4) { + if (end > _linebuf + 3) { valid = true; } else { si_units = -1.0f; @@ -615,7 +615,7 @@ SF0X::collect() } } - debug("val (float): %8.4f, raw: %s, valid: %s\n", (double)si_units, _linebuf, ((valid) ? "OK" : "NO")); + debug("val (float): %8.4f, raw: %s, valid: %s", (double)si_units, _linebuf, ((valid) ? "OK" : "NO")); /* done with this chunk, resetting - even if invalid */ _linebuf_index = 0; From 84d26185df1aa3a727d0db7b06b262e756eb5248 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 31 Aug 2014 21:11:20 +0200 Subject: [PATCH 7/8] Better initial config for Phantom --- ROMFS/px4fmu_common/init.d/3031_phantom | 3 +++ 1 file changed, 3 insertions(+) diff --git a/ROMFS/px4fmu_common/init.d/3031_phantom b/ROMFS/px4fmu_common/init.d/3031_phantom index 31dfe71007..53c48d8aac 100644 --- a/ROMFS/px4fmu_common/init.d/3031_phantom +++ b/ROMFS/px4fmu_common/init.d/3031_phantom @@ -30,6 +30,9 @@ then param set FW_RR_P 0.08 param set FW_R_LIM 50 param set FW_R_RMAX 0 + # Bottom of bay and nominal zero-pitch attitude differ + # the payload bay is pitched up about 7 degrees + param set SENS_BOARD_Y_OFF 7.0 fi set MIXER phantom From 22d2e26b9cab22fbe3f418b22e74fb05048083e0 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 31 Aug 2014 21:55:31 +0200 Subject: [PATCH 8/8] Simply throttle limiting on approach - limit throttle still defaults to 1 --- .../fw_pos_control_l1/fw_pos_control_l1_main.cpp | 6 ++++++ .../fw_pos_control_l1/fw_pos_control_l1_params.c | 13 +++++++++---- 2 files changed, 15 insertions(+), 4 deletions(-) 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 c983ac6f9c..deccab4820 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 @@ -569,6 +569,12 @@ FixedwingPositionControl::parameters_update() param_get(_parameter_handles.land_H1_virt, &(_parameters.land_H1_virt)); param_get(_parameter_handles.land_flare_alt_relative, &(_parameters.land_flare_alt_relative)); param_get(_parameter_handles.land_thrust_lim_alt_relative, &(_parameters.land_thrust_lim_alt_relative)); + + /* check if negative value for 2/3 of flare altitude is set for throttle cut */ + if (_parameters.land_thrust_lim_alt_relative < 0.0f) { + _parameters.land_thrust_lim_alt_relative = 0.66f * _parameters.land_flare_alt_relative; + } + param_get(_parameter_handles.land_heading_hold_horizontal_distance, &(_parameters.land_heading_hold_horizontal_distance)); param_get(_parameter_handles.range_finder_rel_alt, &(_parameters.range_finder_rel_alt)); diff --git a/src/modules/fw_pos_control_l1/fw_pos_control_l1_params.c b/src/modules/fw_pos_control_l1/fw_pos_control_l1_params.c index 41c3744076..890ab3bad2 100644 --- a/src/modules/fw_pos_control_l1/fw_pos_control_l1_params.c +++ b/src/modules/fw_pos_control_l1/fw_pos_control_l1_params.c @@ -379,18 +379,23 @@ PARAM_DEFINE_FLOAT(FW_LND_ANG, 5.0f); PARAM_DEFINE_FLOAT(FW_LND_HVIRT, 10.0f); /** - * Landing flare altitude (relative) + * Landing flare altitude (relative to landing altitude) * + * @unit meter * @group L1 Control */ -PARAM_DEFINE_FLOAT(FW_LND_FLALT, 15.0f); +PARAM_DEFINE_FLOAT(FW_LND_FLALT, 8.0f); /** - * Landing throttle limit altitude (relative) + * Landing throttle limit altitude (relative landing altitude) * + * Default of -1.0f lets the system default to applying throttle + * limiting at 2/3 of the flare altitude. + * + * @unit meter * @group L1 Control */ -PARAM_DEFINE_FLOAT(FW_LND_TLALT, 5.0f); +PARAM_DEFINE_FLOAT(FW_LND_TLALT, -1.0f); /** * Landing heading hold horizontal distance