diff --git a/src/drivers/boards/px4fmu-v4/board_config.h b/src/drivers/boards/px4fmu-v4/board_config.h index 3500d35e61..9a89a7e962 100644 --- a/src/drivers/boards/px4fmu-v4/board_config.h +++ b/src/drivers/boards/px4fmu-v4/board_config.h @@ -214,6 +214,8 @@ __BEGIN_DECLS #define HRT_PPM_CHANNEL 3 /* use capture/compare channel 2 */ #define GPIO_PPM_IN (GPIO_ALT|GPIO_AF2|GPIO_PULLUP|GPIO_PORTB|GPIO_PIN0) +#define SBUS_SERIAL_PORT "/dev/ttyS4" /* XXX not vetted */ + /* PWM input driver. Use FMU AUX5 pins attached to timer4 channel 2 */ #define PWMIN_TIMER 4 #define PWMIN_TIMER_CHANNEL 2 diff --git a/src/drivers/px4fmu/fmu.cpp b/src/drivers/px4fmu/fmu.cpp index 9e1ea99861..452cf1fc2c 100644 --- a/src/drivers/px4fmu/fmu.cpp +++ b/src/drivers/px4fmu/fmu.cpp @@ -71,6 +71,11 @@ #include #include +#include +#include +#include +#include + #include #include #include @@ -799,6 +804,45 @@ PX4FMU::cycle() update_pwm_rev_mask(); } + bool rc_updated = false; + +#ifdef SBUS_SERIAL_PORT + bool sbus_failsafe, sbus_frame_drop; + uint16_t raw_rc_values[input_rc_s::RC_INPUT_MAX_CHANNELS]; + uint16_t raw_rc_count; + bool sbus_updated = sbus_input(_sbus_fd, &raw_rc_values[0], &raw_rc_count, &sbus_failsafe, &sbus_frame_drop, + input_rc_s::RC_INPUT_MAX_CHANNELS); + + if (sbus_updated) { + // we have a new PPM frame. Publish it. + _rc_in.channel_count = raw_rc_count; + + if (_rc_in.channel_count > input_rc_s::RC_INPUT_MAX_CHANNELS) { + _rc_in.channel_count = input_rc_s::RC_INPUT_MAX_CHANNELS; + } + + for (uint8_t i = 0; i < _rc_in.channel_count; i++) { + _rc_in.values[i] = raw_rc_values[i]; + } + + _rc_in.timestamp_publication = hrt_absolute_time(); + _rc_in.timestamp_last_signal = _rc_in.timestamp_publication; + + _rc_in.rc_ppm_frame_length = 0; + _rc_in.rssi = (!sbus_frame_drop) ? RC_INPUT_RSSI_MAX : 0; + _rc_in.rc_failsafe = false; + _rc_in.rc_lost = false; + _rc_in.rc_lost_frame_count = 0; + _rc_in.rc_total_frame_count = 0; + + rc_updated = true; + } +#endif + +#ifdef DSM_SERIAL_PORT + //_dsm_fd = dsm_init(DSM_SERIAL_PORT); +#endif + #ifdef HRT_PPM_CHANNEL // see if we have new PPM input data @@ -824,6 +868,12 @@ PX4FMU::cycle() _rc_in.rc_lost_frame_count = 0; _rc_in.rc_total_frame_count = 0; + rc_updated = true; + } + +#endif + + if (rc_updated) { /* lazily advertise on first publication */ if (_to_input_rc == nullptr) { _to_input_rc = orb_advertise(ORB_ID(input_rc), &_rc_in); @@ -833,7 +883,6 @@ PX4FMU::cycle() } } -#endif work_queue(HPWORK, &_work, (worker_t)&PX4FMU::cycle_trampoline, this, USEC2TICK(CONTROL_INPUT_DROP_LIMIT_MS * 1000)); } diff --git a/src/modules/px4iofirmware/controls.c b/src/modules/px4iofirmware/controls.c index 25874868b1..fa8d5ad48a 100644 --- a/src/modules/px4iofirmware/controls.c +++ b/src/modules/px4iofirmware/controls.c @@ -77,7 +77,7 @@ bool dsm_port_input(uint16_t *rssi, bool *dsm_updated, bool *st24_updated, bool uint16_t temp_count = r_raw_rc_count; uint8_t n_bytes = 0; uint8_t *bytes; - *dsm_updated = dsm_input(r_raw_rc_values, &temp_count, &n_bytes, &bytes); + *dsm_updated = dsm_input(r_raw_rc_values, &temp_count, &n_bytes, &bytes, PX4IO_RC_INPUT_CHANNELS); if (*dsm_updated) { r_raw_rc_count = temp_count & 0x7fff;