fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-21 06:13:35 +08:00
Code Issues Packages Projects Releases Wiki Activity

add SUMD decoder to RCscan

Browse Source
This commit is contained in:
Mark Whitehorn
2016-01-14 17:55:53 +01:00
committed by Lorenz Meier
parent 3d185e18e9
commit e0bbbd356f
1 changed files with 98 additions and 62 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/drivers/px4fmu/fmu.cpp
+98 -62
View File
@@ -119,29 +119,29 @@ public:
int set_i2c_bus_clock(unsigned bus, unsigned clock_hz); int set_i2c_bus_clock(unsigned bus, unsigned clock_hz);
private: private:
enum RC_SCAN { enum RC_SCAN {
RC_SCAN_PPM = 0, RC_SCAN_PPM = 0,
RC_SCAN_SBUS, RC_SCAN_SBUS,
RC_SCAN_DSM, RC_SCAN_DSM,
RC_SCAN_SUMD, RC_SCAN_SUMD,
RC_SCAN_ST24 RC_SCAN_ST24
}; };
enum RC_SCAN _rc_scan_state = RC_SCAN_SBUS; enum RC_SCAN _rc_scan_state = RC_SCAN_SBUS;
char const * RC_SCAN_STRING[5] = { char const *RC_SCAN_STRING[5] = {
"PPM", "PPM",
"SBUS", "SBUS",
"DSM", "DSM",
"SUMD", "SUMD",
"ST24" "ST24"
}; };
hrt_abstime _rc_scan_last_lock = 0; hrt_abstime _rc_scan_last_lock = 0;
hrt_abstime _rc_scan_begin = 0; hrt_abstime _rc_scan_begin = 0;
bool _rc_scan_locked = false; bool _rc_scan_locked = false;
bool _report_lock = true; bool _report_lock = true;
static const unsigned _max_actuators = DIRECT_PWM_OUTPUT_CHANNELS; static const unsigned _max_actuators = DIRECT_PWM_OUTPUT_CHANNELS;
Mode _mode; Mode _mode;
unsigned _pwm_default_rate; unsigned _pwm_default_rate;
@@ -226,7 +226,7 @@ private:
hrt_abstime now, bool frame_drop, bool failsafe, hrt_abstime now, bool frame_drop, bool failsafe,
unsigned frame_drops, int rssi); unsigned frame_drops, int rssi);
void dsm_bind_ioctl(int dsmMode); void dsm_bind_ioctl(int dsmMode);
void set_rc_scan_state(RC_SCAN _rc_scan_state); void set_rc_scan_state(RC_SCAN _rc_scan_state);
}; };
const PX4FMU::GPIOConfig PX4FMU::_gpio_tab[] = { const PX4FMU::GPIOConfig PX4FMU::_gpio_tab[] = {
@@ -674,8 +674,8 @@ void PX4FMU::fill_rc_in(uint16_t raw_rc_count,
void PX4FMU::set_rc_scan_state(RC_SCAN newState) 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]); // warnx("RCscan: %s failed, trying %s", PX4FMU::RC_SCAN_STRING[_rc_scan_state], PX4FMU::RC_SCAN_STRING[newState]);
_rc_scan_begin = 0; _rc_scan_begin = 0;
_rc_scan_state = newState; _rc_scan_state = newState;
} }
void void
@@ -705,7 +705,7 @@ PX4FMU::cycle()
stm32_configgpio(GPIO_PPM_IN & ~(GPIO_AF_MASK | GPIO_PUPD_MASK)); stm32_configgpio(GPIO_PPM_IN & ~(GPIO_AF_MASK | GPIO_PUPD_MASK));
#endif #endif
_initialized = true; _initialized = true;
} }
if (_groups_subscribed != _groups_required) { if (_groups_subscribed != _groups_required) {
@@ -889,8 +889,8 @@ PX4FMU::cycle()
#ifdef RC_SERIAL_PORT #ifdef RC_SERIAL_PORT
// This block scans for a supported serial RC input and locks onto the first one found // This block scans for a supported serial RC input and locks onto the first one found
// Scan for 100 msec, then switch protocol // Scan for 100 msec, then switch protocol
constexpr hrt_abstime rc_scan_max = 100 * 1000; constexpr hrt_abstime rc_scan_max = 100 * 1000;
bool sbus_failsafe, sbus_frame_drop; bool sbus_failsafe, sbus_frame_drop;
uint16_t raw_rc_values[input_rc_s::RC_INPUT_MAX_CHANNELS]; uint16_t raw_rc_values[input_rc_s::RC_INPUT_MAX_CHANNELS];
@@ -899,25 +899,25 @@ PX4FMU::cycle()
bool dsm_11_bit; bool dsm_11_bit;
if (_report_lock && _rc_scan_locked) { if (_report_lock && _rc_scan_locked) {
_report_lock = false; _report_lock = false;
warnx("RCscan: %s RC input locked", RC_SCAN_STRING[_rc_scan_state]); warnx("RCscan: %s RC input locked", RC_SCAN_STRING[_rc_scan_state]);
} }
// read all available data from the serial RC input UART // read all available data from the serial RC input UART
hrt_abstime now = hrt_absolute_time(); hrt_abstime now = hrt_absolute_time();
int newBytes = ::read(_rcs_fd, &_rcs_buf[0], SBUS_FRAME_SIZE); int newBytes = ::read(_rcs_fd, &_rcs_buf[0], SBUS_FRAME_SIZE);
switch (_rc_scan_state) { switch (_rc_scan_state) {
case RC_SCAN_SBUS: case RC_SCAN_SBUS:
if (_rc_scan_begin == 0) { if (_rc_scan_begin == 0) {
_rc_scan_begin = now; _rc_scan_begin = now;
// Configure serial port for SBUS // Configure serial port for SBUS
sbus_config(_rcs_fd, false); sbus_config(_rcs_fd, false);
INVERT_RC_INPUT(true); INVERT_RC_INPUT(true);
} else if (now - _rc_scan_last_lock < rc_scan_max } else if (now - _rc_scan_last_lock < rc_scan_max
|| now - _rc_scan_begin < rc_scan_max) { || now - _rc_scan_begin < rc_scan_max) {
// parse new data // parse new data
if (newBytes > 0) { if (newBytes > 0) {
@@ -928,27 +928,27 @@ PX4FMU::cycle()
// we have a new SBUS frame. Publish it. // we have a new SBUS frame. Publish it.
fill_rc_in(raw_rc_count, raw_rc_values, now, fill_rc_in(raw_rc_count, raw_rc_values, now,
sbus_frame_drop, sbus_failsafe, frame_drops); sbus_frame_drop, sbus_failsafe, frame_drops);
_rc_scan_last_lock = now; _rc_scan_last_lock = now;
_rc_scan_locked = true; _rc_scan_locked = true;
} }
} }
} else if (!_rc_scan_locked) { } else if (!_rc_scan_locked) {
// Scan the next protocol // Scan the next protocol
set_rc_scan_state(RC_SCAN_DSM); set_rc_scan_state(RC_SCAN_DSM);
} }
break; break;
case RC_SCAN_DSM: case RC_SCAN_DSM:
if (_rc_scan_begin == 0) { if (_rc_scan_begin == 0) {
_rc_scan_begin = now; _rc_scan_begin = now;
// // Configure serial port for DSM // // Configure serial port for DSM
dsm_config(_rcs_fd); dsm_config(_rcs_fd);
INVERT_RC_INPUT(false); INVERT_RC_INPUT(false);
} else if (now - _rc_scan_last_lock < rc_scan_max } else if (now - _rc_scan_last_lock < rc_scan_max
|| now - _rc_scan_begin < rc_scan_max) { || now - _rc_scan_begin < rc_scan_max) {
if (newBytes > 0) { if (newBytes > 0) {
// parse new data // parse new data
@@ -959,27 +959,27 @@ PX4FMU::cycle()
// we have a new DSM frame. Publish it. // we have a new DSM frame. Publish it.
fill_rc_in(raw_rc_count, raw_rc_values, now, fill_rc_in(raw_rc_count, raw_rc_values, now,
false, false, frame_drops); false, false, frame_drops);
_rc_scan_last_lock = now; _rc_scan_last_lock = now;
_rc_scan_locked = true; _rc_scan_locked = true;
} }
} }
} else if (!_rc_scan_locked) { } else if (!_rc_scan_locked) {
// Scan the next protocol // Scan the next protocol
set_rc_scan_state(RC_SCAN_ST24); set_rc_scan_state(RC_SCAN_ST24);
} }
break; break;
case RC_SCAN_ST24: case RC_SCAN_ST24:
if (_rc_scan_begin == 0) { if (_rc_scan_begin == 0) {
_rc_scan_begin = now; _rc_scan_begin = now;
// // Configure serial port for DSM // // Configure serial port for DSM
dsm_config(_rcs_fd); dsm_config(_rcs_fd);
INVERT_RC_INPUT(false); INVERT_RC_INPUT(false);
} else if (now - _rc_scan_last_lock < rc_scan_max } else if (now - _rc_scan_last_lock < rc_scan_max
|| now - _rc_scan_begin < rc_scan_max) { || now - _rc_scan_begin < rc_scan_max) {
if (newBytes > 0) { if (newBytes > 0) {
// parse new data // parse new data
@@ -990,40 +990,76 @@ PX4FMU::cycle()
for (unsigned i = 0; i < newBytes; i++) { for (unsigned i = 0; i < newBytes; i++) {
/* set updated flag if one complete packet was parsed */ /* set updated flag if one complete packet was parsed */
st24_rssi = RC_INPUT_RSSI_MAX; st24_rssi = RC_INPUT_RSSI_MAX;
rc_updated |= (OK == st24_decode(_rcs_buf[i], &st24_rssi, &rx_count, rc_updated = (OK == st24_decode(_rcs_buf[i], &st24_rssi, &rx_count,
&raw_rc_count, raw_rc_values, input_rc_s::RC_INPUT_MAX_CHANNELS)); &raw_rc_count, raw_rc_values, input_rc_s::RC_INPUT_MAX_CHANNELS));
} }
if (rc_updated) { if (rc_updated) {
// we have a new ST24 frame. Publish it. // we have a new ST24 frame. Publish it.
fill_rc_in(raw_rc_count, raw_rc_values, now, fill_rc_in(raw_rc_count, raw_rc_values, now,
false, false, frame_drops, st24_rssi); false, false, frame_drops, st24_rssi);
_rc_scan_last_lock = now; _rc_scan_last_lock = now;
_rc_scan_locked = true; _rc_scan_locked = true;
} }
} }
} else if (!_rc_scan_locked) { } else if (!_rc_scan_locked) {
// Scan the next protocol // Scan the next protocol
set_rc_scan_state(RC_SCAN_SUMD); set_rc_scan_state(RC_SCAN_SUMD);
} }
break; break;
case RC_SCAN_SUMD: case RC_SCAN_SUMD:
set_rc_scan_state(RC_SCAN_PPM); if (_rc_scan_begin == 0) {
break; _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) {
if (newBytes > 0) {
// parse new data
uint8_t sumd_rssi, rx_count;
rc_updated = false;
for (unsigned i = 0; i < newBytes; i++) {
/* set updated flag if one complete packet was parsed */
sumd_rssi = RC_INPUT_RSSI_MAX;
rc_updated = (OK == sumd_decode(_rcs_buf[i], &sumd_rssi, &rx_count,
&raw_rc_count, raw_rc_values, input_rc_s::RC_INPUT_MAX_CHANNELS));
}
if (rc_updated) {
// we have a new SUMD frame. Publish it.
fill_rc_in(raw_rc_count, raw_rc_values, now,
false, false, frame_drops, sumd_rssi);
_rc_scan_last_lock = now;
_rc_scan_locked = true;
}
}
} else if (!_rc_scan_locked) {
// Scan the next protocol
set_rc_scan_state(RC_SCAN_SUMD);
}
set_rc_scan_state(RC_SCAN_PPM);
break;
case RC_SCAN_PPM: case RC_SCAN_PPM:
// skip PPM if it's not supported // skip PPM if it's not supported
#ifdef HRT_PPM_CHANNEL #ifdef HRT_PPM_CHANNEL
if (_rc_scan_begin == 0) { if (_rc_scan_begin == 0) {
_rc_scan_begin = now; _rc_scan_begin = now;
// Configure timer input pin for CPPM // Configure timer input pin for CPPM
stm32_configgpio(GPIO_PPM_IN); stm32_configgpio(GPIO_PPM_IN);
} else if (now - _rc_scan_last_lock < rc_scan_max } else if (now - _rc_scan_last_lock < rc_scan_max
|| now - _rc_scan_begin < rc_scan_max) { || now - _rc_scan_begin < rc_scan_max) {
// see if we have new PPM input data // see if we have new PPM input data
if ((ppm_last_valid_decode != _rc_in.timestamp_last_signal) if ((ppm_last_valid_decode != _rc_in.timestamp_last_signal)
@@ -1032,19 +1068,19 @@ PX4FMU::cycle()
rc_updated = true; rc_updated = true;
fill_rc_in(ppm_decoded_channels, ppm_buffer, now, fill_rc_in(ppm_decoded_channels, ppm_buffer, now,
false, false, 0); false, false, 0);
_rc_scan_last_lock = now; _rc_scan_last_lock = now;
_rc_scan_locked = true; _rc_scan_locked = true;
} }
} else if (!_rc_scan_locked) { } else if (!_rc_scan_locked) {
// disable CPPM input by mapping it away from the timer capture input // disable CPPM input by mapping it away from the timer capture input
stm32_configgpio(GPIO_PPM_IN & ~(GPIO_AF_MASK | GPIO_PUPD_MASK)); stm32_configgpio(GPIO_PPM_IN & ~(GPIO_AF_MASK | GPIO_PUPD_MASK));
// Scan the next protocol // Scan the next protocol
set_rc_scan_state(RC_SCAN_SBUS); set_rc_scan_state(RC_SCAN_SBUS);
} }
#else // skip PPM if it's not supported #else // skip PPM if it's not supported
set_rc_scan_state(RC_SCAN_SBUS); set_rc_scan_state(RC_SCAN_SBUS);
#endif // HRT_PPM_CHANNEL #endif // HRT_PPM_CHANNEL