diff options
Diffstat (limited to 'src/modules')
-rw-r--r-- | src/modules/px4iofirmware/mixer.cpp | 11 | ||||
-rw-r--r-- | src/modules/px4iofirmware/registers.c | 18 |
2 files changed, 22 insertions, 7 deletions
diff --git a/src/modules/px4iofirmware/mixer.cpp b/src/modules/px4iofirmware/mixer.cpp index e257e7cb8..62a6ebf13 100644 --- a/src/modules/px4iofirmware/mixer.cpp +++ b/src/modules/px4iofirmware/mixer.cpp @@ -62,7 +62,7 @@ extern "C" { /* * Time that the ESCs need to initialize */ - #define ESC_INIT_TIME_US 500000 + #define ESC_INIT_TIME_US 2000000 /* XXX need to move the RC_CHANNEL_FUNCTION out of rc_channels.h and into systemlib */ #define ROLL 0 @@ -75,6 +75,7 @@ extern "C" { static bool mixer_servos_armed = false; static uint64_t time_armed; +static bool init_complete = false; /* selected control values and count for mixing */ enum mixer_source { @@ -177,6 +178,10 @@ mixer_tick(void) /* mix */ mixed = mixer_group.mix(&outputs[0], IO_SERVO_COUNT); + if (!init_complete && mixer_servos_armed && (hrt_elapsed_time(&time_armed) > ESC_INIT_TIME_US)) { + init_complete = true; + } + /* scale to PWM and update the servo outputs as required */ for (unsigned i = 0; i < mixed; i++) { @@ -184,7 +189,7 @@ mixer_tick(void) r_page_actuators[i] = FLOAT_TO_REG(outputs[i]); /* scale to control range after init time */ - if (mixer_servos_armed && (hrt_elapsed_time(&time_armed) > ESC_INIT_TIME_US)) { + if (init_complete) { r_page_servos[i] = (outputs[i] * (r_page_servo_control_max[i] - r_page_servo_control_min[i])/2 + (r_page_servo_control_max[i] + r_page_servo_control_min[i])/2); @@ -222,11 +227,13 @@ mixer_tick(void) up_pwm_servo_arm(true); mixer_servos_armed = true; time_armed = hrt_absolute_time(); + init_complete = false; } else if (!should_arm && mixer_servos_armed) { /* armed but need to disarm */ up_pwm_servo_arm(false); mixer_servos_armed = false; + init_complete = false; } if (mixer_servos_armed) { diff --git a/src/modules/px4iofirmware/registers.c b/src/modules/px4iofirmware/registers.c index 1fcfb2906..bc1c83901 100644 --- a/src/modules/px4iofirmware/registers.c +++ b/src/modules/px4iofirmware/registers.c @@ -268,7 +268,11 @@ registers_set(uint8_t page, uint8_t offset, const uint16_t *values, unsigned num /* copy channel data */ while ((offset < IO_SERVO_COUNT) && (num_values > 0)) { - if (*values > 1200) + 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; @@ -286,10 +290,14 @@ registers_set(uint8_t page, uint8_t offset, const uint16_t *values, unsigned num /* copy channel data */ while ((offset < IO_SERVO_COUNT) && (num_values > 0)) { - if (*values > 1200) - r_page_servo_control_max[offset] = 1200; - else if (*values < 900) - r_page_servo_control_max[offset] = 900; + 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; |