diff --git a/src/drivers/drv_pwm_output.h b/src/drivers/drv_pwm_output.h index cb6f58d666..26fdf1d8a3 100644 --- a/src/drivers/drv_pwm_output.h +++ b/src/drivers/drv_pwm_output.h @@ -111,6 +111,11 @@ __BEGIN_DECLS */ #define PWM_DEFAULT_MAX 2000 +/** + * Default trim PWM in us + */ +#define PWM_DEFAULT_TRIM 0 + /** * Lowest PWM allowed as the maximum PWM */ @@ -217,57 +222,62 @@ struct pwm_output_rc_config { /** get the maximum PWM value the output will send */ #define PWM_SERVO_GET_MAX_PWM _PX4_IOC(_PWM_SERVO_BASE, 19) +/** set the maximum PWM value the output will send */ +#define PWM_SERVO_SET_TRIM_PWM _PX4_IOC(_PWM_SERVO_BASE, 20) + +/** get the maximum PWM value the output will send */ +#define PWM_SERVO_GET_TRIM_PWM _PX4_IOC(_PWM_SERVO_BASE, 21) + /** set the number of servos in (unsigned)arg - allows change of * split between servos and GPIO */ -#define PWM_SERVO_SET_COUNT _PX4_IOC(_PWM_SERVO_BASE, 20) +#define PWM_SERVO_SET_COUNT _PX4_IOC(_PWM_SERVO_BASE, 22) /** set the lockdown override flag to enable outputs in HIL */ -#define PWM_SERVO_SET_DISABLE_LOCKDOWN _PX4_IOC(_PWM_SERVO_BASE, 21) +#define PWM_SERVO_SET_DISABLE_LOCKDOWN _PX4_IOC(_PWM_SERVO_BASE, 23) /** get the lockdown override flag to enable outputs in HIL */ -#define PWM_SERVO_GET_DISABLE_LOCKDOWN _PX4_IOC(_PWM_SERVO_BASE, 22) +#define PWM_SERVO_GET_DISABLE_LOCKDOWN _PX4_IOC(_PWM_SERVO_BASE, 24) /** force safety switch off (to disable use of safety switch) */ -#define PWM_SERVO_SET_FORCE_SAFETY_OFF _PX4_IOC(_PWM_SERVO_BASE, 23) +#define PWM_SERVO_SET_FORCE_SAFETY_OFF _PX4_IOC(_PWM_SERVO_BASE, 25) /** force failsafe mode (failsafe values are set immediately even if failsafe condition not met) */ -#define PWM_SERVO_SET_FORCE_FAILSAFE _PX4_IOC(_PWM_SERVO_BASE, 24) +#define PWM_SERVO_SET_FORCE_FAILSAFE _PX4_IOC(_PWM_SERVO_BASE, 26) /** make failsafe non-recoverable (termination) if it occurs */ -#define PWM_SERVO_SET_TERMINATION_FAILSAFE _PX4_IOC(_PWM_SERVO_BASE, 25) +#define PWM_SERVO_SET_TERMINATION_FAILSAFE _PX4_IOC(_PWM_SERVO_BASE, 27) /** force safety switch on (to enable use of safety switch) */ -#define PWM_SERVO_SET_FORCE_SAFETY_ON _PX4_IOC(_PWM_SERVO_BASE, 26) +#define PWM_SERVO_SET_FORCE_SAFETY_ON _PX4_IOC(_PWM_SERVO_BASE, 28) /** set RC config for a channel. This takes a pointer to pwm_output_rc_config */ -#define PWM_SERVO_SET_RC_CONFIG _PX4_IOC(_PWM_SERVO_BASE, 27) +#define PWM_SERVO_SET_RC_CONFIG _PX4_IOC(_PWM_SERVO_BASE, 29) /** set the 'OVERRIDE OK' bit, which allows for RC control on FMU loss */ -#define PWM_SERVO_SET_OVERRIDE_OK _PX4_IOC(_PWM_SERVO_BASE, 28) +#define PWM_SERVO_SET_OVERRIDE_OK _PX4_IOC(_PWM_SERVO_BASE, 30) /** clear the 'OVERRIDE OK' bit, which allows for RC control on FMU loss */ -#define PWM_SERVO_CLEAR_OVERRIDE_OK _PX4_IOC(_PWM_SERVO_BASE, 29) +#define PWM_SERVO_CLEAR_OVERRIDE_OK _PX4_IOC(_PWM_SERVO_BASE, 31) /** setup OVERRIDE_IMMEDIATE behaviour on FMU fail */ -#define PWM_SERVO_SET_OVERRIDE_IMMEDIATE _PX4_IOC(_PWM_SERVO_BASE, 30) +#define PWM_SERVO_SET_OVERRIDE_IMMEDIATE _PX4_IOC(_PWM_SERVO_BASE, 32) /** set SBUS output frame rate in Hz */ -#define PWM_SERVO_SET_SBUS_RATE _PX4_IOC(_PWM_SERVO_BASE, 31) +#define PWM_SERVO_SET_SBUS_RATE _PX4_IOC(_PWM_SERVO_BASE, 33) /** set auxillary output mode. These correspond to enum Mode in px4fmu/fmu.cpp */ #define PWM_SERVO_MODE_NONE 0 -#define PWM_SERVO_MODE_1PWM 1 -#define PWM_SERVO_MODE_2PWM 2 -#define PWM_SERVO_MODE_2PWM2CAP 3 -#define PWM_SERVO_MODE_3PWM 4 -#define PWM_SERVO_MODE_3PWM1CAP 5 -#define PWM_SERVO_MODE_4PWM 6 -#define PWM_SERVO_MODE_6PWM 7 -#define PWM_SERVO_MODE_8PWM 8 -#define PWM_SERVO_MODE_4CAP 9 -#define PWM_SERVO_MODE_5CAP 10 -#define PWM_SERVO_MODE_6CAP 11 -#define PWM_SERVO_SET_MODE _PX4_IOC(_PWM_SERVO_BASE, 32) +#define PWM_SERVO_MODE_2PWM 1 +#define PWM_SERVO_MODE_2PWM2CAP 2 +#define PWM_SERVO_MODE_3PWM 3 +#define PWM_SERVO_MODE_3PWM1CAP 4 +#define PWM_SERVO_MODE_4PWM 5 +#define PWM_SERVO_MODE_6PWM 6 +#define PWM_SERVO_MODE_8PWM 7 +#define PWM_SERVO_MODE_4CAP 8 +#define PWM_SERVO_MODE_5CAP 9 +#define PWM_SERVO_MODE_6CAP 10 +#define PWM_SERVO_SET_MODE _PX4_IOC(_PWM_SERVO_BASE, 34) /* * diff --git a/src/drivers/pwm_out_rc_in/pwm_out_rc_in.cpp b/src/drivers/pwm_out_rc_in/pwm_out_rc_in.cpp index b9fa461aaf..b0e48bc451 100644 --- a/src/drivers/pwm_out_rc_in/pwm_out_rc_in.cpp +++ b/src/drivers/pwm_out_rc_in/pwm_out_rc_in.cpp @@ -426,18 +426,20 @@ void task_main(int argc, char *argv[]) uint16_t disarmed_pwm[4]; uint16_t min_pwm[4]; uint16_t max_pwm[4]; + uint16_t trim_pwm[4]; for (unsigned int i = 0; i < 4; i++) { disarmed_pwm[i] = _pwm_disarmed; min_pwm[i] = _pwm_min; max_pwm[i] = _pwm_max; + trim_pwm[i] = 0; } uint16_t pwm[4]; // TODO FIXME: pre-armed seems broken pwm_limit_calc(_armed.armed, false/*_armed.prearmed*/, _outputs.noutputs, reverse_mask, - disarmed_pwm, min_pwm, max_pwm, _outputs.output, pwm, &_pwm_limit); + disarmed_pwm, min_pwm, max_pwm, trim_pwm, _outputs.output, pwm, &_pwm_limit); send_outputs_mavlink(pwm, 4); diff --git a/src/drivers/px4fmu/fmu.cpp b/src/drivers/px4fmu/fmu.cpp index b513a7d413..be48dc61b7 100644 --- a/src/drivers/px4fmu/fmu.cpp +++ b/src/drivers/px4fmu/fmu.cpp @@ -220,6 +220,7 @@ private: uint16_t _disarmed_pwm[_max_actuators]; uint16_t _min_pwm[_max_actuators]; uint16_t _max_pwm[_max_actuators]; + uint16_t _trim_pwm[_max_actuators]; uint16_t _reverse_pwm_mask; unsigned _num_failsafe_set; unsigned _num_disarmed_set; @@ -342,6 +343,7 @@ PX4FMU::PX4FMU() : for (unsigned i = 0; i < _max_actuators; i++) { _min_pwm[i] = PWM_DEFAULT_MIN; _max_pwm[i] = PWM_DEFAULT_MAX; + _trim_pwm[i] = PWM_DEFAULT_TRIM; } _control_topics[0] = ORB_ID(actuator_controls_0); @@ -1115,7 +1117,7 @@ PX4FMU::cycle() /* the PWM limit call takes care of out of band errors, NaN and constrains */ pwm_limit_calc(_throttle_armed, arm_nothrottle(), num_outputs, _reverse_pwm_mask, - _disarmed_pwm, _min_pwm, _max_pwm, outputs, pwm_limited, &_pwm_limit); + _trim_pwm, _disarmed_pwm, _min_pwm, _max_pwm, outputs, pwm_limited, &_pwm_limit); /* overwrite outputs in case of lockdown with disarmed PWM values */ @@ -1881,6 +1883,46 @@ PX4FMU::pwm_ioctl(file *filp, int cmd, unsigned long arg) break; } + case PWM_SERVO_SET_TRIM_PWM: { + struct pwm_output_values *pwm = (struct pwm_output_values *)arg; + + /* discard if too many values are sent */ + if (pwm->channel_count > _max_actuators) { + ret = -EINVAL; + break; + } + + for (unsigned i = 0; i < pwm->channel_count; i++) { + if (pwm->values[i] == 0) { + /* allow 0 - turns the trim option off */ + _trim_pwm[i] = 0; + + } else if (pwm->values[i] < PWM_LOWEST_MAX) { + _trim_pwm[i] = PWM_LOWEST_MAX; + + } else if (pwm->values[i] > PWM_HIGHEST_MAX) { + _trim_pwm[i] = PWM_HIGHEST_MAX; + + } else { + _trim_pwm[i] = pwm->values[i]; + } + } + + break; + } + + case PWM_SERVO_GET_TRIM_PWM: { + struct pwm_output_values *pwm = (struct pwm_output_values *)arg; + + for (unsigned i = 0; i < _max_actuators; i++) { + pwm->values[i] = _trim_pwm[i]; + } + + pwm->channel_count = _max_actuators; + arg = (unsigned long)&pwm; + break; + } + #if defined(BOARD_HAS_PWM) && BOARD_HAS_PWM >= 8 case PWM_SERVO_SET(7): diff --git a/src/drivers/px4io/px4io.cpp b/src/drivers/px4io/px4io.cpp index 34dde3d240..f937bc5a2b 100644 --- a/src/drivers/px4io/px4io.cpp +++ b/src/drivers/px4io/px4io.cpp @@ -2688,6 +2688,30 @@ PX4IO::ioctl(file *filep, int cmd, unsigned long arg) break; + case PWM_SERVO_SET_TRIM_PWM: { + struct pwm_output_values *pwm = (struct pwm_output_values *)arg; + + if (pwm->channel_count > _max_actuators) + /* fail with error */ + { + return -E2BIG; + } + + /* copy values to registers in IO */ + ret = io_reg_set(PX4IO_PAGE_CONTROL_TRIM_PWM, 0, pwm->values, pwm->channel_count); + break; + } + + case PWM_SERVO_GET_TRIM_PWM: + + ret = io_reg_get(PX4IO_PAGE_CONTROL_TRIM_PWM, 0, (uint16_t *)arg, _max_actuators); + + if (ret != OK) { + ret = -EIO; + } + + break; + case PWM_SERVO_GET_COUNT: *(unsigned *)arg = _max_actuators; break; diff --git a/src/modules/px4iofirmware/mixer.cpp b/src/modules/px4iofirmware/mixer.cpp index 315d7cf138..52a05030a2 100644 --- a/src/modules/px4iofirmware/mixer.cpp +++ b/src/modules/px4iofirmware/mixer.cpp @@ -249,7 +249,7 @@ mixer_tick(void) /* the pwm limit call takes care of out of band errors */ pwm_limit_calc(should_arm, should_arm_nothrottle, mixed, r_setup_pwm_reverse, r_page_servo_disarmed, - r_page_servo_control_min, r_page_servo_control_max, outputs, r_page_servos, &pwm_limit); + r_page_servo_control_min, r_page_servo_control_max, r_page_servo_control_trim, outputs, r_page_servos, &pwm_limit); /* clamp unused outputs to zero */ for (unsigned i = mixed; i < PX4IO_SERVO_COUNT; i++) { diff --git a/src/modules/px4iofirmware/protocol.h b/src/modules/px4iofirmware/protocol.h index f95313e14a..02f57f035c 100644 --- a/src/modules/px4iofirmware/protocol.h +++ b/src/modules/px4iofirmware/protocol.h @@ -298,8 +298,11 @@ enum { /* DSM bind states */ /* PWM maximum values for certain ESCs */ #define PX4IO_PAGE_CONTROL_MAX_PWM 107 /**< 0..CONFIG_ACTUATOR_COUNT-1 */ +/* PWM mtrim values for central position */ +#define PX4IO_PAGE_CONTROL_TRIM_PWM 108 /**< 0..CONFIG_ACTUATOR_COUNT-1 */ + /* PWM disarmed values that are active, even when SAFETY_SAFE */ -#define PX4IO_PAGE_DISARMED_PWM 108 /* 0..CONFIG_ACTUATOR_COUNT-1 */ +#define PX4IO_PAGE_DISARMED_PWM 109 /* 0..CONFIG_ACTUATOR_COUNT-1 */ /** * As-needed mixer data upload. diff --git a/src/modules/px4iofirmware/px4io.h b/src/modules/px4iofirmware/px4io.h index 6b3781495d..1daa9eaa66 100644 --- a/src/modules/px4iofirmware/px4io.h +++ b/src/modules/px4iofirmware/px4io.h @@ -90,6 +90,7 @@ extern uint16_t r_page_rc_input_config[]; /* PX4IO_PAGE_RC_INPUT_CONFIG */ extern uint16_t r_page_servo_failsafe[]; /* PX4IO_PAGE_FAILSAFE_PWM */ extern uint16_t r_page_servo_control_min[]; /* PX4IO_PAGE_CONTROL_MIN_PWM */ extern uint16_t r_page_servo_control_max[]; /* PX4IO_PAGE_CONTROL_MAX_PWM */ +extern uint16_t r_page_servo_control_trim[]; /* PX4IO_PAGE_CONTROL_TRIM_PWM */ extern uint16_t r_page_servo_disarmed[]; /* PX4IO_PAGE_DISARMED_PWM */ /* diff --git a/src/modules/px4iofirmware/registers.c b/src/modules/px4iofirmware/registers.c index 119f2c2141..8b875f731f 100644 --- a/src/modules/px4iofirmware/registers.c +++ b/src/modules/px4iofirmware/registers.c @@ -260,6 +260,14 @@ uint16_t r_page_servo_control_max[PX4IO_SERVO_COUNT] = { PWM_DEFAULT_MAX, PWM_D /** * PAGE 108 * + * trim PWM values for center position + * + */ +uint16_t r_page_servo_control_trim[PX4IO_SERVO_COUNT] = { PWM_DEFAULT_TRIM, PWM_DEFAULT_TRIM, PWM_DEFAULT_TRIM, PWM_DEFAULT_TRIM, PWM_DEFAULT_TRIM, PWM_DEFAULT_TRIM, PWM_DEFAULT_TRIM, PWM_DEFAULT_TRIM }; + +/** + * PAGE 109 + * * disarmed PWM values for difficult ESCs * */ @@ -386,6 +394,32 @@ registers_set(uint8_t page, uint8_t offset, const uint16_t *values, unsigned num break; + case PX4IO_PAGE_CONTROL_TRIM_PWM: + + /* copy channel data */ + while ((offset < PX4IO_SERVO_COUNT) && (num_values > 0)) { + + if (*values == 0) { + /* allow 0 - turns the trim option off */ + r_page_servo_control_trim[offset] = 0; + + } else if (*values > PWM_HIGHEST_MAX) { + r_page_servo_control_trim[offset] = PWM_HIGHEST_MAX; + + } else if (*values < PWM_LOWEST_MAX) { + r_page_servo_control_trim[offset] = PWM_LOWEST_MAX; + + } else { + r_page_servo_control_trim[offset] = *values; + } + + offset++; + num_values--; + values++; + } + + break; + case PX4IO_PAGE_DISARMED_PWM: { /* flag for all outputs */ bool all_disarmed_off = true; @@ -1000,6 +1034,10 @@ registers_get(uint8_t page, uint8_t offset, uint16_t **values, unsigned *num_val SELECT_PAGE(r_page_servo_control_max); break; + case PX4IO_PAGE_CONTROL_TRIM_PWM: + SELECT_PAGE(r_page_servo_control_trim); + break; + case PX4IO_PAGE_DISARMED_PWM: SELECT_PAGE(r_page_servo_disarmed); break; diff --git a/src/modules/systemlib/pwm_limit/pwm_limit.c b/src/modules/systemlib/pwm_limit/pwm_limit.c index e64b8d635d..f4d7087263 100644 --- a/src/modules/systemlib/pwm_limit/pwm_limit.c +++ b/src/modules/systemlib/pwm_limit/pwm_limit.c @@ -56,7 +56,7 @@ void pwm_limit_init(pwm_limit_t *limit) } void pwm_limit_calc(const bool armed, const bool pre_armed, const unsigned num_channels, const uint16_t reverse_mask, - const uint16_t *disarmed_pwm, const uint16_t *min_pwm, const uint16_t *max_pwm, + const uint16_t *disarmed_pwm, const uint16_t *min_pwm, const uint16_t *max_pwm, const uint16_t *trim_pwm, const float *output, uint16_t *effective_pwm, pwm_limit_t *limit) { @@ -206,7 +206,15 @@ void pwm_limit_calc(const bool armed, const bool pre_armed, const unsigned num_c control_value = -1.0f * control_value; } - effective_pwm[i] = control_value * (max_pwm[i] - min_pwm[i]) / 2 + (max_pwm[i] + min_pwm[i]) / 2; + if (trim_pwm[i] == 0) { + effective_pwm[i] = control_value * (max_pwm[i] - min_pwm[i]) / 2 + (max_pwm[i] + min_pwm[i]) / 2; + + } else if (control_value < 0) { + effective_pwm[i] = control_value * (trim_pwm[i] - min_pwm[i]) + trim_pwm[i]; + + } else { + effective_pwm[i] = control_value * (max_pwm[i] - trim_pwm[i]) + trim_pwm[i]; + } /* last line of defense against invalid inputs */ if (effective_pwm[i] < min_pwm[i]) { diff --git a/src/modules/systemlib/pwm_limit/pwm_limit.h b/src/modules/systemlib/pwm_limit/pwm_limit.h index 644059e5a4..fd6629fa43 100644 --- a/src/modules/systemlib/pwm_limit/pwm_limit.h +++ b/src/modules/systemlib/pwm_limit/pwm_limit.h @@ -73,7 +73,8 @@ __EXPORT void pwm_limit_init(pwm_limit_t *limit); __EXPORT void pwm_limit_calc(const bool armed, const bool pre_armed, const unsigned num_channels, const uint16_t reverse_mask, const uint16_t *disarmed_pwm, - const uint16_t *min_pwm, const uint16_t *max_pwm, const float *output, uint16_t *effective_pwm, pwm_limit_t *limit); + const uint16_t *min_pwm, const uint16_t *max_pwm, const uint16_t *trim_pwm, + const float *output, uint16_t *effective_pwm, pwm_limit_t *limit); __END_DECLS diff --git a/src/systemcmds/pwm/pwm.c b/src/systemcmds/pwm/pwm.c index ac6b8bbd51..abd7785f1f 100644 --- a/src/systemcmds/pwm/pwm.c +++ b/src/systemcmds/pwm/pwm.c @@ -90,6 +90,7 @@ usage(const char *reason) "disarmed ...\t\t\tDisarmed PWM\n" "min ...\t\t\t\tMinimum PWM\n" "max ...\t\t\t\tMaximum PWM\n" + "trim ...\t\t\tTrim PWM\n" "\t[-c ]\t\t(e.g. 1234)\n" "\t[-m ]\t(e.g. 0xF)\n" "\t[-a]\t\t\tConfigure all outputs\n" @@ -448,6 +449,49 @@ pwm_main(int argc, char *argv[]) exit(0); + } else if (!strcmp(argv[1], "trim")) { + + if (set_mask == 0) { + usage("no channels set"); + } + + struct pwm_output_values pwm_values; + + memset(&pwm_values, 0, sizeof(pwm_values)); + + pwm_values.channel_count = servo_count; + + /* first get current state before modifying it */ + ret = ioctl(fd, PWM_SERVO_GET_TRIM_PWM, (long unsigned int)&pwm_values); + + if (ret != OK) { + errx(ret, "failed get trim values"); + } + + for (unsigned i = 0; i < servo_count; i++) { + if (set_mask & 1 << i) { + pwm_values.values[i] = pwm_value; + + if (print_verbose) { + warnx("Channel %d: trim PWM: %d", i + 1, pwm_value); + } + } + } + + if (pwm_values.channel_count == 0) { + usage("no PWM values added"); + + } else { + + ret = ioctl(fd, PWM_SERVO_SET_TRIM_PWM, (long unsigned int)&pwm_values); + + if (ret != OK) { + errx(ret, "failed setting trim values"); + } + } + + exit(0); + } else if (!strcmp(argv[1], "disarmed")) { if (set_mask == 0) { @@ -768,6 +812,8 @@ pwm_main(int argc, char *argv[]) struct pwm_output_values max_pwm; + struct pwm_output_values trim_pwm; + ret = ioctl(fd, PWM_SERVO_GET_FAILSAFE_PWM, (unsigned long)&failsafe_pwm); if (ret != OK) { @@ -792,6 +838,12 @@ pwm_main(int argc, char *argv[]) err(1, "PWM_SERVO_GET_MAX_PWM"); } + ret = ioctl(fd, PWM_SERVO_GET_TRIM_PWM, (unsigned long)&trim_pwm); + + if (ret != OK) { + err(1, "PWM_SERVO_GET_TRIM_PWM"); + } + /* print current servo values */ for (unsigned i = 0; i < servo_count; i++) { servo_position_t spos; @@ -809,8 +861,8 @@ pwm_main(int argc, char *argv[]) } - printf(" failsafe: %d, disarmed: %d us, min: %d us, max: %d us)", - failsafe_pwm.values[i], disarmed_pwm.values[i], min_pwm.values[i], max_pwm.values[i]); + printf(" failsafe: %d, disarmed: %d us, min: %d us, max: %d us, trim: %d us)", + failsafe_pwm.values[i], disarmed_pwm.values[i], min_pwm.values[i], max_pwm.values[i], trim_pwm.values[i]); printf("\n"); } else { diff --git a/src/systemcmds/tests/test_mixer.cpp b/src/systemcmds/tests/test_mixer.cpp index 40436c9bd3..f04d56fafc 100644 --- a/src/systemcmds/tests/test_mixer.cpp +++ b/src/systemcmds/tests/test_mixer.cpp @@ -87,6 +87,7 @@ int test_mixer(int argc, char *argv[]) uint16_t r_page_servo_disarmed[output_max]; uint16_t r_page_servo_control_min[output_max]; uint16_t r_page_servo_control_max[output_max]; + uint16_t r_page_servo_control_trim[output_max]; uint16_t r_page_servos[output_max]; uint16_t servo_predicted[output_max]; int16_t reverse_pwm_mask = 0; @@ -200,6 +201,7 @@ int test_mixer(int argc, char *argv[]) r_page_servo_disarmed[i] = PWM_MOTOR_OFF; r_page_servo_control_min[i] = PWM_DEFAULT_MIN; r_page_servo_control_max[i] = PWM_DEFAULT_MAX; + r_page_servo_control_trim[i] = PWM_DEFAULT_TRIM; } //PX4_INFO("PRE-ARM TEST: DISABLING SAFETY"); @@ -209,7 +211,7 @@ int test_mixer(int argc, char *argv[]) mixed = mixer_group.mix(&outputs[0], output_max, NULL); pwm_limit_calc(should_arm, should_prearm, mixed, reverse_pwm_mask, r_page_servo_disarmed, r_page_servo_control_min, - r_page_servo_control_max, outputs, r_page_servos, &pwm_limit); + r_page_servo_control_max, r_page_servo_control_trim, outputs, r_page_servos, &pwm_limit); //warnx("mixed %d outputs (max %d), values:", mixed, output_max); for (unsigned i = 0; i < mixed; i++) { @@ -250,7 +252,7 @@ int test_mixer(int argc, char *argv[]) mixed = mixer_group.mix(&outputs[0], output_max, NULL); pwm_limit_calc(should_arm, should_prearm, mixed, reverse_pwm_mask, r_page_servo_disarmed, r_page_servo_control_min, - r_page_servo_control_max, outputs, r_page_servos, &pwm_limit); + r_page_servo_control_max, r_page_servo_control_trim, outputs, r_page_servos, &pwm_limit); //warnx("mixed %d outputs (max %d), values:", mixed, output_max); for (unsigned i = 0; i < mixed; i++) { @@ -288,14 +290,14 @@ int test_mixer(int argc, char *argv[]) r_page_servo_disarmed[i] = PWM_LOWEST_MIN; r_page_servo_control_min[i] = PWM_DEFAULT_MIN; r_page_servo_control_max[i] = PWM_DEFAULT_MAX; + r_page_servo_control_trim[i] = PWM_DEFAULT_TRIM; } /* mix */ mixed = mixer_group.mix(&outputs[0], output_max, NULL); pwm_limit_calc(should_arm, should_prearm, mixed, reverse_pwm_mask, r_page_servo_disarmed, r_page_servo_control_min, - r_page_servo_control_max, outputs, - r_page_servos, &pwm_limit); + r_page_servo_control_max, r_page_servo_control_trim, outputs, r_page_servos, &pwm_limit); //fprintf(stderr, "mixed %d outputs (max %d)", mixed, output_max); @@ -323,8 +325,7 @@ int test_mixer(int argc, char *argv[]) mixed = mixer_group.mix(&outputs[0], output_max, NULL); pwm_limit_calc(should_arm, should_prearm, mixed, reverse_pwm_mask, r_page_servo_disarmed, r_page_servo_control_min, - r_page_servo_control_max, outputs, - r_page_servos, &pwm_limit); + r_page_servo_control_max, r_page_servo_control_trim, outputs, r_page_servos, &pwm_limit); //warnx("mixed %d outputs (max %d), values:", mixed, output_max); for (unsigned i = 0; i < mixed; i++) { @@ -361,8 +362,7 @@ int test_mixer(int argc, char *argv[]) mixed = mixer_group.mix(&outputs[0], output_max, NULL); pwm_limit_calc(should_arm, should_prearm, mixed, reverse_pwm_mask, r_page_servo_disarmed, r_page_servo_control_min, - r_page_servo_control_max, outputs, - r_page_servos, &pwm_limit); + r_page_servo_control_max, r_page_servo_control_trim, outputs, r_page_servos, &pwm_limit); //warnx("mixed %d outputs (max %d), values:", mixed, output_max); for (unsigned i = 0; i < mixed; i++) {