diff --git a/libraries/AP_Motors/AP_MotorsMatrix.cpp b/libraries/AP_Motors/AP_MotorsMatrix.cpp index 2140b0f4e8..4b4232389e 100644 --- a/libraries/AP_Motors/AP_MotorsMatrix.cpp +++ b/libraries/AP_Motors/AP_MotorsMatrix.cpp @@ -49,7 +49,7 @@ void AP_MotorsMatrix::set_update_rate( uint16_t speed_hz ) uint32_t mask = 0; for( i=0; iset_freq( mask, _speed_hz ); @@ -81,7 +81,7 @@ void AP_MotorsMatrix::enable() // enable output channels for( i=0; ienable_ch(_motor_to_channel_map[i]); + hal.rcout->enable_ch(pgm_read_byte(&_motor_to_channel_map[i])); } } } @@ -100,7 +100,7 @@ void AP_MotorsMatrix::output_min() // fill the motor_out[] array for HIL use and send minimum value to each motor for( i=0; iwrite(_motor_to_channel_map[i], _rc_throttle.radio_min); + hal.rcout->write(pgm_read_byte(&_motor_to_channel_map[i]), _rc_throttle.radio_min); } } } @@ -317,7 +317,7 @@ void AP_MotorsMatrix::output_armed() // send output to each motor for( i=0; iwrite(_motor_to_channel_map[i], motor_out[i]); + hal.rcout->write(pgm_read_byte(&_motor_to_channel_map[i]), motor_out[i]); } } } @@ -356,9 +356,9 @@ void AP_MotorsMatrix::output_test() for( j=0; jwrite(_motor_to_channel_map[j], _rc_throttle.radio_min + _min_throttle); + hal.rcout->write(pgm_read_byte(&_motor_to_channel_map[j]), _rc_throttle.radio_min + _min_throttle); hal.scheduler->delay(300); - hal.rcout->write(_motor_to_channel_map[j], _rc_throttle.radio_min); + hal.rcout->write(pgm_read_byte(&_motor_to_channel_map[j]), _rc_throttle.radio_min); hal.scheduler->delay(2000); } }