AP_Motors: added rc_map_mask() function

This commit is contained in:
Andrew Tridgell 2016-01-05 07:35:24 +11:00
parent dfccf8f713
commit 7c9ee9363b
6 changed files with 23 additions and 14 deletions

View File

@ -316,7 +316,7 @@ void AP_MotorsHeli_Single::calculate_roll_pitch_collective_factors()
uint16_t AP_MotorsHeli_Single::get_motor_mask() uint16_t AP_MotorsHeli_Single::get_motor_mask()
{ {
// heli uses channels 1,2,3,4,7 and 8 // heli uses channels 1,2,3,4,7 and 8
return (1U << 0 | 1U << 1 | 1U << 2 | 1U << 3 | 1U << AP_MOTORS_HELI_SINGLE_AUX | 1U << AP_MOTORS_HELI_SINGLE_RSC); return rc_map_mask(1U << 0 | 1U << 1 | 1U << 2 | 1U << 3 | 1U << AP_MOTORS_HELI_SINGLE_AUX | 1U << AP_MOTORS_HELI_SINGLE_RSC);
} }
// update_motor_controls - sends commands to motor controllers // update_motor_controls - sends commands to motor controllers

View File

@ -114,7 +114,7 @@ uint16_t AP_MotorsMatrix::get_motor_mask()
mask |= 1U << i; mask |= 1U << i;
} }
} }
return mask; return rc_map_mask(mask);
} }
void AP_MotorsMatrix::output_armed_not_stabilizing() void AP_MotorsMatrix::output_armed_not_stabilizing()

View File

@ -130,7 +130,7 @@ void AP_MotorsSingle::output_min()
uint16_t AP_MotorsSingle::get_motor_mask() uint16_t AP_MotorsSingle::get_motor_mask()
{ {
// single copter uses channels 1,2,3,4 and 7 // single copter uses channels 1,2,3,4 and 7
return (1U << 0 | 1U << 1 | 1U << 2 | 1U << 3 | 1U << 6); return rc_map_mask(1U << 0 | 1U << 1 | 1U << 2 | 1U << 3 | 1U << 6);
} }
void AP_MotorsSingle::output_armed_not_stabilizing() void AP_MotorsSingle::output_armed_not_stabilizing()

View File

@ -130,7 +130,7 @@ void AP_MotorsTri::output_min()
uint16_t AP_MotorsTri::get_motor_mask() uint16_t AP_MotorsTri::get_motor_mask()
{ {
// tri copter uses channels 1,2,4 and 7 // tri copter uses channels 1,2,4 and 7
return (1U << AP_MOTORS_MOT_1) | return rc_map_mask(1U << AP_MOTORS_MOT_1) |
(1U << AP_MOTORS_MOT_2) | (1U << AP_MOTORS_MOT_2) |
(1U << AP_MOTORS_MOT_4) | (1U << AP_MOTORS_MOT_4) |
(1U << AP_MOTORS_CH_TRI_YAW); (1U << AP_MOTORS_CH_TRI_YAW);

View File

@ -81,6 +81,23 @@ void AP_Motors::rc_write(uint8_t chan, uint16_t pwm)
set frequency of a set of channels set frequency of a set of channels
*/ */
void AP_Motors::rc_set_freq(uint32_t mask, uint16_t freq_hz) void AP_Motors::rc_set_freq(uint32_t mask, uint16_t freq_hz)
{
hal.rcout->set_freq(rc_map_mask(mask), freq_hz);
}
void AP_Motors::rc_enable_ch(uint8_t chan)
{
if (_motor_map_mask & (1U<<chan)) {
// we have a mapped motor number for this channel
chan = _motor_map[chan];
}
hal.rcout->enable_ch(chan);
}
/*
map an internal motor mask to real motor mask
*/
uint32_t AP_Motors::rc_map_mask(uint32_t mask) const
{ {
uint32_t mask2 = 0; uint32_t mask2 = 0;
for (uint8_t i=0; i<32; i++) { for (uint8_t i=0; i<32; i++) {
@ -94,14 +111,5 @@ void AP_Motors::rc_set_freq(uint32_t mask, uint16_t freq_hz)
} }
} }
} }
hal.rcout->set_freq(mask2, freq_hz); return mask2;
}
void AP_Motors::rc_enable_ch(uint8_t chan)
{
if (_motor_map_mask & (1U<<chan)) {
// we have a mapped motor number for this channel
chan = _motor_map[chan];
}
hal.rcout->enable_ch(chan);
} }

View File

@ -132,6 +132,7 @@ protected:
virtual void rc_write(uint8_t chan, uint16_t pwm); virtual void rc_write(uint8_t chan, uint16_t pwm);
virtual void rc_set_freq(uint32_t mask, uint16_t freq_hz); virtual void rc_set_freq(uint32_t mask, uint16_t freq_hz);
virtual void rc_enable_ch(uint8_t chan); virtual void rc_enable_ch(uint8_t chan);
virtual uint32_t rc_map_mask(uint32_t mask) const;
// update the throttle input filter // update the throttle input filter
virtual void update_throttle_filter() = 0; virtual void update_throttle_filter() = 0;