dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

Merge remote-tracking branch 'upstream/master' into obcfailsafe 

Conflicts:
	src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp
This commit is contained in:
Thomas Gubler 2014-09-02 15:00:26 +02:00
parent fe0642d5e9 2780dc39ce
commit 18f0ee3f28
6 changed files with 57 additions and 26 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

3
ROMFS/px4fmu_common/init.d/3031_phantom
View File

@ -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

40
src/drivers/sf0x/sf0x.cpp
View File

@ -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;
}

5
src/lib/external_lgpl/tecs/tecs.cpp
View File

@ -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;

10
src/lib/external_lgpl/tecs/tecs.h
View File

@ -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;

12
src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp
View File

@ -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,

13
src/modules/fw_pos_control_l1/fw_pos_control_l1_params.c
View File

@ -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