MotorsSingle: implement get_motor_mask

This commit is contained in:
Randy Mackay 2014-07-26 16:28:10 +09:00
parent 6ae855c1cf
commit 6d99524e56
2 changed files with 12 additions and 0 deletions

View File

@ -151,6 +151,14 @@ void AP_MotorsSingle::output_min()
hal.rcout->write(pgm_read_byte(&_motor_to_channel_map[AP_MOTORS_MOT_7]), _rc_throttle.radio_min); hal.rcout->write(pgm_read_byte(&_motor_to_channel_map[AP_MOTORS_MOT_7]), _rc_throttle.radio_min);
} }
// get_motor_mask - returns a bitmask of which outputs are being used for motors or servos (1 means being used)
// this can be used to ensure other pwm outputs (i.e. for servos) do not conflict
uint16_t AP_MotorsSingle::get_motor_mask()
{
// single copter uses channels 1,2,3,4 and 7
return (1U << 0 | 1U << 1 | 1U << 2 | 1U << 3 | 1U << 6);
}
// output_armed - sends commands to the motors // output_armed - sends commands to the motors
void AP_MotorsSingle::output_armed() void AP_MotorsSingle::output_armed()
{ {

View File

@ -52,6 +52,10 @@ public:
// output_min - sends minimum values out to the motors // output_min - sends minimum values out to the motors
virtual void output_min(); virtual void output_min();
// get_motor_mask - returns a bitmask of which outputs are being used for motors or servos (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();
// var_info for holding Parameter information // var_info for holding Parameter information
static const struct AP_Param::GroupInfo var_info[]; static const struct AP_Param::GroupInfo var_info[];