diff options
Diffstat (limited to 'src/modules/px4iofirmware')
-rw-r--r-- | src/modules/px4iofirmware/mixer.cpp | 66 | ||||
-rw-r--r-- | src/modules/px4iofirmware/protocol.h | 4 | ||||
-rw-r--r-- | src/modules/px4iofirmware/registers.c | 10 |
3 files changed, 71 insertions, 9 deletions
diff --git a/src/modules/px4iofirmware/mixer.cpp b/src/modules/px4iofirmware/mixer.cpp index 1234e2eea..0b8ed6dc5 100644 --- a/src/modules/px4iofirmware/mixer.cpp +++ b/src/modules/px4iofirmware/mixer.cpp @@ -85,6 +85,9 @@ static int mixer_callback(uintptr_t handle, static MixerGroup mixer_group(mixer_callback, 0); +/* Set the failsafe values of all mixed channels (based on zero throttle, controls centered) */ +static void mixer_set_failsafe(); + void mixer_tick(void) { @@ -102,13 +105,16 @@ mixer_tick(void) r_status_flags |= PX4IO_P_STATUS_FLAGS_FMU_OK; } + /* default to failsafe mixing */ source = MIX_FAILSAFE; /* * Decide which set of controls we're using. */ - if ((r_status_flags & PX4IO_P_STATUS_FLAGS_RAW_PWM) || - !(r_status_flags & PX4IO_P_STATUS_FLAGS_MIXER_OK)) { + + /* do not mix if mixer is invalid or if RAW_PWM mode is on and FMU is good */ + if ((r_status_flags & PX4IO_P_STATUS_FLAGS_RAW_PWM) && + !(r_status_flags & PX4IO_P_STATUS_FLAGS_MIXER_OK)) { /* don't actually mix anything - we already have raw PWM values or not a valid mixer. */ @@ -117,6 +123,7 @@ mixer_tick(void) } else { if (!(r_status_flags & PX4IO_P_STATUS_FLAGS_OVERRIDE) && + (r_status_flags & PX4IO_P_STATUS_FLAGS_FMU_OK) && (r_status_flags & PX4IO_P_STATUS_FLAGS_MIXER_OK)) { /* mix from FMU controls */ @@ -133,14 +140,28 @@ mixer_tick(void) } /* + * Set failsafe status flag depending on mixing source + */ + if (source == MIX_FAILSAFE) { + r_status_flags |= PX4IO_P_STATUS_FLAGS_FAILSAFE; + } else { + r_status_flags &= ~(PX4IO_P_STATUS_FLAGS_FAILSAFE); + } + + /* * Run the mixers. */ if (source == MIX_FAILSAFE) { /* copy failsafe values to the servo outputs */ - for (unsigned i = 0; i < IO_SERVO_COUNT; i++) + for (unsigned i = 0; i < IO_SERVO_COUNT; i++) { r_page_servos[i] = r_page_servo_failsafe[i]; + /* safe actuators for FMU feedback */ + r_page_actuators[i] = (r_page_servos[i] - 1500) / 600.0f; + } + + } else if (source != MIX_NONE) { float outputs[IO_SERVO_COUNT]; @@ -156,7 +177,7 @@ mixer_tick(void) r_page_actuators[i] = FLOAT_TO_REG(outputs[i]); /* scale to servo output */ - r_page_servos[i] = (outputs[i] * 500.0f) + 1500; + r_page_servos[i] = (outputs[i] * 600.0f) + 1500; } for (unsigned i = mixed; i < IO_SERVO_COUNT; i++) @@ -175,7 +196,7 @@ mixer_tick(void) bool should_arm = ( /* FMU is armed */ (r_setup_arming & PX4IO_P_SETUP_ARMING_FMU_ARMED) && /* IO is armed */ (r_status_flags & PX4IO_P_STATUS_FLAGS_ARMED) && - /* there is valid input */ (r_status_flags & (PX4IO_P_STATUS_FLAGS_RAW_PWM | PX4IO_P_STATUS_FLAGS_MIXER_OK)) && + /* there is valid input via direct PWM or mixer */ (r_status_flags & (PX4IO_P_STATUS_FLAGS_RAW_PWM | PX4IO_P_STATUS_FLAGS_MIXER_OK)) && /* IO initialised without error */ (r_status_flags & PX4IO_P_STATUS_FLAGS_INIT_OK) && /* FMU is available or FMU is not available but override is an option */ ((r_status_flags & PX4IO_P_STATUS_FLAGS_FMU_OK) || (!(r_status_flags & PX4IO_P_STATUS_FLAGS_FMU_OK) && (r_setup_arming & PX4IO_P_SETUP_ARMING_MANUAL_OVERRIDE_OK) )) @@ -225,7 +246,7 @@ mixer_callback(uintptr_t handle, case MIX_FAILSAFE: case MIX_NONE: - /* XXX we could allow for configuration of per-output failsafe values */ + control = 0.0f; return -1; } @@ -303,8 +324,41 @@ mixer_handle_text(const void *buffer, size_t length) memcpy(&mixer_text[0], &mixer_text[mixer_text_length - resid], resid); mixer_text_length = resid; + + /* update failsafe values */ + mixer_set_failsafe(); } break; } } + +static void +mixer_set_failsafe() +{ + /* + * Check if a custom failsafe value has been written, + * else use the opportunity to set decent defaults. + */ + if (r_setup_arming & PX4IO_P_SETUP_ARMING_FAILSAFE_CUSTOM) + return; + + float outputs[IO_SERVO_COUNT]; + unsigned mixed; + + /* mix */ + mixed = mixer_group.mix(&outputs[0], IO_SERVO_COUNT); + + /* scale to PWM and update the servo outputs as required */ + for (unsigned i = 0; i < mixed; i++) { + + /* scale to servo output */ + r_page_servo_failsafe[i] = (outputs[i] * 600.0f) + 1500; + + } + + /* disable the rest of the outputs */ + for (unsigned i = mixed; i < IO_SERVO_COUNT; i++) + r_page_servo_failsafe[i] = 0; + +} diff --git a/src/modules/px4iofirmware/protocol.h b/src/modules/px4iofirmware/protocol.h index e575bd841..674f9dddd 100644 --- a/src/modules/px4iofirmware/protocol.h +++ b/src/modules/px4iofirmware/protocol.h @@ -85,7 +85,7 @@ #define PX4IO_P_CONFIG_ACTUATOR_COUNT 5 /* hardcoded max actuator output count */ #define PX4IO_P_CONFIG_RC_INPUT_COUNT 6 /* hardcoded max R/C input count supported */ #define PX4IO_P_CONFIG_ADC_INPUT_COUNT 7 /* hardcoded max ADC inputs */ -#define PX4IO_P_CONFIG_RELAY_COUNT 8 /* harcoded # of relay outputs */ +#define PX4IO_P_CONFIG_RELAY_COUNT 8 /* hardcoded # of relay outputs */ /* dynamic status page */ #define PX4IO_PAGE_STATUS 1 @@ -104,6 +104,7 @@ #define PX4IO_P_STATUS_FLAGS_MIXER_OK (1 << 8) /* mixer is OK */ #define PX4IO_P_STATUS_FLAGS_ARM_SYNC (1 << 9) /* the arming state between IO and FMU is in sync */ #define PX4IO_P_STATUS_FLAGS_INIT_OK (1 << 10) /* initialisation of the IO completed without error */ +#define PX4IO_P_STATUS_FLAGS_FAILSAFE (1 << 11) /* failsafe is active */ #define PX4IO_P_STATUS_ALARMS 3 /* alarm flags - alarms latch, write 1 to a bit to clear it */ #define PX4IO_P_STATUS_ALARMS_VBATT_LOW (1 << 0) /* VBatt is very close to regulator dropout */ @@ -148,6 +149,7 @@ #define PX4IO_P_SETUP_ARMING_IO_ARM_OK (1 << 0) /* OK to arm the IO side */ #define PX4IO_P_SETUP_ARMING_FMU_ARMED (1 << 1) /* FMU is already armed */ #define PX4IO_P_SETUP_ARMING_MANUAL_OVERRIDE_OK (1 << 2) /* OK to switch to manual override via override RC channel */ +#define PX4IO_P_SETUP_ARMING_FAILSAFE_CUSTOM (1 << 3) /* use custom failsafe values, not 0 values of mixer */ #define PX4IO_P_SETUP_ARMING_INAIR_RESTART_OK (1 << 4) /* OK to try in-air restart */ #define PX4IO_P_SETUP_PWM_RATES 2 /* bitmask, 0 = low rate, 1 = high rate */ diff --git a/src/modules/px4iofirmware/registers.c b/src/modules/px4iofirmware/registers.c index 9698a0f2f..df7d6dcd3 100644 --- a/src/modules/px4iofirmware/registers.c +++ b/src/modules/px4iofirmware/registers.c @@ -41,6 +41,7 @@ #include <stdbool.h> #include <stdlib.h> +#include <string.h> #include <drivers/drv_hrt.h> @@ -178,8 +179,10 @@ uint16_t r_page_rc_input_config[MAX_CONTROL_CHANNELS * PX4IO_P_RC_CONFIG_STRIDE * PAGE 105 * * Failsafe servo PWM values + * + * Disable pulses as default. */ -uint16_t r_page_servo_failsafe[IO_SERVO_COUNT]; +uint16_t r_page_servo_failsafe[IO_SERVO_COUNT] = { 0 }; void registers_set(uint8_t page, uint8_t offset, const uint16_t *values, unsigned num_values) @@ -230,11 +233,14 @@ registers_set(uint8_t page, uint8_t offset, const uint16_t *values, unsigned num case PX4IO_PAGE_FAILSAFE_PWM: /* copy channel data */ - while ((offset < PX4IO_CONTROL_CHANNELS) && (num_values > 0)) { + while ((offset < IO_SERVO_COUNT) && (num_values > 0)) { /* XXX range-check value? */ r_page_servo_failsafe[offset] = *values; + /* flag the failsafe values as custom */ + r_setup_arming |= PX4IO_P_SETUP_ARMING_FAILSAFE_CUSTOM; + offset++; num_values--; values++; |