mirror of https://github.com/ArduPilot/ardupilot
AP_Motors: include boost_throttle in get_motor_mask
This commit is contained in:
parent
f85e84cc89
commit
a5c59ce36e
|
@ -103,14 +103,19 @@ void AP_MotorsCoax::output_to_motors()
|
|||
// this can be used to ensure other pwm outputs (i.e. for servos) do not conflict
|
||||
uint16_t AP_MotorsCoax::get_motor_mask()
|
||||
{
|
||||
uint32_t mask =
|
||||
uint32_t motor_mask =
|
||||
1U << AP_MOTORS_MOT_1 |
|
||||
1U << AP_MOTORS_MOT_2 |
|
||||
1U << AP_MOTORS_MOT_3 |
|
||||
1U << AP_MOTORS_MOT_4 |
|
||||
1U << AP_MOTORS_MOT_5 |
|
||||
1U << AP_MOTORS_MOT_6;
|
||||
return rc_map_mask(mask);
|
||||
uint16_t mask = rc_map_mask(motor_mask);
|
||||
|
||||
// add parent's mask
|
||||
mask |= AP_MotorsMulticopter::get_motor_mask();
|
||||
|
||||
return mask;
|
||||
}
|
||||
|
||||
// sends commands to the motors
|
||||
|
|
|
@ -122,13 +122,18 @@ void AP_MotorsMatrix::output_to_motors()
|
|||
// this can be used to ensure other pwm outputs (i.e. for servos) do not conflict
|
||||
uint16_t AP_MotorsMatrix::get_motor_mask()
|
||||
{
|
||||
uint16_t mask = 0;
|
||||
uint16_t motor_mask = 0;
|
||||
for (uint8_t i=0; i<AP_MOTORS_MAX_NUM_MOTORS; i++) {
|
||||
if (motor_enabled[i]) {
|
||||
mask |= 1U << i;
|
||||
motor_mask |= 1U << i;
|
||||
}
|
||||
}
|
||||
return rc_map_mask(mask);
|
||||
uint16_t mask = rc_map_mask(motor_mask);
|
||||
|
||||
// add parent's mask
|
||||
mask |= AP_MotorsMulticopter::get_motor_mask();
|
||||
|
||||
return mask;
|
||||
}
|
||||
|
||||
// output_armed - sends commands to the motors
|
||||
|
|
|
@ -610,6 +610,13 @@ void AP_MotorsMulticopter::output_motor_mask(float thrust, uint8_t mask)
|
|||
}
|
||||
}
|
||||
|
||||
// get_motor_mask - returns a bitmask of which outputs are being used for motors (1 means being used)
|
||||
// this can be used to ensure other pwm outputs (i.e. for servos) do not conflict
|
||||
uint16_t AP_MotorsMulticopter::get_motor_mask()
|
||||
{
|
||||
return SRV_Channels::get_output_channel_mask(SRV_Channel::k_boost_throttle);
|
||||
}
|
||||
|
||||
// save parameters as part of disarming
|
||||
void AP_MotorsMulticopter::save_params_on_disarm()
|
||||
{
|
||||
|
|
|
@ -86,6 +86,10 @@ public:
|
|||
// flight. Thrust is in the range 0 to 1
|
||||
virtual void output_motor_mask(float thrust, uint8_t mask);
|
||||
|
||||
// get_motor_mask - returns a bitmask of which outputs are being used for motors (1 means being used)
|
||||
// this can be used to ensure other pwm outputs (i.e. for servos) do not conflict
|
||||
virtual uint16_t get_motor_mask() override;
|
||||
|
||||
// get minimum or maximum pwm value that can be output to motors
|
||||
int16_t get_pwm_output_min() const;
|
||||
int16_t get_pwm_output_max() const;
|
||||
|
|
|
@ -106,14 +106,20 @@ void AP_MotorsSingle::output_to_motors()
|
|||
// this can be used to ensure other pwm outputs (i.e. for servos) do not conflict
|
||||
uint16_t AP_MotorsSingle::get_motor_mask()
|
||||
{
|
||||
uint32_t mask =
|
||||
uint32_t motor_mask =
|
||||
1U << AP_MOTORS_MOT_1 |
|
||||
1U << AP_MOTORS_MOT_2 |
|
||||
1U << AP_MOTORS_MOT_3 |
|
||||
1U << AP_MOTORS_MOT_4 |
|
||||
1U << AP_MOTORS_MOT_5 |
|
||||
1U << AP_MOTORS_MOT_6;
|
||||
return rc_map_mask(mask);
|
||||
|
||||
uint16_t mask = rc_map_mask(motor_mask);
|
||||
|
||||
// add parent's mask
|
||||
mask |= AP_MotorsMulticopter::get_motor_mask();
|
||||
|
||||
return mask;
|
||||
}
|
||||
|
||||
// sends commands to the motors
|
||||
|
|
|
@ -109,10 +109,16 @@ void AP_MotorsTri::output_to_motors()
|
|||
uint16_t AP_MotorsTri::get_motor_mask()
|
||||
{
|
||||
// tri copter uses channels 1,2,4 and 7
|
||||
return rc_map_mask((1U << AP_MOTORS_MOT_1) |
|
||||
(1U << AP_MOTORS_MOT_2) |
|
||||
(1U << AP_MOTORS_MOT_4) |
|
||||
(1U << AP_MOTORS_CH_TRI_YAW));
|
||||
uint16_t motor_mask = (1U << AP_MOTORS_MOT_1) |
|
||||
(1U << AP_MOTORS_MOT_2) |
|
||||
(1U << AP_MOTORS_MOT_4) |
|
||||
(1U << AP_MOTORS_CH_TRI_YAW);
|
||||
uint16_t mask = rc_map_mask(motor_mask);
|
||||
|
||||
// add parent's mask
|
||||
mask |= AP_MotorsMulticopter::get_motor_mask();
|
||||
|
||||
return mask;
|
||||
}
|
||||
|
||||
// output_armed - sends commands to the motors
|
||||
|
|
Loading…
Reference in New Issue