diff --git a/src/drivers/px4fmu/fmu.cpp b/src/drivers/px4fmu/fmu.cpp index 54c955bb05..c757dcc29b 100644 --- a/src/drivers/px4fmu/fmu.cpp +++ b/src/drivers/px4fmu/fmu.cpp @@ -171,9 +171,10 @@ private: uint8_t control_group, uint8_t control_index, float &input); - void subscribe(); + void subscribe(); int set_pwm_rate(unsigned rate_map, unsigned default_rate, unsigned alt_rate); int pwm_ioctl(file *filp, int cmd, unsigned long arg); + void update_pwm_rev_mask(); struct GPIOConfig { uint32_t input; @@ -549,6 +550,26 @@ PX4FMU::subscribe() } } +void +PX4FMU::update_pwm_rev_mask() +{ + _reverse_pwm_mask = 0; + + for (unsigned i = 0; i < _max_actuators; i++) { + char pname[16]; + int32_t ival; + + /* fill the channel reverse mask from parameters */ + sprintf(pname, "PWM_AUX_REV%d", i + 1); + param_t param_h = param_find(pname); + + if (param_h != PARAM_INVALID) { + param_get(param_h, &ival); + _reverse_pwm_mask |= ((int16_t)(ival != 0)) << i; + } + } +} + void PX4FMU::task_main() { @@ -574,7 +595,7 @@ PX4FMU::task_main() /* initialize PWM limit lib */ pwm_limit_init(&_pwm_limit); - log("starting"); + update_pwm_rev_mask(); /* loop until killed */ while (!_task_should_exit) { @@ -731,25 +752,11 @@ PX4FMU::task_main() } orb_check(_param_sub, &updated); - if (updated) { parameter_update_s pupdate; orb_copy(ORB_ID(parameter_update), _param_sub, &pupdate); - _reverse_pwm_mask = 0; - - for (unsigned i = 0; i < _max_actuators; i++) { - char pname[16]; - int32_t ival; - - /* fill the channel reverse mask from parameters */ - sprintf(pname, "PWM_AUX_REV%d", i + 1); - param_t param_h = param_find(pname); - - if (param_h != PARAM_INVALID) { - param_get(param_h, &ival); - _reverse_pwm_mask |= ((int16_t)(ival != 0)) << i; - } - } + + update_pwm_rev_mask(); } #ifdef HRT_PPM_CHANNEL diff --git a/src/drivers/px4fmu/px4fmu_params.c b/src/drivers/px4fmu/px4fmu_params.c index 461d7cb2f8..4377c22e77 100644 --- a/src/drivers/px4fmu/px4fmu_params.c +++ b/src/drivers/px4fmu/px4fmu_params.c @@ -51,7 +51,7 @@ * @max 1 * @group PWM Outputs */ -PARAM_DEFINE_INT32(PWM_AUX_REV1, 1); +PARAM_DEFINE_INT32(PWM_AUX_REV1, 0); /** * Inverter for main output channel 2 @@ -62,7 +62,7 @@ PARAM_DEFINE_INT32(PWM_AUX_REV1, 1); * @max 1 * @group PWM Outputs */ -PARAM_DEFINE_INT32(PWM_AUX_REV2, 1); +PARAM_DEFINE_INT32(PWM_AUX_REV2, 0); /** * Inverter for main output channel 3 @@ -73,7 +73,7 @@ PARAM_DEFINE_INT32(PWM_AUX_REV2, 1); * @max 1 * @group PWM Outputs */ -PARAM_DEFINE_INT32(PWM_AUX_REV3, 1); +PARAM_DEFINE_INT32(PWM_AUX_REV3, 0); /** * Inverter for main output channel 4 @@ -84,7 +84,7 @@ PARAM_DEFINE_INT32(PWM_AUX_REV3, 1); * @max 1 * @group PWM Outputs */ -PARAM_DEFINE_INT32(PWM_AUX_REV4, 1); +PARAM_DEFINE_INT32(PWM_AUX_REV4, 0); /** * Inverter for main output channel 5 @@ -95,7 +95,7 @@ PARAM_DEFINE_INT32(PWM_AUX_REV4, 1); * @max 1 * @group PWM Outputs */ -PARAM_DEFINE_INT32(PWM_AUX_REV5, 1); +PARAM_DEFINE_INT32(PWM_AUX_REV5, 0); /** * Inverter for main output channel 6 @@ -106,4 +106,4 @@ PARAM_DEFINE_INT32(PWM_AUX_REV5, 1); * @max 1 * @group PWM Outputs */ -PARAM_DEFINE_INT32(PWM_AUX_REV6, 1); +PARAM_DEFINE_INT32(PWM_AUX_REV6, 0);