diff options
Diffstat (limited to 'src/modules/px4iofirmware/registers.c')
| -rw-r--r-- | src/modules/px4iofirmware/registers.c | 119 |
1 files changed, 112 insertions, 7 deletions
diff --git a/src/modules/px4iofirmware/registers.c b/src/modules/px4iofirmware/registers.c index 3f241d29c..3ff9307cd 100644 --- a/src/modules/px4iofirmware/registers.c +++ b/src/modules/px4iofirmware/registers.c @@ -158,7 +158,9 @@ volatile uint16_t r_page_setup[] = #define PX4IO_P_SETUP_ARMING_VALID (PX4IO_P_SETUP_ARMING_FMU_ARMED | \ PX4IO_P_SETUP_ARMING_MANUAL_OVERRIDE_OK | \ PX4IO_P_SETUP_ARMING_INAIR_RESTART_OK | \ - PX4IO_P_SETUP_ARMING_IO_ARM_OK) + PX4IO_P_SETUP_ARMING_IO_ARM_OK) | \ + PX4IO_P_SETUP_ARMING_FAILSAFE_CUSTOM | \ + PX4IO_P_SETUP_ARMING_ALWAYS_PWM_ENABLE #define PX4IO_P_SETUP_RATES_VALID ((1 << PX4IO_SERVO_COUNT) - 1) #define PX4IO_P_SETUP_RELAYS_VALID ((1 << PX4IO_RELAY_CHANNELS) - 1) @@ -196,6 +198,30 @@ uint16_t r_page_rc_input_config[PX4IO_CONTROL_CHANNELS * PX4IO_P_RC_CONFIG_STRI */ uint16_t r_page_servo_failsafe[PX4IO_SERVO_COUNT] = { 0 }; +/** + * PAGE 106 + * + * minimum PWM values when armed + * + */ +uint16_t r_page_servo_control_min[PX4IO_SERVO_COUNT] = { 900, 900, 900, 900, 900, 900, 900, 900 }; + +/** + * PAGE 107 + * + * maximum PWM values when armed + * + */ +uint16_t r_page_servo_control_max[PX4IO_SERVO_COUNT] = { 2100, 2100, 2100, 2100, 2100, 2100, 2100, 2100 }; + +/** + * PAGE 108 + * + * idle PWM values for difficult ESCs + * + */ +uint16_t r_page_servo_idle[PX4IO_SERVO_COUNT] = { 900, 900, 900, 900, 900, 900, 900, 900 }; + int registers_set(uint8_t page, uint8_t offset, const uint16_t *values, unsigned num_values) { @@ -259,6 +285,75 @@ registers_set(uint8_t page, uint8_t offset, const uint16_t *values, unsigned num } break; + case PX4IO_PAGE_CONTROL_MIN_PWM: + + /* copy channel data */ + while ((offset < PX4IO_SERVO_COUNT) && (num_values > 0)) { + + if (*values == 0) + /* set to default */ + r_page_servo_control_min[offset] = 900; + + else if (*values > 1200) + r_page_servo_control_min[offset] = 1200; + else if (*values < 900) + r_page_servo_control_min[offset] = 900; + else + r_page_servo_control_min[offset] = *values; + + offset++; + num_values--; + values++; + } + break; + + case PX4IO_PAGE_CONTROL_MAX_PWM: + + /* copy channel data */ + while ((offset < PX4IO_SERVO_COUNT) && (num_values > 0)) { + + if (*values == 0) + /* set to default */ + r_page_servo_control_max[offset] = 2100; + + else if (*values > 2100) + r_page_servo_control_max[offset] = 2100; + else if (*values < 1800) + r_page_servo_control_max[offset] = 1800; + else + r_page_servo_control_max[offset] = *values; + + offset++; + num_values--; + values++; + } + break; + + case PX4IO_PAGE_IDLE_PWM: + + /* copy channel data */ + while ((offset < PX4IO_SERVO_COUNT) && (num_values > 0)) { + + if (*values == 0) + /* set to default */ + r_page_servo_idle[offset] = 0; + + else if (*values < 900) + r_page_servo_idle[offset] = 900; + else if (*values > 2100) + r_page_servo_idle[offset] = 2100; + else + r_page_servo_idle[offset] = *values; + + /* flag the failsafe values as custom */ + r_setup_arming |= PX4IO_P_SETUP_ARMING_ALWAYS_PWM_ENABLE; + + offset++; + num_values--; + values++; + } + break; + /* handle text going to the mixer parser */ case PX4IO_PAGE_MIXERLOAD: mixer_handle_text(values, num_values * sizeof(*values)); @@ -331,9 +426,11 @@ registers_set_one(uint8_t page, uint8_t offset, uint16_t value) * so that an in-air reset of FMU can not lead to a * lockup of the IO arming state. */ - if ((r_setup_arming & PX4IO_P_SETUP_ARMING_FMU_ARMED) && !(value & PX4IO_P_SETUP_ARMING_FMU_ARMED)) { - r_status_flags &= ~PX4IO_P_STATUS_FLAGS_ARMED; - } + + // XXX do not reset IO's safety state by FMU for now + // if ((r_setup_arming & PX4IO_P_SETUP_ARMING_FMU_ARMED) && !(value & PX4IO_P_SETUP_ARMING_FMU_ARMED)) { + // r_status_flags &= ~PX4IO_P_STATUS_FLAGS_ARMED; + // } r_setup_arming = value; @@ -397,9 +494,8 @@ registers_set_one(uint8_t page, uint8_t offset, uint16_t value) case PX4IO_PAGE_RC_CONFIG: { - /* do not allow a RC config change while fully armed */ - if (/* FMU is armed */ (r_setup_arming & PX4IO_P_SETUP_ARMING_FMU_ARMED) && - /* IO is armed */ (r_status_flags & PX4IO_P_STATUS_FLAGS_ARMED)) { + /* do not allow a RC config change while outputs armed */ + if ((r_status_flags & PX4IO_P_STATUS_FLAGS_OUTPUTS_ARMED)) { break; } @@ -659,6 +755,15 @@ registers_get(uint8_t page, uint8_t offset, uint16_t **values, unsigned *num_val case PX4IO_PAGE_FAILSAFE_PWM: SELECT_PAGE(r_page_servo_failsafe); break; + case PX4IO_PAGE_CONTROL_MIN_PWM: + SELECT_PAGE(r_page_servo_control_min); + break; + case PX4IO_PAGE_CONTROL_MAX_PWM: + SELECT_PAGE(r_page_servo_control_max); + break; + case PX4IO_PAGE_IDLE_PWM: + SELECT_PAGE(r_page_servo_idle); + break; default: return -1; |