diff options
Diffstat (limited to 'src/modules/px4iofirmware/controls.c')
| -rw-r--r-- | src/modules/px4iofirmware/controls.c | 25 |
1 files changed, 23 insertions, 2 deletions
diff --git a/src/modules/px4iofirmware/controls.c b/src/modules/px4iofirmware/controls.c index 7b127759a..58c9429b6 100644 --- a/src/modules/px4iofirmware/controls.c +++ b/src/modules/px4iofirmware/controls.c @@ -41,6 +41,7 @@ #include <stdbool.h> #include <drivers/drv_hrt.h> +#include <drivers/drv_rc_input.h> #include <systemlib/perf_counter.h> #include <systemlib/ppm_decode.h> #include <rc/st24.h> @@ -60,6 +61,8 @@ static perf_counter_t c_gather_ppm; static int _dsm_fd; +static uint16_t rc_value_override = 0; + bool dsm_port_input(uint16_t *rssi, bool *dsm_updated, bool *st24_updated) { perf_begin(c_gather_dsm); @@ -68,7 +71,6 @@ bool dsm_port_input(uint16_t *rssi, bool *dsm_updated, bool *st24_updated) uint8_t *bytes; *dsm_updated = dsm_input(r_raw_rc_values, &temp_count, &n_bytes, &bytes); if (*dsm_updated) { - r_raw_rc_flags |= PX4IO_P_STATUS_FLAGS_RC_DSM; r_raw_rc_count = temp_count & 0x7fff; if (temp_count & 0x8000) r_raw_rc_flags |= PX4IO_P_RAW_RC_FLAGS_RC_DSM11; @@ -89,6 +91,7 @@ bool dsm_port_input(uint16_t *rssi, bool *dsm_updated, bool *st24_updated) for (unsigned i = 0; i < n_bytes; i++) { /* set updated flag if one complete packet was parsed */ + st24_rssi = RC_INPUT_RSSI_MAX; *st24_updated |= (OK == st24_decode(bytes[i], &st24_rssi, &rx_count, &st24_channel_count, r_raw_rc_values, PX4IO_RC_INPUT_CHANNELS)); } @@ -168,6 +171,12 @@ controls_tick() { perf_begin(c_gather_dsm); bool dsm_updated, st24_updated; (void)dsm_port_input(&rssi, &dsm_updated, &st24_updated); + if (dsm_updated) { + r_status_flags |= PX4IO_P_STATUS_FLAGS_RC_DSM; + } + if (st24_updated) { + r_status_flags |= PX4IO_P_STATUS_FLAGS_RC_ST24; + } perf_end(c_gather_dsm); perf_begin(c_gather_sbus); @@ -316,6 +325,9 @@ controls_tick() { r_rc_values[mapped] = SIGNED_TO_REG(scaled); assigned_channels |= (1 << mapped); + } else if (mapped == PX4IO_P_RC_CONFIG_ASSIGNMENT_MODESWITCH) { + /* pick out override channel, indicated by special mapping */ + rc_value_override = SIGNED_TO_REG(scaled); } } } @@ -409,9 +421,18 @@ controls_tick() { * requested override. * */ - if ((r_status_flags & PX4IO_P_STATUS_FLAGS_RC_OK) && (REG_TO_SIGNED(r_rc_values[4]) < RC_CHANNEL_LOW_THRESH)) + if ((r_status_flags & PX4IO_P_STATUS_FLAGS_RC_OK) && (REG_TO_SIGNED(rc_value_override) < RC_CHANNEL_LOW_THRESH)) override = true; + /* + if the FMU is dead then enable override if we have a + mixer and OVERRIDE_IMMEDIATE is set + */ + if (!(r_status_flags & PX4IO_P_STATUS_FLAGS_FMU_OK) && + (r_setup_arming & PX4IO_P_SETUP_ARMING_OVERRIDE_IMMEDIATE) && + (r_status_flags & PX4IO_P_STATUS_FLAGS_MIXER_OK)) + override = true; + if (override) { r_status_flags |= PX4IO_P_STATUS_FLAGS_OVERRIDE; |