diff options
author | Thomas Gubler <thomasgubler@gmail.com> | 2014-09-02 15:00:26 +0200 |
---|---|---|
committer | Thomas Gubler <thomasgubler@gmail.com> | 2014-09-02 15:00:26 +0200 |
commit | 18f0ee3f28763c330dfd0c0998c47c46e5edde04 (patch) | |
tree | aba5b6aea9899928bc8f24cd80098820ea348a3c | |
parent | fe0642d5e992304572b6d2eae922bbed1d27406f (diff) | |
parent | 2780dc39ce5d47f2d9dfa921062100a1dc86c2be (diff) | |
download | px4-firmware-18f0ee3f28763c330dfd0c0998c47c46e5edde04.tar.gz px4-firmware-18f0ee3f28763c330dfd0c0998c47c46e5edde04.tar.bz2 px4-firmware-18f0ee3f28763c330dfd0c0998c47c46e5edde04.zip |
Merge remote-tracking branch 'upstream/master' into obcfailsafe
Conflicts:
src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp
-rw-r--r-- | ROMFS/px4fmu_common/init.d/3031_phantom | 3 | ||||
-rw-r--r-- | src/drivers/sf0x/sf0x.cpp | 40 | ||||
-rw-r--r-- | src/lib/external_lgpl/tecs/tecs.cpp | 5 | ||||
-rw-r--r-- | src/lib/external_lgpl/tecs/tecs.h | 10 | ||||
-rw-r--r-- | src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp | 12 | ||||
-rw-r--r-- | src/modules/fw_pos_control_l1/fw_pos_control_l1_params.c | 13 |
6 files changed, 57 insertions, 26 deletions
diff --git a/ROMFS/px4fmu_common/init.d/3031_phantom b/ROMFS/px4fmu_common/init.d/3031_phantom index 31dfe7100..53c48d8aa 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 diff --git a/src/drivers/sf0x/sf0x.cpp b/src/drivers/sf0x/sf0x.cpp index 80ecab2ee..d382d08d0 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,15 +588,16 @@ 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') { - char buf[sizeof(_linebuf)]; - memcpy(buf, &_linebuf[i+1], (lend + 1) - (i + 1)); - 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 */ - if (i > 0 && _linebuf[i] == '.') { + if (i > 0 && ((_linebuf[i - 1] >= '0') && (_linebuf[i - 1] <= '9')) && (_linebuf[i] == '.')) { valid = true; } } @@ -607,7 +605,7 @@ SF0X::collect() if (valid) { si_units = strtod(_linebuf, &end); - /* we require at least 3 characters for a valid number */ + /* we require at least four characters for a valid number */ if (end > _linebuf + 3) { valid = true; } else { @@ -617,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; @@ -709,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; } diff --git a/src/lib/external_lgpl/tecs/tecs.cpp b/src/lib/external_lgpl/tecs/tecs.cpp index a57a0481a..d27bf776f 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 bcc2d90e5..36ae4ecaf 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 0c35ccb2c..5d62c90ab 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 @@ -578,6 +578,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)); @@ -843,7 +849,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; } @@ -1419,6 +1425,10 @@ void FixedwingPositionControl::tecs_update_pitch_throttle(float alt_sp, float v_ pitch_min_rad = M_DEG_TO_RAD_F * -1.0f; pitch_max_rad = M_DEG_TO_RAD_F * 5.0f; } + +/* 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, 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 41c374407..890ab3bad 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 |