aepko/Ardupilot2
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

AP_Motors: Heli: get output mask from swashplate lib

Browse Source
This commit is contained in:
Iampete1 2023-05-05 01:04:41 +01:00 committed by Randy Mackay
parent b61b761141
commit d32d1dfec8
4 changed files with 18 additions and 18 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

11
libraries/AP_Motors/AP_MotorsHeli_Dual.cpp
View File

@ -215,16 +215,7 @@ void AP_MotorsHeli_Dual::set_update_rate( uint16_t speed_hz )
_speed_hz = speed_hz;
// setup fast channels
uint32_t mask = 0;
for (uint8_t i=0; i<AP_MOTORS_HELI_DUAL_NUM_SWASHPLATE_SERVOS; i++) {
mask |= 1U << (AP_MOTORS_MOT_1+i);
}
if (_swashplate1.get_swash_type() == SWASHPLATE_TYPE_H4_90 || _swashplate1.get_swash_type() == SWASHPLATE_TYPE_H4_45) {
mask |= 1U << (AP_MOTORS_MOT_7);
}
if (_swashplate2.get_swash_type() == SWASHPLATE_TYPE_H4_90 || _swashplate2.get_swash_type() == SWASHPLATE_TYPE_H4_45) {
mask |= 1U << (AP_MOTORS_MOT_8);
}
uint32_t mask = _swashplate1.get_output_mask() | _swashplate2.get_output_mask();
rc_set_freq(mask, _speed_hz);
}

10
libraries/AP_Motors/AP_MotorsHeli_Single.cpp
View File

@ -188,14 +188,8 @@ void AP_MotorsHeli_Single::set_update_rate( uint16_t speed_hz )
_speed_hz = speed_hz;
// setup fast channels
uint32_t mask =
1U << AP_MOTORS_MOT_1 |
1U << AP_MOTORS_MOT_2 |
1U << AP_MOTORS_MOT_3 |
1U << AP_MOTORS_MOT_4;
if (_swashplate.get_swash_type() == SWASHPLATE_TYPE_H4_90 || _swashplate.get_swash_type() == SWASHPLATE_TYPE_H4_45) {
mask |= 1U << (AP_MOTORS_MOT_5);
}
uint32_t mask = (1U << AP_MOTORS_MOT_4) | _swashplate.get_output_mask();
rc_set_freq(mask, _speed_hz);
}

12
libraries/AP_Motors/AP_MotorsHeli_Swash.cpp
View File

@ -261,3 +261,15 @@ void AP_MotorsHeli_Swash::rc_write(uint8_t chan, float swash_in)
SRV_Channel::Aux_servo_function_t function = SRV_Channels::get_motor_function(chan);
SRV_Channels::set_output_pwm_trimmed(function, pwm);
}
// Get function output mask
uint32_t AP_MotorsHeli_Swash::get_output_mask() const
{
uint32_t mask = 0;
for (uint8_t i = 0; i < _max_num_servos; i++) {
if (_enabled[i]) {
mask |= 1U < _motor_num[i];
}
}
return mask;
}

3
libraries/AP_Motors/AP_MotorsHeli_Swash.h
View File

@ -36,6 +36,9 @@ public:
// get_phase_angle - returns the rotor phase angle
int16_t get_phase_angle() const { return _phase_angle; }
// Get function output mask
uint32_t get_output_mask() const;
// var_info
static const struct AP_Param::GroupInfo var_info[];