diff --git a/libraries/AP_Motors/AP_MotorsHexa.h b/libraries/AP_Motors/AP_MotorsHexa.h index 2b6de1dcde..8afe215606 100644 --- a/libraries/AP_Motors/AP_MotorsHexa.h +++ b/libraries/AP_Motors/AP_MotorsHexa.h @@ -19,7 +19,7 @@ public: AP_MotorsHexa(RC_Channel& rc_roll, RC_Channel& rc_pitch, RC_Channel& rc_throttle, RC_Channel& rc_yaw, uint16_t loop_rate, uint16_t speed_hz = AP_MOTORS_SPEED_DEFAULT) : AP_MotorsMatrix(rc_roll, rc_pitch, rc_throttle, rc_yaw, loop_rate, speed_hz) { }; - // setup_motors - configures the motors for a quad + // setup_motors - configures the motors for a hexa virtual void setup_motors(); protected: diff --git a/libraries/AP_Motors/AP_MotorsOcta.h b/libraries/AP_Motors/AP_MotorsOcta.h index a122947726..d3b7899488 100644 --- a/libraries/AP_Motors/AP_MotorsOcta.h +++ b/libraries/AP_Motors/AP_MotorsOcta.h @@ -19,7 +19,7 @@ public: AP_MotorsOcta(RC_Channel& rc_roll, RC_Channel& rc_pitch, RC_Channel& rc_throttle, RC_Channel& rc_yaw, uint16_t loop_rate, uint16_t speed_hz = AP_MOTORS_SPEED_DEFAULT) : AP_MotorsMatrix(rc_roll, rc_pitch, rc_throttle, rc_yaw, loop_rate, speed_hz) { }; - // setup_motors - configures the motors for a quad + // setup_motors - configures the motors for an octa virtual void setup_motors(); protected: diff --git a/libraries/AP_Motors/AP_MotorsTri.cpp b/libraries/AP_Motors/AP_MotorsTri.cpp index 045d8e264d..036e56f5c2 100644 --- a/libraries/AP_Motors/AP_MotorsTri.cpp +++ b/libraries/AP_Motors/AP_MotorsTri.cpp @@ -77,7 +77,7 @@ void AP_MotorsTri::output_min() hal.rcout->write(pgm_read_byte(&_motor_to_channel_map[AP_MOTORS_MOT_1]), _rc_throttle.radio_min); hal.rcout->write(pgm_read_byte(&_motor_to_channel_map[AP_MOTORS_MOT_2]), _rc_throttle.radio_min); hal.rcout->write(pgm_read_byte(&_motor_to_channel_map[AP_MOTORS_MOT_4]), _rc_throttle.radio_min); - hal.rcout->write(pgm_read_byte(&_motor_to_channel_map[AP_MOTORS_CH_TRI_YAW]), _rc_yaw.radio_trim); + hal.rcout->write(AP_MOTORS_CH_TRI_YAW, _rc_yaw.radio_trim); } // get_motor_mask - returns a bitmask of which outputs are being used for motors or servos (1 means being used) @@ -85,7 +85,10 @@ void AP_MotorsTri::output_min() uint16_t AP_MotorsTri::get_motor_mask() { // tri copter uses channels 1,2,4 and 7 - return (1U << 0 | 1U << 1 | 1U << 3 | 1U << AP_MOTORS_CH_TRI_YAW); + return (1U << pgm_read_byte(&_motor_to_channel_map[AP_MOTORS_MOT_1])) | + (1U << pgm_read_byte(&_motor_to_channel_map[AP_MOTORS_MOT_2])) | + (1U << pgm_read_byte(&_motor_to_channel_map[AP_MOTORS_MOT_4])) | + (1U << AP_MOTORS_CH_TRI_YAW); } // output_armed - sends commands to the motors @@ -226,7 +229,7 @@ void AP_MotorsTri::output_test(uint8_t motor_seq, int16_t pwm) break; case 3: // back servo - hal.rcout->write(pgm_read_byte(&_motor_to_channel_map[AP_MOTORS_MOT_7]), pwm); + hal.rcout->write(AP_MOTORS_CH_TRI_YAW, pwm); break; case 4: // front left motor diff --git a/libraries/AP_Motors/AP_MotorsY6.h b/libraries/AP_Motors/AP_MotorsY6.h index 2f2a8a04b6..792e1ba163 100644 --- a/libraries/AP_Motors/AP_MotorsY6.h +++ b/libraries/AP_Motors/AP_MotorsY6.h @@ -21,7 +21,7 @@ public: AP_MotorsY6(RC_Channel& rc_roll, RC_Channel& rc_pitch, RC_Channel& rc_throttle, RC_Channel& rc_yaw, uint16_t loop_rate, uint16_t speed_hz = AP_MOTORS_SPEED_DEFAULT) : AP_MotorsMatrix(rc_roll, rc_pitch, rc_throttle, rc_yaw, loop_rate, speed_hz) { }; - // setup_motors - configures the motors for a quad + // setup_motors - configures the motors for a Y6 virtual void setup_motors(); protected: