diff --git a/libraries/AP_Motors/AP_MotorsHeli.cpp b/libraries/AP_Motors/AP_MotorsHeli.cpp index faf432c669..405069f826 100644 --- a/libraries/AP_Motors/AP_MotorsHeli.cpp +++ b/libraries/AP_Motors/AP_MotorsHeli.cpp @@ -198,10 +198,12 @@ void AP_MotorsHeli::set_update_rate( uint16_t speed_hz ) _speed_hz = speed_hz; // setup fast channels - hal.rcout->set_freq(_motor_to_channel_map[AP_MOTORS_MOT_1], _speed_hz); - hal.rcout->set_freq(_motor_to_channel_map[AP_MOTORS_MOT_2], _speed_hz); - hal.rcout->set_freq(_motor_to_channel_map[AP_MOTORS_MOT_3], _speed_hz); - hal.rcout->set_freq(_motor_to_channel_map[AP_MOTORS_MOT_4], _speed_hz); + uint32_t mask = + 1U << _motor_to_channel_map[AP_MOTORS_MOT_1] | + 1U << _motor_to_channel_map[AP_MOTORS_MOT_2] | + 1U << _motor_to_channel_map[AP_MOTORS_MOT_3] | + 1U << _motor_to_channel_map[AP_MOTORS_MOT_4]; + hal.rcout->set_freq(mask, _speed_hz); } // enable - starts allowing signals to be sent to motors diff --git a/libraries/AP_Motors/AP_MotorsMatrix.cpp b/libraries/AP_Motors/AP_MotorsMatrix.cpp index 726a8d11d0..f7215b651c 100644 --- a/libraries/AP_Motors/AP_MotorsMatrix.cpp +++ b/libraries/AP_Motors/AP_MotorsMatrix.cpp @@ -34,11 +34,13 @@ void AP_MotorsMatrix::set_update_rate( uint16_t speed_hz ) _speed_hz = speed_hz; // check each enabled motor + uint32_t mask = 0; for( i=0; iset_freq( _motor_to_channel_map[i], _speed_hz ); + mask |= 1U << _motor_to_channel_map[i]; } } + hal.rcout->set_freq( mask, _speed_hz ); } // set frame orientation (normally + or X) diff --git a/libraries/AP_Motors/AP_MotorsTri.cpp b/libraries/AP_Motors/AP_MotorsTri.cpp index 7be68e54d1..44f78eb253 100644 --- a/libraries/AP_Motors/AP_MotorsTri.cpp +++ b/libraries/AP_Motors/AP_MotorsTri.cpp @@ -30,9 +30,11 @@ void AP_MotorsTri::set_update_rate( uint16_t speed_hz ) _speed_hz = speed_hz; // set update rate for the 3 motors (but not the servo on channel 7) - hal.rcout->set_freq(_motor_to_channel_map[AP_MOTORS_MOT_1], _speed_hz); - hal.rcout->set_freq(_motor_to_channel_map[AP_MOTORS_MOT_2], _speed_hz); - hal.rcout->set_freq(_motor_to_channel_map[AP_MOTORS_MOT_4], _speed_hz); + uint32_t mask = + 1U << _motor_to_channel_map[AP_MOTORS_MOT_1] | + 1U << _motor_to_channel_map[AP_MOTORS_MOT_2] | + 1U << _motor_to_channel_map[AP_MOTORS_MOT_4]; + hal.rcout->set_freq(mask, _speed_hz); } // enable - starts allowing signals to be sent to motors