diff options
| author | Lorenz Meier <lm@inf.ethz.ch> | 2013-10-30 09:03:19 +0100 |
|---|---|---|
| committer | Lorenz Meier <lm@inf.ethz.ch> | 2013-10-30 09:03:19 +0100 |
| commit | 44f88bf0a741fe530dea1c8b3137f30117a7b4b5 (patch) | |
| tree | 422fde62a2e15f0e8f8ccf24d1b3137f714e676b /src/modules/px4iofirmware/registers.c | |
| parent | 2293aa4e0ae809e31ba1aa71492253460a5e3aab (diff) | |
| download | px4-firmware-44f88bf0a741fe530dea1c8b3137f30117a7b4b5.tar.gz px4-firmware-44f88bf0a741fe530dea1c8b3137f30117a7b4b5.tar.bz2 px4-firmware-44f88bf0a741fe530dea1c8b3137f30117a7b4b5.zip | |
Fix to allow setting again zero disarmed PWM values after boot
Diffstat (limited to 'src/modules/px4iofirmware/registers.c')
| -rw-r--r-- | src/modules/px4iofirmware/registers.c | 45 |
1 files changed, 29 insertions, 16 deletions
diff --git a/src/modules/px4iofirmware/registers.c b/src/modules/px4iofirmware/registers.c index 6a0532bee..40597adf1 100644 --- a/src/modules/px4iofirmware/registers.c +++ b/src/modules/px4iofirmware/registers.c @@ -338,26 +338,39 @@ registers_set(uint8_t page, uint8_t offset, const uint16_t *values, unsigned num break; case PX4IO_PAGE_DISARMED_PWM: + { + /* flag for all outputs */ + bool all_disarmed_off = true; + + /* copy channel data */ + while ((offset < PX4IO_SERVO_COUNT) && (num_values > 0)) { + + if (*values == 0) { + /* 0 means disabling always PWM */ + r_page_servo_disarmed[offset] = 0; + } else if (*values < PWM_MIN) { + r_page_servo_disarmed[offset] = PWM_MIN; + all_disarmed_off = false; + } else if (*values > PWM_MAX) { + r_page_servo_disarmed[offset] = PWM_MAX; + all_disarmed_off = false; + } else { + r_page_servo_disarmed[offset] = *values; + all_disarmed_off = false; + } - /* copy channel data */ - while ((offset < PX4IO_SERVO_COUNT) && (num_values > 0)) { + offset++; + num_values--; + values++; + } - if (*values == 0) { - /* ignore 0 */ - } else if (*values < PWM_MIN) { - r_page_servo_disarmed[offset] = PWM_MIN; - } else if (*values > PWM_MAX) { - r_page_servo_disarmed[offset] = PWM_MAX; + if (all_disarmed_off) { + /* disable PWM output if disarmed */ + r_setup_arming &= ~(PX4IO_P_SETUP_ARMING_ALWAYS_PWM_ENABLE); } else { - r_page_servo_disarmed[offset] = *values; + /* enable PWM output always */ + r_setup_arming |= PX4IO_P_SETUP_ARMING_ALWAYS_PWM_ENABLE; } - - /* flag the failsafe values as custom */ - r_setup_arming |= PX4IO_P_SETUP_ARMING_ALWAYS_PWM_ENABLE; - - offset++; - num_values--; - values++; } break; |