From 1d3f25ee6c9983ec5da9de4d4f7b463f880f3a87 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Sat, 19 Oct 2013 10:43:41 +0200 Subject: pwm systemcmd can now set the failsafe values, fmu uses failsafe values as well now, fix to only send the appropriate number of pwm values to IO at once --- src/modules/px4iofirmware/registers.c | 13 ++++++++++--- 1 file changed, 10 insertions(+), 3 deletions(-) (limited to 'src/modules/px4iofirmware/registers.c') diff --git a/src/modules/px4iofirmware/registers.c b/src/modules/px4iofirmware/registers.c index a338afe16..6a0532bee 100644 --- a/src/modules/px4iofirmware/registers.c +++ b/src/modules/px4iofirmware/registers.c @@ -199,7 +199,7 @@ uint16_t r_page_rc_input_config[PX4IO_CONTROL_CHANNELS * PX4IO_P_RC_CONFIG_STRI * * Disable pulses as default. */ -uint16_t r_page_servo_failsafe[PX4IO_SERVO_COUNT] = { 0 }; +uint16_t r_page_servo_failsafe[PX4IO_SERVO_COUNT] = { 0, 0, 0, 0, 0, 0, 0, 0 }; /** * PAGE 106 @@ -276,8 +276,15 @@ registers_set(uint8_t page, uint8_t offset, const uint16_t *values, unsigned num /* copy channel data */ while ((offset < PX4IO_SERVO_COUNT) && (num_values > 0)) { - /* XXX range-check value? */ - r_page_servo_failsafe[offset] = *values; + if (*values == 0) { + /* ignore 0 */ + } else if (*values < PWM_MIN) { + r_page_servo_failsafe[offset] = PWM_MIN; + } else if (*values > PWM_MAX) { + r_page_servo_failsafe[offset] = PWM_MAX; + } else { + r_page_servo_failsafe[offset] = *values; + } /* flag the failsafe values as custom */ r_setup_arming |= PX4IO_P_SETUP_ARMING_FAILSAFE_CUSTOM; -- cgit v1.2.3