yemai/bizhang_-obav
1
0
Fork 0
mirror of https://gitee.com/xiaohuolufeihua/bizhang_-obav.git synced 2026-05-22 01:12:31 +00:00
Code Issues Packages Projects Releases Wiki Activity

reduce scan interval to 100msec

Browse Source
This commit is contained in:
Mark Whitehorn
2016-01-11 14:47:43 -07:00
committed by Lorenz Meier
parent 9eecca6a71
commit eb36eac137
1 changed files with 59 additions and 58 deletions
Download Patch File Download Diff File Expand all files Collapse all files

117
src/drivers/px4fmu/fmu.cpp
View File

@@ -119,14 +119,16 @@ public:
int set_i2c_bus_clock(unsigned bus, unsigned clock_hz);
private:
enum RC_SCAN {
enum RC_SCAN {
RC_SCAN_PPM = 0,
RC_SCAN_SBUS,
RC_SCAN_DSM,
RC_SCAN_SUMD,
RC_SCAN_ST24
};
const char* RC_SCAN_STRING[5] = {
enum RC_SCAN _rc_scan_state = RC_SCAN_SBUS;
char const * RC_SCAN_STRING[5] = {
"PPM",
"SBUS",
"DSM",
@@ -134,7 +136,12 @@ private:
"ST24"
};
static const unsigned _max_actuators = DIRECT_PWM_OUTPUT_CHANNELS;
hrt_abstime _rc_scan_last_lock = 0;
hrt_abstime _rc_scan_begin = 0;
bool _rc_scan_locked = false;
bool _report_lock = true;
static const unsigned _max_actuators = DIRECT_PWM_OUTPUT_CHANNELS;
Mode _mode;
unsigned _pwm_default_rate;
@@ -219,6 +226,7 @@ private:
hrt_abstime now, bool frame_drop, bool failsafe,
unsigned frame_drops, int rssi);
void dsm_bind_ioctl(int dsmMode);
void set_rc_scan_state(RC_SCAN _rc_scan_state);
};
const PX4FMU::GPIOConfig PX4FMU::_gpio_tab[] = {
@@ -663,6 +671,13 @@ void PX4FMU::fill_rc_in(uint16_t raw_rc_count,
_rc_in.rc_total_frame_count = 0;
}
void PX4FMU::set_rc_scan_state(RC_SCAN newState)
{
warnx("RCscan: %s failed, trying %s", PX4FMU::RC_SCAN_STRING[_rc_scan_state], PX4FMU::RC_SCAN_STRING[newState]);
_rc_scan_begin = 0;
_rc_scan_state = newState;
}
void
PX4FMU::cycle()
{
@@ -874,14 +889,8 @@ PX4FMU::cycle()
#ifdef RC_SERIAL_PORT
// This block scans for a supported serial RC input and locks onto the first one found
static hrt_abstime rc_scan_last_lock = 0;
static hrt_abstime rc_scan_begin = 0;
static bool rc_scan_locked = false;
static bool report_lock = true;
static enum RC_SCAN rc_scan_state = RC_SCAN_SBUS;
// Scan for one second, then switch protocol
constexpr hrt_abstime rc_scan_max = 1000 * 1000;
// Scan for 100 msec, then switch protocol
constexpr hrt_abstime rc_scan_max = 100 * 1000;
bool sbus_failsafe, sbus_frame_drop;
uint16_t raw_rc_values[input_rc_s::RC_INPUT_MAX_CHANNELS];
@@ -890,25 +899,25 @@ PX4FMU::cycle()
bool dsm_11_bit;
if (report_lock && rc_scan_locked) {
report_lock = false;
warnx("fmu: %s RC input locked", RC_SCAN_STRING[rc_scan_state]);
if (_report_lock && _rc_scan_locked) {
_report_lock = false;
warnx("RCscan: %s RC input locked", RC_SCAN_STRING[_rc_scan_state]);
}
// read all available data from the serial RC input UART
hrt_abstime now = hrt_absolute_time();
int newBytes = ::read(_rcs_fd, &_rcs_buf[0], SBUS_FRAME_SIZE);
switch (rc_scan_state) {
switch (_rc_scan_state) {
case RC_SCAN_SBUS:
if (rc_scan_begin == 0) {
rc_scan_begin = now;
if (_rc_scan_begin == 0) {
_rc_scan_begin = now;
// Configure serial port for SBUS
sbus_config(_rcs_fd, false);
INVERT_RC_INPUT(true);
} else if (now - rc_scan_last_lock < rc_scan_max
|| now - rc_scan_begin < rc_scan_max) {
} else if (now - _rc_scan_last_lock < rc_scan_max
|| now - _rc_scan_begin < rc_scan_max) {
// parse new data
if (newBytes > 0) {
@@ -919,29 +928,27 @@ PX4FMU::cycle()
// we have a new SBUS frame. Publish it.
fill_rc_in(raw_rc_count, raw_rc_values, now,
sbus_frame_drop, sbus_failsafe, frame_drops);
rc_scan_last_lock = now;
rc_scan_locked = true;
_rc_scan_last_lock = now;
_rc_scan_locked = true;
}
}
} else if (!rc_scan_locked) {
// This triggers the port re-configuration
rc_scan_begin = 0;
} else if (!_rc_scan_locked) {
// Scan the next protocol
rc_scan_state = RC_SCAN_DSM;
set_rc_scan_state(RC_SCAN_DSM);
}
break;
case RC_SCAN_DSM:
if (rc_scan_begin == 0) {
rc_scan_begin = now;
if (_rc_scan_begin == 0) {
_rc_scan_begin = now;
// // Configure serial port for DSM
dsm_config(_rcs_fd);
INVERT_RC_INPUT(false);
} else if (now - rc_scan_last_lock < rc_scan_max
|| now - rc_scan_begin < rc_scan_max) {
} else if (now - _rc_scan_last_lock < rc_scan_max
|| now - _rc_scan_begin < rc_scan_max) {
if (newBytes > 0) {
// parse new data
@@ -952,29 +959,27 @@ PX4FMU::cycle()
// we have a new DSM frame. Publish it.
fill_rc_in(raw_rc_count, raw_rc_values, now,
false, false, frame_drops);
rc_scan_last_lock = now;
rc_scan_locked = true;
_rc_scan_last_lock = now;
_rc_scan_locked = true;
}
}
} else if (!rc_scan_locked) {
// This triggers the port re-configuration
rc_scan_begin = 0;
} else if (!_rc_scan_locked) {
// Scan the next protocol
rc_scan_state = RC_SCAN_ST24;
set_rc_scan_state(RC_SCAN_ST24);
}
break;
case RC_SCAN_ST24:
if (rc_scan_begin == 0) {
rc_scan_begin = now;
if (_rc_scan_begin == 0) {
_rc_scan_begin = now;
// // Configure serial port for DSM
dsm_config(_rcs_fd);
INVERT_RC_INPUT(false);
} else if (now - rc_scan_last_lock < rc_scan_max
|| now - rc_scan_begin < rc_scan_max) {
} else if (now - _rc_scan_last_lock < rc_scan_max
|| now - _rc_scan_begin < rc_scan_max) {
if (newBytes > 0) {
// parse new data
@@ -993,34 +998,32 @@ PX4FMU::cycle()
// we have a new ST24 frame. Publish it.
fill_rc_in(raw_rc_count, raw_rc_values, now,
false, false, frame_drops, st24_rssi);
rc_scan_last_lock = now;
rc_scan_locked = true;
_rc_scan_last_lock = now;
_rc_scan_locked = true;
}
}
} else if (!rc_scan_locked) {
// This triggers the port re-configuration
rc_scan_begin = 0;
} else if (!_rc_scan_locked) {
// Scan the next protocol
rc_scan_state = RC_SCAN_SUMD;
set_rc_scan_state(RC_SCAN_SUMD);
}
break;
case RC_SCAN_SUMD:
rc_scan_state = RC_SCAN_PPM;
break;
set_rc_scan_state(RC_SCAN_PPM);
break;
case RC_SCAN_PPM:
// skip PPM if it's not supported
#ifdef HRT_PPM_CHANNEL
if (rc_scan_begin == 0) {
rc_scan_begin = now;
if (_rc_scan_begin == 0) {
_rc_scan_begin = now;
// Configure timer input pin for CPPM
stm32_configgpio(GPIO_PPM_IN);
} else if (now - rc_scan_last_lock < rc_scan_max
|| now - rc_scan_begin < rc_scan_max) {
} else if (now - _rc_scan_last_lock < rc_scan_max
|| now - _rc_scan_begin < rc_scan_max) {
// see if we have new PPM input data
if ((ppm_last_valid_decode != _rc_in.timestamp_last_signal)
@@ -1029,21 +1032,19 @@ PX4FMU::cycle()
rc_updated = true;
fill_rc_in(ppm_decoded_channels, ppm_buffer, now,
false, false, 0);
rc_scan_last_lock = now;
rc_scan_locked = true;
_rc_scan_last_lock = now;
_rc_scan_locked = true;
}
} else if (!rc_scan_locked) {
// This triggers the port re-configuration
rc_scan_begin = 0;
} else if (!_rc_scan_locked) {
// disable CPPM input by mapping it away from the timer capture input
stm32_configgpio(GPIO_PPM_IN & ~(GPIO_AF_MASK | GPIO_PUPD_MASK));
// Scan the next protocol
rc_scan_state = RC_SCAN_SBUS;
set_rc_scan_state(RC_SCAN_SBUS);
}
#else // skip PPM if it's not supported
rc_scan_state = RC_SCAN_SBUS;
set_rc_scan_state(RC_SCAN_SBUS);
#endif // HRT_PPM_CHANNEL