Browse Source

Motors: Use _motor_to_channel_map in tricopter consistently.

In AP_MotorsTri.cpp the AP_MOTORS_MOT_1, _2 and _4 constants are
always mapped to actual output channels through _motor_to_channel_map
while the _CH_TRI_YAW is not, but there were a few inconsistencies
in this that could lead to things like PWM min and max values being
set on wrong channels.

It looks like all in all _motor_to_channel_map being in PROGMEM
probably doesn't help save memory and I'm not sure how useful it is
in the first place but regardless the usage should be consistent.
mission-4.1.18
Andrzej Zaborowski 10 years ago committed by Randy Mackay
parent
commit
132cdc4916
  1. 2
      libraries/AP_Motors/AP_MotorsHexa.h
  2. 2
      libraries/AP_Motors/AP_MotorsOcta.h
  3. 9
      libraries/AP_Motors/AP_MotorsTri.cpp
  4. 2
      libraries/AP_Motors/AP_MotorsY6.h

2
libraries/AP_Motors/AP_MotorsHexa.h

@ -19,7 +19,7 @@ public: @@ -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:

2
libraries/AP_Motors/AP_MotorsOcta.h

@ -19,7 +19,7 @@ public: @@ -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:

9
libraries/AP_Motors/AP_MotorsTri.cpp

@ -77,7 +77,7 @@ void AP_MotorsTri::output_min() @@ -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() @@ -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) @@ -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

2
libraries/AP_Motors/AP_MotorsY6.h

@ -21,7 +21,7 @@ public: @@ -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:

Loading…
Cancel
Save