mirror of
https://github.com/ArduPilot/ardupilot
synced 2025-01-03 06:28:27 -04:00
AP_Motors: Tradheli - add swashplate library
This commit is contained in:
bnsgeyer
Randy Mackay
parent
ca81cd0f1b
commit
d7e6298366
@ -182,9 +182,6 @@ protected:
|
||||
// calculate_scalars - must be implemented by child classes
|
||||
virtual void calculate_scalars() = 0;
|
||||
|
||||
// calculate_roll_pitch_collective_factors - calculate factors based on swash type and servo position
|
||||
virtual void calculate_roll_pitch_collective_factors() = 0;
|
||||
|
||||
// servo_test - move servos through full range of movement
|
||||
// to be overloaded by child classes, different vehicle types would have different movement patterns
|
||||
virtual void servo_test() = 0;
|
||||
|
||||
@ -23,77 +23,9 @@ extern const AP_HAL::HAL& hal;
|
||||
const AP_Param::GroupInfo AP_MotorsHeli_Dual::var_info[] = {
|
||||
AP_NESTEDGROUPINFO(AP_MotorsHeli, 0),
|
||||
|
||||
// @Param: SV1_POS
|
||||
// @DisplayName: Servo 1 Position
|
||||
// @Description: Angular location of swash servo #1
|
||||
// @Range: -180 180
|
||||
// @Units: deg
|
||||
// @User: Standard
|
||||
// @Increment: 1
|
||||
AP_GROUPINFO("SV1_POS", 1, AP_MotorsHeli_Dual, _servo1_pos, AP_MOTORS_HELI_DUAL_SERVO1_POS),
|
||||
// Indices 1-6 were used by servo position params and should not be used
|
||||
|
||||
// @Param: SV2_POS
|
||||
// @DisplayName: Servo 2 Position
|
||||
// @Description: Angular location of swash servo #2
|
||||
// @Range: -180 180
|
||||
// @Units: deg
|
||||
// @User: Standard
|
||||
// @Increment: 1
|
||||
AP_GROUPINFO("SV2_POS", 2, AP_MotorsHeli_Dual, _servo2_pos, AP_MOTORS_HELI_DUAL_SERVO2_POS),
|
||||
|
||||
// @Param: SV3_POS
|
||||
// @DisplayName: Servo 3 Position
|
||||
// @Description: Angular location of swash servo #3
|
||||
// @Range: -180 180
|
||||
// @Units: deg
|
||||
// @User: Standard
|
||||
// @Increment: 1
|
||||
AP_GROUPINFO("SV3_POS", 3, AP_MotorsHeli_Dual, _servo3_pos, AP_MOTORS_HELI_DUAL_SERVO3_POS),
|
||||
|
||||
// @Param: SV4_POS
|
||||
// @DisplayName: Servo 4 Position
|
||||
// @Description: Angular location of swash servo #4
|
||||
// @Range: -180 180
|
||||
// @Units: deg
|
||||
// @User: Standard
|
||||
// @Increment: 1
|
||||
AP_GROUPINFO("SV4_POS", 4, AP_MotorsHeli_Dual, _servo4_pos, AP_MOTORS_HELI_DUAL_SERVO4_POS),
|
||||
|
||||
// @Param: SV5_POS
|
||||
// @DisplayName: Servo 5 Position
|
||||
// @Description: Angular location of swash servo #5
|
||||
// @Range: -180 180
|
||||
// @Units: deg
|
||||
// @User: Standard
|
||||
// @Increment: 1
|
||||
AP_GROUPINFO("SV5_POS", 5, AP_MotorsHeli_Dual, _servo5_pos, AP_MOTORS_HELI_DUAL_SERVO5_POS),
|
||||
|
||||
// @Param: SV6_POS
|
||||
// @DisplayName: Servo 6 Position
|
||||
// @Description: Angular location of swash servo #6
|
||||
// @Range: -180 180
|
||||
// @Units: deg
|
||||
// @User: Standard
|
||||
// @Increment: 1
|
||||
AP_GROUPINFO("SV6_POS", 6, AP_MotorsHeli_Dual, _servo6_pos, AP_MOTORS_HELI_DUAL_SERVO6_POS),
|
||||
|
||||
// @Param: PHANG1
|
||||
// @DisplayName: Swashplate 1 Phase Angle Compensation
|
||||
// @Description: Phase angle correction for rotor head. If pitching the swash forward induces a roll, this can be correct the problem
|
||||
// @Range: -90 90
|
||||
// @Units: deg
|
||||
// @User: Advanced
|
||||
// @Increment: 1
|
||||
AP_GROUPINFO("PHANG1", 7, AP_MotorsHeli_Dual, _swash1_phase_angle, 0),
|
||||
|
||||
// @Param: PHANG2
|
||||
// @DisplayName: Swashplate 2 Phase Angle Compensation
|
||||
// @Description: Phase angle correction for rotor head. If pitching the swash forward induces a roll, this can be correct the problem
|
||||
// @Range: -90 90
|
||||
// @Units: deg
|
||||
// @User: Advanced
|
||||
// @Increment: 1
|
||||
AP_GROUPINFO("PHANG2", 8, AP_MotorsHeli_Dual, _swash2_phase_angle, 0),
|
||||
// Indices 7-8 were used by phase angle params and should not be used
|
||||
|
||||
// @Param: DUAL_MODE
|
||||
// @DisplayName: Dual Mode
|
||||
@ -153,11 +85,41 @@ const AP_Param::GroupInfo AP_MotorsHeli_Dual::var_info[] = {
|
||||
AP_GROUPINFO("COL2_MID", 18, AP_MotorsHeli_Dual, _collective2_mid, AP_MOTORS_HELI_DUAL_COLLECTIVE2_MID),
|
||||
|
||||
// @Param: COL_CTRL_DIR
|
||||
// @DisplayName: Collective Control Direction
|
||||
// @DisplayName: Collective Control Direction for swash 1
|
||||
// @Description: Direction collective moves for positive pitch. 0 for Normal, 1 for Reversed
|
||||
// @Values: 0:Normal,1:Reversed
|
||||
// @User: Standard
|
||||
AP_GROUPINFO("COL_CTRL_DIR", 19, AP_MotorsHeli_Dual, _collective_direction, AP_MOTORS_HELI_DUAL_COLLECTIVE_DIRECTION_NORMAL),
|
||||
AP_GROUPINFO("COL_CTRL_DIR", 19, AP_MotorsHeli_Dual, _swash1_coll_dir, (int8_t)COLLECTIVE_DIRECTION_NORMAL),
|
||||
|
||||
// @Param: COL_CTRL_DIR2
|
||||
// @DisplayName: Collective Control Direction for swash 2
|
||||
// @Description: Direction collective moves for positive pitch. 0 for Normal, 1 for Reversed
|
||||
// @Values: 0:Normal,1:Reversed
|
||||
// @User: Standard
|
||||
AP_GROUPINFO("COL_CTRL_DIR2", 20, AP_MotorsHeli_Dual, _swash2_coll_dir, (int8_t)COLLECTIVE_DIRECTION_NORMAL),
|
||||
|
||||
// @Param: SWASH1_TYPE
|
||||
// @DisplayName: Swash Type for swashplate 1
|
||||
// @Description: Swash Type Setting
|
||||
// @Values: 0:H3 CCPM Adjustable, 1:H1 Straight Swash, 2:H3_140 CCPM
|
||||
// @User: Standard
|
||||
AP_GROUPINFO("SWASH1_TYPE", 21, AP_MotorsHeli_Dual, _swashplate1_type, (int8_t)SWASHPLATE_TYPE_H3),
|
||||
|
||||
// @Param: SWASH2_TYPE
|
||||
// @DisplayName: Swash Type for swashplate 2
|
||||
// @Description: Swash Type Setting
|
||||
// @Values: 0:H3 CCPM Adjustable, 1:H1 Straight Swash, 2:H3_140 CCPM
|
||||
// @User: Standard
|
||||
AP_GROUPINFO("SWASH2_TYPE", 22, AP_MotorsHeli_Dual, _swashplate2_type, (int8_t)SWASHPLATE_TYPE_H3),
|
||||
|
||||
// @Group: SW1_H3_
|
||||
// @Path: Swash.cpp
|
||||
AP_SUBGROUPINFO(_swash1_H3, "SW1_H3_", 23, AP_MotorsHeli_Dual, SwashInt16Param),
|
||||
|
||||
// @Group: SW2_H3_
|
||||
// @Path: Swash.cpp
|
||||
AP_SUBGROUPINFO(_swash2_H3, "SW2_H3_", 24, AP_MotorsHeli_Dual, SwashInt16Param),
|
||||
|
||||
|
||||
AP_GROUPEND
|
||||
};
|
||||
@ -173,6 +135,12 @@ void AP_MotorsHeli_Dual::set_update_rate( uint16_t speed_hz )
|
||||
for (uint8_t i=0; i<AP_MOTORS_HELI_DUAL_NUM_SWASHPLATE_SERVOS; i++) {
|
||||
mask |= 1U << (AP_MOTORS_MOT_1+i);
|
||||
}
|
||||
if (_swashplate1_type == SWASHPLATE_TYPE_H4_90 || _swashplate1_type == SWASHPLATE_TYPE_H4_45) {
|
||||
mask |= 1U << (AP_MOTORS_MOT_7);
|
||||
}
|
||||
if (_swashplate2_type == SWASHPLATE_TYPE_H4_90 || _swashplate2_type == SWASHPLATE_TYPE_H4_45) {
|
||||
mask |= 1U << (AP_MOTORS_MOT_8);
|
||||
}
|
||||
|
||||
rc_set_freq(mask, _speed_hz);
|
||||
}
|
||||
@ -185,6 +153,12 @@ bool AP_MotorsHeli_Dual::init_outputs()
|
||||
for (uint8_t i=0; i<AP_MOTORS_HELI_DUAL_NUM_SWASHPLATE_SERVOS; i++) {
|
||||
add_motor_num(CH_1+i);
|
||||
}
|
||||
if (_swashplate1_type == SWASHPLATE_TYPE_H4_90 || _swashplate1_type == SWASHPLATE_TYPE_H4_45) {
|
||||
add_motor_num(CH_7);
|
||||
}
|
||||
if (_swashplate2_type == SWASHPLATE_TYPE_H4_90 || _swashplate2_type == SWASHPLATE_TYPE_H4_45) {
|
||||
add_motor_num(CH_8);
|
||||
}
|
||||
|
||||
// set rotor servo range
|
||||
_rotor.init_servo();
|
||||
@ -195,6 +169,12 @@ bool AP_MotorsHeli_Dual::init_outputs()
|
||||
for (uint8_t i=0; i<AP_MOTORS_HELI_DUAL_NUM_SWASHPLATE_SERVOS; i++) {
|
||||
reset_swash_servo(SRV_Channels::get_motor_function(i));
|
||||
}
|
||||
if (_swashplate1_type == SWASHPLATE_TYPE_H4_90 || _swashplate1_type == SWASHPLATE_TYPE_H4_45) {
|
||||
reset_swash_servo(SRV_Channels::get_motor_function(6));
|
||||
}
|
||||
if (_swashplate2_type == SWASHPLATE_TYPE_H4_90 || _swashplate2_type == SWASHPLATE_TYPE_H4_45) {
|
||||
reset_swash_servo(SRV_Channels::get_motor_function(7));
|
||||
}
|
||||
|
||||
_flags.initialised_ok = true;
|
||||
|
||||
@ -290,82 +270,100 @@ void AP_MotorsHeli_Dual::calculate_scalars()
|
||||
_collective_mid_pct = ((float)(_collective_mid-_collective_min))/((float)(_collective_max-_collective_min));
|
||||
_collective2_mid_pct = ((float)(_collective2_mid-_collective2_min))/((float)(_collective2_max-_collective2_min));
|
||||
|
||||
// calculate factors based on swash type and servo position
|
||||
calculate_roll_pitch_collective_factors();
|
||||
// configure swashplate 1 and update scalars
|
||||
// uint8_t swashplate1_type = _swashplate1_type;
|
||||
if (_swashplate1_type == SWASHPLATE_TYPE_H3) {
|
||||
if (_swash1_H3.get_enable() == 0) {
|
||||
_swash1_H3.set_enable(1);
|
||||
}
|
||||
_swashplate1.set_servo1_pos(_swash1_H3.get_servo1_pos());
|
||||
_swashplate1.set_servo2_pos(_swash1_H3.get_servo2_pos());
|
||||
_swashplate1.set_servo3_pos(_swash1_H3.get_servo3_pos());
|
||||
_swashplate1.set_phase_angle(_swash1_H3.get_phase_angle());
|
||||
} else {
|
||||
if (_swash1_H3.get_enable() == 1) {
|
||||
_swash1_H3.set_enable(0);
|
||||
}
|
||||
}
|
||||
_swashplate1.set_swash_type(static_cast<SwashPlateType>((uint8_t)_swashplate1_type));
|
||||
_swashplate1.set_collective_direction(static_cast<CollectiveDirection>((uint8_t)_swash1_coll_dir));
|
||||
_swashplate1.calculate_roll_pitch_collective_factors();
|
||||
|
||||
// configure swashplate 2 and update scalars
|
||||
if (_swashplate2_type == SWASHPLATE_TYPE_H3) {
|
||||
if (_swash2_H3.get_enable() == 0) {
|
||||
_swash2_H3.set_enable(1);
|
||||
}
|
||||
_swashplate2.set_servo1_pos(_swash2_H3.get_servo1_pos());
|
||||
_swashplate2.set_servo2_pos(_swash2_H3.get_servo2_pos());
|
||||
_swashplate2.set_servo3_pos(_swash2_H3.get_servo3_pos());
|
||||
_swashplate2.set_phase_angle(_swash2_H3.get_phase_angle());
|
||||
} else {
|
||||
if (_swash2_H3.get_enable() == 1) {
|
||||
_swash2_H3.set_enable(0);
|
||||
}
|
||||
}
|
||||
_swashplate2.set_swash_type(static_cast<SwashPlateType>((uint8_t)_swashplate2_type));
|
||||
_swashplate2.set_collective_direction(static_cast<CollectiveDirection>((uint8_t)_swash2_coll_dir));
|
||||
_swashplate2.calculate_roll_pitch_collective_factors();
|
||||
|
||||
// set mode of main rotor controller and trigger recalculation of scalars
|
||||
_rotor.set_control_mode(static_cast<RotorControlMode>(_rsc_mode.get()));
|
||||
calculate_armed_scalars();
|
||||
}
|
||||
|
||||
// calculate_swash_factors - calculate factors based on swash type and servo position
|
||||
// To Do: support H3-140 swashplates in Heli Dual?
|
||||
void AP_MotorsHeli_Dual::calculate_roll_pitch_collective_factors()
|
||||
// get_swashplate - calculate movement of each swashplate based on configuration
|
||||
float AP_MotorsHeli_Dual::get_swashplate (int8_t swash_num, int8_t swash_axis, float pitch_input, float roll_input, float yaw_input, float coll_input)
|
||||
{
|
||||
float swash_tilt = 0.0f;
|
||||
if (_dual_mode == AP_MOTORS_HELI_DUAL_MODE_TRANSVERSE) {
|
||||
// roll factors
|
||||
_rollFactor[CH_1] = _dcp_scaler;
|
||||
_rollFactor[CH_2] = _dcp_scaler;
|
||||
_rollFactor[CH_3] = _dcp_scaler;
|
||||
|
||||
_rollFactor[CH_4] = -_dcp_scaler;
|
||||
_rollFactor[CH_5] = -_dcp_scaler;
|
||||
_rollFactor[CH_6] = -_dcp_scaler;
|
||||
|
||||
// pitch factors
|
||||
_pitchFactor[CH_1] = cosf(radians(_servo1_pos - _swash1_phase_angle));
|
||||
_pitchFactor[CH_2] = cosf(radians(_servo2_pos - _swash1_phase_angle));
|
||||
_pitchFactor[CH_3] = cosf(radians(_servo3_pos - _swash1_phase_angle));
|
||||
|
||||
_pitchFactor[CH_4] = cosf(radians(_servo4_pos - _swash2_phase_angle));
|
||||
_pitchFactor[CH_5] = cosf(radians(_servo5_pos - _swash2_phase_angle));
|
||||
_pitchFactor[CH_6] = cosf(radians(_servo6_pos - _swash2_phase_angle));
|
||||
|
||||
// yaw factors
|
||||
_yawFactor[CH_1] = cosf(radians(_servo1_pos + 180 - _swash1_phase_angle)) * _yaw_scaler;
|
||||
_yawFactor[CH_2] = cosf(radians(_servo2_pos + 180 - _swash1_phase_angle)) * _yaw_scaler;
|
||||
_yawFactor[CH_3] = cosf(radians(_servo3_pos + 180 - _swash1_phase_angle)) * _yaw_scaler;
|
||||
|
||||
_yawFactor[CH_4] = cosf(radians(_servo4_pos - _swash2_phase_angle)) * _yaw_scaler;
|
||||
_yawFactor[CH_5] = cosf(radians(_servo5_pos - _swash2_phase_angle)) * _yaw_scaler;
|
||||
_yawFactor[CH_6] = cosf(radians(_servo6_pos - _swash2_phase_angle)) * _yaw_scaler;
|
||||
// roll tilt
|
||||
if (swash_axis == AP_MOTORS_HELI_DUAL_SWASH_AXIS_ROLL) {
|
||||
if (swash_num == 1) {
|
||||
swash_tilt = 0.0f;
|
||||
} else if (swash_num == 2) {
|
||||
swash_tilt = 0.0f;
|
||||
}
|
||||
} else if (swash_axis == AP_MOTORS_HELI_DUAL_SWASH_AXIS_PITCH) {
|
||||
// pitch tilt
|
||||
if (swash_num == 1) {
|
||||
swash_tilt = pitch_input - _yaw_scaler * yaw_input;
|
||||
} else if (swash_num == 2) {
|
||||
swash_tilt = pitch_input + _yaw_scaler * yaw_input;
|
||||
}
|
||||
} else if (swash_axis == AP_MOTORS_HELI_DUAL_SWASH_AXIS_COLL) {
|
||||
// collective
|
||||
if (swash_num == 1) {
|
||||
swash_tilt = 0.45f * _dcp_scaler * roll_input + coll_input;
|
||||
} else if (swash_num == 2) {
|
||||
swash_tilt = -0.45f * _dcp_scaler * roll_input + coll_input;
|
||||
}
|
||||
}
|
||||
} else { // AP_MOTORS_HELI_DUAL_MODE_TANDEM
|
||||
// roll factors
|
||||
_rollFactor[CH_1] = cosf(radians(_servo1_pos + 90 - _swash1_phase_angle));
|
||||
_rollFactor[CH_2] = cosf(radians(_servo2_pos + 90 - _swash1_phase_angle));
|
||||
_rollFactor[CH_3] = cosf(radians(_servo3_pos + 90 - _swash1_phase_angle));
|
||||
|
||||
_rollFactor[CH_4] = cosf(radians(_servo4_pos + 90 - _swash2_phase_angle));
|
||||
_rollFactor[CH_5] = cosf(radians(_servo5_pos + 90 - _swash2_phase_angle));
|
||||
_rollFactor[CH_6] = cosf(radians(_servo6_pos + 90 - _swash2_phase_angle));
|
||||
|
||||
// pitch factors
|
||||
_pitchFactor[CH_1] = _dcp_scaler;
|
||||
_pitchFactor[CH_2] = _dcp_scaler;
|
||||
_pitchFactor[CH_3] = _dcp_scaler;
|
||||
|
||||
_pitchFactor[CH_4] = -_dcp_scaler;
|
||||
_pitchFactor[CH_5] = -_dcp_scaler;
|
||||
_pitchFactor[CH_6] = -_dcp_scaler;
|
||||
|
||||
// yaw factors
|
||||
_yawFactor[CH_1] = cosf(radians(_servo1_pos + 90 - _swash1_phase_angle)) * _yaw_scaler;
|
||||
_yawFactor[CH_2] = cosf(radians(_servo2_pos + 90 - _swash1_phase_angle)) * _yaw_scaler;
|
||||
_yawFactor[CH_3] = cosf(radians(_servo3_pos + 90 - _swash1_phase_angle)) * _yaw_scaler;
|
||||
|
||||
_yawFactor[CH_4] = cosf(radians(_servo4_pos + 270 - _swash2_phase_angle)) * _yaw_scaler;
|
||||
_yawFactor[CH_5] = cosf(radians(_servo5_pos + 270 - _swash2_phase_angle)) * _yaw_scaler;
|
||||
_yawFactor[CH_6] = cosf(radians(_servo6_pos + 270 - _swash2_phase_angle)) * _yaw_scaler;
|
||||
// roll tilt
|
||||
if (swash_axis == AP_MOTORS_HELI_DUAL_SWASH_AXIS_ROLL) {
|
||||
if (swash_num == 1) {
|
||||
swash_tilt = roll_input + _yaw_scaler * yaw_input;
|
||||
} else if (swash_num == 2) {
|
||||
swash_tilt = roll_input - _yaw_scaler * yaw_input;
|
||||
}
|
||||
} else if (swash_axis == AP_MOTORS_HELI_DUAL_SWASH_AXIS_PITCH) {
|
||||
// pitch tilt
|
||||
if (swash_num == 1) {
|
||||
swash_tilt = 0.0f;
|
||||
} else if (swash_num == 2) {
|
||||
swash_tilt = 0.0f;
|
||||
}
|
||||
} else if (swash_axis == AP_MOTORS_HELI_DUAL_SWASH_AXIS_COLL) {
|
||||
// collective
|
||||
if (swash_num == 1) {
|
||||
swash_tilt = 0.45f * _dcp_scaler * pitch_input + coll_input;
|
||||
} else if (swash_num == 2) {
|
||||
swash_tilt = -0.45f * _dcp_scaler * pitch_input + coll_input;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
// collective factors
|
||||
_collectiveFactor[CH_1] = 1;
|
||||
_collectiveFactor[CH_2] = 1;
|
||||
_collectiveFactor[CH_3] = 1;
|
||||
|
||||
_collectiveFactor[CH_4] = 1;
|
||||
_collectiveFactor[CH_5] = 1;
|
||||
_collectiveFactor[CH_6] = 1;
|
||||
return swash_tilt;
|
||||
}
|
||||
|
||||
// get_motor_mask - returns a bitmask of which outputs are being used for motors or servos (1 means being used)
|
||||
@ -377,6 +375,12 @@ uint16_t AP_MotorsHeli_Dual::get_motor_mask()
|
||||
for (uint8_t i=0; i<AP_MOTORS_HELI_DUAL_NUM_SWASHPLATE_SERVOS; i++) {
|
||||
mask |= 1U << (AP_MOTORS_MOT_1+i);
|
||||
}
|
||||
if (_swashplate1_type == SWASHPLATE_TYPE_H4_90 || _swashplate1_type == SWASHPLATE_TYPE_H4_45) {
|
||||
mask |= 1U << AP_MOTORS_MOT_7;
|
||||
}
|
||||
if (_swashplate2_type == SWASHPLATE_TYPE_H4_90 || _swashplate2_type == SWASHPLATE_TYPE_H4_45) {
|
||||
mask |= 1U << AP_MOTORS_MOT_8;
|
||||
}
|
||||
mask |= 1U << AP_MOTORS_HELI_DUAL_RSC;
|
||||
return mask;
|
||||
}
|
||||
@ -495,29 +499,34 @@ void AP_MotorsHeli_Dual::move_actuators(float roll_out, float pitch_out, float c
|
||||
float collective2_scaler = ((float)(_collective2_max-_collective2_min))*0.001f;
|
||||
float collective2_out_scaled = collective2_out * collective2_scaler + (_collective2_min - 1000)*0.001f;
|
||||
|
||||
// Collective control direction. Swash plates move up for negative collective pitch, down for positive collective pitch
|
||||
if (_collective_direction == AP_MOTORS_HELI_DUAL_COLLECTIVE_DIRECTION_REVERSED){
|
||||
collective_out_scaled = 1 - collective_out_scaled;
|
||||
collective2_out_scaled = 1 - collective2_out_scaled;
|
||||
}
|
||||
|
||||
// feed power estimate into main rotor controller
|
||||
// ToDo: add main rotor cyclic power?
|
||||
_rotor.set_collective(fabsf(collective_out));
|
||||
|
||||
// swashplate servos
|
||||
_servo_out[CH_1] = (_rollFactor[CH_1] * roll_out + _pitchFactor[CH_1] * pitch_out + _yawFactor[CH_1] * yaw_out)*0.45f + _collectiveFactor[CH_1] * collective_out_scaled;
|
||||
_servo_out[CH_2] = (_rollFactor[CH_2] * roll_out + _pitchFactor[CH_2] * pitch_out + _yawFactor[CH_2] * yaw_out)*0.45f + _collectiveFactor[CH_2] * collective_out_scaled;
|
||||
_servo_out[CH_3] = (_rollFactor[CH_3] * roll_out + _pitchFactor[CH_3] * pitch_out + _yawFactor[CH_3] * yaw_out)*0.45f + _collectiveFactor[CH_3] * collective_out_scaled;
|
||||
// compute swashplate tilt
|
||||
float swash1_pitch = get_swashplate(1, AP_MOTORS_HELI_DUAL_SWASH_AXIS_PITCH, pitch_out, roll_out, yaw_out, collective_out_scaled);
|
||||
float swash1_roll = get_swashplate(1, AP_MOTORS_HELI_DUAL_SWASH_AXIS_ROLL, pitch_out, roll_out, yaw_out, collective_out_scaled);
|
||||
float swash1_coll = get_swashplate(1, AP_MOTORS_HELI_DUAL_SWASH_AXIS_COLL, pitch_out, roll_out, yaw_out, collective_out_scaled);
|
||||
float swash2_pitch = get_swashplate(2, AP_MOTORS_HELI_DUAL_SWASH_AXIS_PITCH, pitch_out, roll_out, yaw_out, collective2_out_scaled);
|
||||
float swash2_roll = get_swashplate(2, AP_MOTORS_HELI_DUAL_SWASH_AXIS_ROLL, pitch_out, roll_out, yaw_out, collective2_out_scaled);
|
||||
float swash2_coll = get_swashplate(2, AP_MOTORS_HELI_DUAL_SWASH_AXIS_COLL, pitch_out, roll_out, yaw_out, collective2_out_scaled);
|
||||
|
||||
_servo_out[CH_4] = (_rollFactor[CH_4] * roll_out + _pitchFactor[CH_4] * pitch_out + _yawFactor[CH_4] * yaw_out)*0.45f + _collectiveFactor[CH_4] * collective2_out_scaled;
|
||||
_servo_out[CH_5] = (_rollFactor[CH_5] * roll_out + _pitchFactor[CH_5] * pitch_out + _yawFactor[CH_5] * yaw_out)*0.45f + _collectiveFactor[CH_5] * collective2_out_scaled;
|
||||
_servo_out[CH_6] = (_rollFactor[CH_6] * roll_out + _pitchFactor[CH_6] * pitch_out + _yawFactor[CH_6] * yaw_out)*0.45f + _collectiveFactor[CH_6] * collective2_out_scaled;
|
||||
|
||||
// rescale from -1..1, so we can use the pwm calc that includes trim
|
||||
for (uint8_t i=0; i<AP_MOTORS_HELI_DUAL_NUM_SWASHPLATE_SERVOS; i++) {
|
||||
_servo_out[i] = 2*_servo_out[i] - 1;
|
||||
// get servo positions from swashplate library
|
||||
_servo_out[CH_1] = _swashplate1.get_servo_out(CH_1,swash1_pitch,swash1_roll,swash1_coll);
|
||||
_servo_out[CH_2] = _swashplate1.get_servo_out(CH_2,swash1_pitch,swash1_roll,swash1_coll);
|
||||
_servo_out[CH_3] = _swashplate1.get_servo_out(CH_3,swash1_pitch,swash1_roll,swash1_coll);
|
||||
if (_swashplate1_type == SWASHPLATE_TYPE_H4_90 || _swashplate1_type == SWASHPLATE_TYPE_H4_45) {
|
||||
_servo_out[CH_7] = _swashplate1.get_servo_out(CH_4,swash1_pitch,swash1_roll,swash1_coll);
|
||||
}
|
||||
|
||||
// get servo positions from swashplate library
|
||||
_servo_out[CH_4] = _swashplate2.get_servo_out(CH_1,swash2_pitch,swash2_roll,swash2_coll);
|
||||
_servo_out[CH_5] = _swashplate2.get_servo_out(CH_2,swash2_pitch,swash2_roll,swash2_coll);
|
||||
_servo_out[CH_6] = _swashplate2.get_servo_out(CH_3,swash2_pitch,swash2_roll,swash2_coll);
|
||||
if (_swashplate2_type == SWASHPLATE_TYPE_H4_90 || _swashplate2_type == SWASHPLATE_TYPE_H4_45) {
|
||||
_servo_out[CH_8] = _swashplate2.get_servo_out(CH_4,swash2_pitch,swash2_roll,swash2_coll);
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
void AP_MotorsHeli_Dual::output_to_motors()
|
||||
@ -527,7 +536,16 @@ void AP_MotorsHeli_Dual::output_to_motors()
|
||||
}
|
||||
// actually move the servos. PWM is sent based on nominal 1500 center. servo output shifts center based on trim value.
|
||||
for (uint8_t i=0; i<AP_MOTORS_HELI_DUAL_NUM_SWASHPLATE_SERVOS; i++) {
|
||||
rc_write_swash(i, _servo_out[i]);
|
||||
rc_write_swash(i, _servo_out[CH_1+i]);
|
||||
}
|
||||
|
||||
// write to servo for 4 servo of 4 servo swashplate
|
||||
if (_swashplate1_type == SWASHPLATE_TYPE_H4_90 || _swashplate1_type == SWASHPLATE_TYPE_H4_45) {
|
||||
rc_write_swash(AP_MOTORS_MOT_7, _servo_out[CH_7]);
|
||||
}
|
||||
// write to servo for 4 servo of 4 servo swashplate
|
||||
if (_swashplate2_type == SWASHPLATE_TYPE_H4_90 || _swashplate2_type == SWASHPLATE_TYPE_H4_45) {
|
||||
rc_write_swash(AP_MOTORS_MOT_8, _servo_out[CH_8]);
|
||||
}
|
||||
|
||||
switch (_spool_mode) {
|
||||
|
||||
@ -11,18 +11,7 @@
|
||||
|
||||
#include "AP_MotorsHeli.h"
|
||||
#include "AP_MotorsHeli_RSC.h"
|
||||
|
||||
// servo position defaults
|
||||
#define AP_MOTORS_HELI_DUAL_SERVO1_POS -60
|
||||
#define AP_MOTORS_HELI_DUAL_SERVO2_POS 60
|
||||
#define AP_MOTORS_HELI_DUAL_SERVO3_POS 180
|
||||
#define AP_MOTORS_HELI_DUAL_SERVO4_POS -60
|
||||
#define AP_MOTORS_HELI_DUAL_SERVO5_POS 60
|
||||
#define AP_MOTORS_HELI_DUAL_SERVO6_POS 180
|
||||
|
||||
// collective control direction definitions
|
||||
#define AP_MOTORS_HELI_DUAL_COLLECTIVE_DIRECTION_NORMAL 0
|
||||
#define AP_MOTORS_HELI_DUAL_COLLECTIVE_DIRECTION_REVERSED 1
|
||||
#include "AP_MotorsHeli_Swash.h"
|
||||
|
||||
// rsc function output channel
|
||||
#define AP_MOTORS_HELI_DUAL_RSC CH_8
|
||||
@ -31,6 +20,11 @@
|
||||
#define AP_MOTORS_HELI_DUAL_MODE_TANDEM 0 // tandem mode (rotors front and aft)
|
||||
#define AP_MOTORS_HELI_DUAL_MODE_TRANSVERSE 1 // transverse mode (rotors side by side)
|
||||
|
||||
// tandem modes
|
||||
#define AP_MOTORS_HELI_DUAL_SWASH_AXIS_PITCH 0 // swashplate pitch tilt axis
|
||||
#define AP_MOTORS_HELI_DUAL_SWASH_AXIS_ROLL 1 // swashplate roll tilt axis
|
||||
#define AP_MOTORS_HELI_DUAL_SWASH_AXIS_COLL 2 // swashplate collective axis
|
||||
|
||||
// default differential-collective-pitch scaler
|
||||
#define AP_MOTORS_HELI_DUAL_DCP_SCALER 0.25f
|
||||
|
||||
@ -105,14 +99,18 @@ protected:
|
||||
// update_motor_controls - sends commands to motor controllers
|
||||
void update_motor_control(RotorControlState state) override;
|
||||
|
||||
// calculate_roll_pitch_collective_factors - calculate factors based on swash type and servo position
|
||||
void calculate_roll_pitch_collective_factors () override;
|
||||
// calculate_swashplate_tilt - calculate tilt of each swashplate based on configuration
|
||||
float get_swashplate (int8_t swash_num, int8_t swash_axis, float pitch_input, float roll_input, float yaw_input, float coll_input);
|
||||
|
||||
// move_actuators - moves swash plate to attitude of parameters passed in
|
||||
void move_actuators(float roll_out, float pitch_out, float coll_in, float yaw_out) override;
|
||||
|
||||
// objects we depend upon
|
||||
AP_MotorsHeli_RSC _rotor; // main rotor controller
|
||||
AP_MotorsHeli_Swash _swashplate1; // swashplate1
|
||||
AP_MotorsHeli_Swash _swashplate2; // swashplate2
|
||||
SwashInt16Param _swash1_H3; // H3 servo positions for swash 1
|
||||
SwashInt16Param _swash2_H3; // H3 servo positions for swash 2
|
||||
|
||||
// internal variables
|
||||
float _oscillate_angle = 0.0f; // cyclic oscillation angle, used by servo_test function
|
||||
@ -120,19 +118,16 @@ protected:
|
||||
float _collective_test = 0.0f; // over-ride for collective output, used by servo_test function
|
||||
float _roll_test = 0.0f; // over-ride for roll output, used by servo_test function
|
||||
float _pitch_test = 0.0f; // over-ride for pitch output, used by servo_test function
|
||||
float _servo_out[AP_MOTORS_HELI_DUAL_NUM_SWASHPLATE_SERVOS]; // output value sent to motor
|
||||
float _servo_out[8]; // output value sent to motor
|
||||
|
||||
// parameters
|
||||
AP_Int16 _collective2_min; // Lowest possible servo position for the rear swashplate
|
||||
AP_Int16 _collective2_max; // Highest possible servo position for the rear swashplate
|
||||
AP_Int16 _collective2_mid; // Swash servo position corresponding to zero collective pitch for the rear swashplate (or zero lift for Asymmetrical blades)
|
||||
AP_Int16 _servo1_pos; // angular location of swash servo #1
|
||||
AP_Int16 _servo2_pos; // angular location of swash servo #2
|
||||
AP_Int16 _servo3_pos; // angular location of swash servo #3
|
||||
AP_Int16 _servo4_pos; // angular location of swash servo #4
|
||||
AP_Int16 _servo5_pos; // angular location of swash servo #5
|
||||
AP_Int16 _servo6_pos; // angular location of swash servo #6
|
||||
AP_Int8 _collective_direction; // Collective control direction, normal or reversed
|
||||
AP_Int8 _swashplate1_type; // Swash Type Setting
|
||||
AP_Int8 _swashplate2_type; // Swash Type Setting
|
||||
AP_Int8 _swash1_coll_dir; // Collective control direction, normal or reversed
|
||||
AP_Int8 _swash2_coll_dir; // Collective control direction, normal or reversed
|
||||
AP_Int16 _swash1_phase_angle; // phase angle correction for 1st swash.
|
||||
AP_Int16 _swash2_phase_angle; // phase angle correction for 2nd swash.
|
||||
AP_Int8 _dual_mode; // which dual mode the heli is
|
||||
@ -142,10 +137,6 @@ protected:
|
||||
|
||||
// internal variables
|
||||
float _collective2_mid_pct = 0.0f; // collective mid parameter value for rear swashplate converted to 0 ~ 1 range
|
||||
float _rollFactor[AP_MOTORS_HELI_DUAL_NUM_SWASHPLATE_SERVOS];
|
||||
float _pitchFactor[AP_MOTORS_HELI_DUAL_NUM_SWASHPLATE_SERVOS];
|
||||
float _collectiveFactor[AP_MOTORS_HELI_DUAL_NUM_SWASHPLATE_SERVOS];
|
||||
float _yawFactor[AP_MOTORS_HELI_DUAL_NUM_SWASHPLATE_SERVOS];
|
||||
};
|
||||
|
||||
#endif // AP_MotorsHeli_Dual
|
||||
|
||||
@ -81,7 +81,7 @@ protected:
|
||||
void update_motor_control(RotorControlState state) override;
|
||||
|
||||
// calculate_roll_pitch_collective_factors - setup rate factors
|
||||
void calculate_roll_pitch_collective_factors () override;
|
||||
void calculate_roll_pitch_collective_factors ();
|
||||
|
||||
// move_actuators - moves swash plate to attitude of parameters passed in
|
||||
void move_actuators(float roll_out, float pitch_out, float coll_in, float yaw_out) override;
|
||||
|
||||
@ -24,32 +24,7 @@ extern const AP_HAL::HAL& hal;
|
||||
const AP_Param::GroupInfo AP_MotorsHeli_Single::var_info[] = {
|
||||
AP_NESTEDGROUPINFO(AP_MotorsHeli, 0),
|
||||
|
||||
// @Param: SV1_POS
|
||||
// @DisplayName: Servo 1 Position
|
||||
// @Description: Angular location of swash servo #1 - only used for H3 swash type
|
||||
// @Range: -180 180
|
||||
// @Units: deg
|
||||
// @User: Standard
|
||||
// @Increment: 1
|
||||
AP_GROUPINFO("SV1_POS", 1, AP_MotorsHeli_Single, _servo1_pos, AP_MOTORS_HELI_SINGLE_SERVO1_POS),
|
||||
|
||||
// @Param: SV2_POS
|
||||
// @DisplayName: Servo 2 Position
|
||||
// @Description: Angular location of swash servo #2 - only used for H3 swash type
|
||||
// @Range: -180 180
|
||||
// @Units: deg
|
||||
// @User: Standard
|
||||
// @Increment: 1
|
||||
AP_GROUPINFO("SV2_POS", 2, AP_MotorsHeli_Single, _servo2_pos, AP_MOTORS_HELI_SINGLE_SERVO2_POS),
|
||||
|
||||
// @Param: SV3_POS
|
||||
// @DisplayName: Servo 3 Position
|
||||
// @Description: Angular location of swash servo #3 - only used for H3 swash type
|
||||
// @Range: -180 180
|
||||
// @Units: deg
|
||||
// @User: Standard
|
||||
// @Increment: 1
|
||||
AP_GROUPINFO("SV3_POS", 3, AP_MotorsHeli_Single, _servo3_pos, AP_MOTORS_HELI_SINGLE_SERVO3_POS),
|
||||
// Indices 1-3 were used by servo position params and should not be used
|
||||
|
||||
// @Param: TAIL_TYPE
|
||||
// @DisplayName: Tail Type
|
||||
@ -63,7 +38,7 @@ const AP_Param::GroupInfo AP_MotorsHeli_Single::var_info[] = {
|
||||
// @Description: Swash Type Setting
|
||||
// @Values: 0:H3 CCPM Adjustable, 1:H1 Straight Swash, 2:H3_140 CCPM
|
||||
// @User: Standard
|
||||
AP_GROUPINFO("SWASH_TYPE", 5, AP_MotorsHeli_Single, _swash_type, AP_MOTORS_HELI_SINGLE_SWASH_H3),
|
||||
AP_GROUPINFO("SWASH_TYPE", 5, AP_MotorsHeli_Single, _swashplate_type, (int8_t)SWASHPLATE_TYPE_H3),
|
||||
|
||||
// @Param: GYR_GAIN
|
||||
// @DisplayName: External Gyro Gain
|
||||
@ -74,14 +49,7 @@ const AP_Param::GroupInfo AP_MotorsHeli_Single::var_info[] = {
|
||||
// @User: Standard
|
||||
AP_GROUPINFO("GYR_GAIN", 6, AP_MotorsHeli_Single, _ext_gyro_gain_std, AP_MOTORS_HELI_SINGLE_EXT_GYRO_GAIN),
|
||||
|
||||
// @Param: PHANG
|
||||
// @DisplayName: Swashplate Phase Angle Compensation
|
||||
// @Description: Only for H3 swashplate. If pitching the swash forward induces a roll, this can be correct the problem
|
||||
// @Range: -30 30
|
||||
// @Units: deg
|
||||
// @User: Advanced
|
||||
// @Increment: 1
|
||||
AP_GROUPINFO("PHANG", 7, AP_MotorsHeli_Single, _phase_angle, 0),
|
||||
// Index 7 was used for phase angle and should not be used
|
||||
|
||||
// @Param: COLYAW
|
||||
// @DisplayName: Collective-Yaw Mixing
|
||||
@ -123,10 +91,14 @@ const AP_Param::GroupInfo AP_MotorsHeli_Single::var_info[] = {
|
||||
// @Description: Direction collective moves for positive pitch. 0 for Normal, 1 for Reversed
|
||||
// @Values: 0:Normal,1:Reversed
|
||||
// @User: Standard
|
||||
AP_GROUPINFO("COL_CTRL_DIR", 19, AP_MotorsHeli_Single, _collective_direction, AP_MOTORS_HELI_SINGLE_COLLECTIVE_DIRECTION_NORMAL),
|
||||
AP_GROUPINFO("COL_CTRL_DIR", 19, AP_MotorsHeli_Single, _swash_coll_dir, (int8_t)COLLECTIVE_DIRECTION_NORMAL),
|
||||
|
||||
// parameters up to and including 29 are reserved for tradheli
|
||||
|
||||
// @Group: H3_
|
||||
// @Path: Swash.cpp
|
||||
AP_SUBGROUPINFO(_swash_H3, "H3_", 20, AP_MotorsHeli_Single, SwashInt16Param),
|
||||
|
||||
AP_GROUPEND
|
||||
};
|
||||
|
||||
@ -144,6 +116,9 @@ void AP_MotorsHeli_Single::set_update_rate( uint16_t speed_hz )
|
||||
1U << AP_MOTORS_MOT_2 |
|
||||
1U << AP_MOTORS_MOT_3 |
|
||||
1U << AP_MOTORS_MOT_4;
|
||||
if (_swashplate_type == SWASHPLATE_TYPE_H4_90 || _swashplate_type == SWASHPLATE_TYPE_H4_45) {
|
||||
mask |= 1U << (AP_MOTORS_MOT_5);
|
||||
}
|
||||
rc_set_freq(mask, _speed_hz);
|
||||
}
|
||||
|
||||
@ -155,6 +130,10 @@ bool AP_MotorsHeli_Single::init_outputs()
|
||||
for (uint8_t i=0; i<AP_MOTORS_HELI_SINGLE_NUM_SWASHPLATE_SERVOS; i++) {
|
||||
add_motor_num(CH_1+i);
|
||||
}
|
||||
if (_swashplate_type == SWASHPLATE_TYPE_H4_90 || _swashplate_type == SWASHPLATE_TYPE_H4_45) {
|
||||
add_motor_num(CH_5);
|
||||
}
|
||||
|
||||
// yaw servo
|
||||
add_motor_num(CH_4);
|
||||
|
||||
@ -178,6 +157,9 @@ bool AP_MotorsHeli_Single::init_outputs()
|
||||
for (uint8_t i=0; i<AP_MOTORS_HELI_SINGLE_NUM_SWASHPLATE_SERVOS; i++) {
|
||||
reset_swash_servo(SRV_Channels::get_motor_function(i));
|
||||
}
|
||||
if (_swashplate_type == SWASHPLATE_TYPE_H4_90 || _swashplate_type == SWASHPLATE_TYPE_H4_45) {
|
||||
reset_swash_servo(SRV_Channels::get_motor_function(4));
|
||||
}
|
||||
|
||||
// yaw servo is an angle from -4500 to 4500
|
||||
SRV_Channels::set_angle(SRV_Channel::k_motor4, YAW_SERVO_MAX_ANGLE);
|
||||
@ -269,8 +251,23 @@ void AP_MotorsHeli_Single::calculate_scalars()
|
||||
// calculate collective mid point as a number from 0 to 1
|
||||
_collective_mid_pct = ((float)(_collective_mid-_collective_min))/((float)(_collective_max-_collective_min));
|
||||
|
||||
// calculate factors based on swash type and servo position
|
||||
calculate_roll_pitch_collective_factors();
|
||||
// configure swashplate and update scalars
|
||||
if (_swashplate_type == SWASHPLATE_TYPE_H3) {
|
||||
if (_swash_H3.get_enable() == 0) {
|
||||
_swash_H3.set_enable(1);
|
||||
}
|
||||
_swashplate.set_servo1_pos(_swash_H3.get_servo1_pos());
|
||||
_swashplate.set_servo2_pos(_swash_H3.get_servo2_pos());
|
||||
_swashplate.set_servo3_pos(_swash_H3.get_servo3_pos());
|
||||
_swashplate.set_phase_angle(_swash_H3.get_phase_angle());
|
||||
} else {
|
||||
if (_swash_H3.get_enable() == 1) {
|
||||
_swash_H3.set_enable(0);
|
||||
}
|
||||
}
|
||||
_swashplate.set_swash_type(static_cast<SwashPlateType>((uint8_t)_swashplate_type));
|
||||
_swashplate.set_collective_direction(static_cast<CollectiveDirection>((uint8_t)_swash_coll_dir));
|
||||
_swashplate.calculate_roll_pitch_collective_factors();
|
||||
|
||||
// send setpoints to main rotor controller and trigger recalculation of scalars
|
||||
_main_rotor.set_control_mode(static_cast<RotorControlMode>(_rsc_mode.get()));
|
||||
@ -292,57 +289,6 @@ void AP_MotorsHeli_Single::calculate_scalars()
|
||||
}
|
||||
}
|
||||
|
||||
// CCPM Mixers - calculate mixing scale factors by swashplate type
|
||||
void AP_MotorsHeli_Single::calculate_roll_pitch_collective_factors()
|
||||
{
|
||||
if (_swash_type == AP_MOTORS_HELI_SINGLE_SWASH_H3) { //Three-Servo adjustable CCPM mixer factors
|
||||
// aileron factors
|
||||
_rollFactor[CH_1] = cosf(radians(_servo1_pos + 90 - _phase_angle));
|
||||
_rollFactor[CH_2] = cosf(radians(_servo2_pos + 90 - _phase_angle));
|
||||
_rollFactor[CH_3] = cosf(radians(_servo3_pos + 90 - _phase_angle));
|
||||
|
||||
// elevator factors
|
||||
_pitchFactor[CH_1] = cosf(radians(_servo1_pos - _phase_angle));
|
||||
_pitchFactor[CH_2] = cosf(radians(_servo2_pos - _phase_angle));
|
||||
_pitchFactor[CH_3] = cosf(radians(_servo3_pos - _phase_angle));
|
||||
|
||||
// collective factors
|
||||
_collectiveFactor[CH_1] = 1;
|
||||
_collectiveFactor[CH_2] = 1;
|
||||
_collectiveFactor[CH_3] = 1;
|
||||
} else if (_swash_type == AP_MOTORS_HELI_SINGLE_SWASH_H3_140) { //Three-Servo H3-140 CCPM mixer factors
|
||||
// aileron factors
|
||||
_rollFactor[CH_1] = 1;
|
||||
_rollFactor[CH_2] = -1;
|
||||
_rollFactor[CH_3] = 0;
|
||||
|
||||
// elevator factors
|
||||
_pitchFactor[CH_1] = 1;
|
||||
_pitchFactor[CH_2] = 1;
|
||||
_pitchFactor[CH_3] = -1;
|
||||
|
||||
// collective factors
|
||||
_collectiveFactor[CH_1] = 1;
|
||||
_collectiveFactor[CH_2] = 1;
|
||||
_collectiveFactor[CH_3] = 1;
|
||||
} else { //H1 straight outputs, no mixing
|
||||
// aileron factors
|
||||
_rollFactor[CH_1] = 1;
|
||||
_rollFactor[CH_2] = 0;
|
||||
_rollFactor[CH_3] = 0;
|
||||
|
||||
// elevator factors
|
||||
_pitchFactor[CH_1] = 0;
|
||||
_pitchFactor[CH_2] = 1;
|
||||
_pitchFactor[CH_3] = 0;
|
||||
|
||||
// collective factors
|
||||
_collectiveFactor[CH_1] = 0;
|
||||
_collectiveFactor[CH_2] = 0;
|
||||
_collectiveFactor[CH_3] = 1;
|
||||
}
|
||||
}
|
||||
|
||||
// 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_MotorsHeli_Single::get_motor_mask()
|
||||
@ -351,6 +297,10 @@ uint16_t AP_MotorsHeli_Single::get_motor_mask()
|
||||
// setup fast channels
|
||||
uint32_t mask = 1U << 0 | 1U << 1 | 1U << 2 | 1U << 3 | 1U << AP_MOTORS_HELI_SINGLE_RSC;
|
||||
|
||||
if (_swashplate_type == SWASHPLATE_TYPE_H4_90 || _swashplate_type == SWASHPLATE_TYPE_H4_45) {
|
||||
mask |= 1U << 4;
|
||||
}
|
||||
|
||||
if (_tail_type == AP_MOTORS_HELI_SINGLE_TAILTYPE_SERVO_EXTGYRO) {
|
||||
mask |= 1U << AP_MOTORS_HELI_SINGLE_EXTGYRO;
|
||||
}
|
||||
@ -457,22 +407,13 @@ void AP_MotorsHeli_Single::move_actuators(float roll_out, float pitch_out, float
|
||||
float collective_scalar = ((float)(_collective_max-_collective_min))*0.001f;
|
||||
float collective_out_scaled = collective_out * collective_scalar + (_collective_min - 1000)*0.001f;
|
||||
|
||||
// Collective control direction. Swash moves up for negative collective pitch, down for positive collective pitch
|
||||
if (_collective_direction == AP_MOTORS_HELI_SINGLE_COLLECTIVE_DIRECTION_REVERSED){
|
||||
collective_out_scaled = 1 - collective_out_scaled;
|
||||
// get servo positions from swashplate library
|
||||
_servo1_out = _swashplate.get_servo_out(CH_1,pitch_out,roll_out,collective_out_scaled);
|
||||
_servo2_out = _swashplate.get_servo_out(CH_2,pitch_out,roll_out,collective_out_scaled);
|
||||
_servo3_out = _swashplate.get_servo_out(CH_3,pitch_out,roll_out,collective_out_scaled);
|
||||
if (_swashplate_type == SWASHPLATE_TYPE_H4_90 || _swashplate_type == SWASHPLATE_TYPE_H4_45) {
|
||||
_servo5_out = _swashplate.get_servo_out(CH_4,pitch_out,roll_out,collective_out_scaled);
|
||||
}
|
||||
_servo1_out = ((_rollFactor[CH_1] * roll_out) + (_pitchFactor[CH_1] * pitch_out))*0.45f + _collectiveFactor[CH_1] * collective_out_scaled;
|
||||
_servo2_out = ((_rollFactor[CH_2] * roll_out) + (_pitchFactor[CH_2] * pitch_out))*0.45f + _collectiveFactor[CH_2] * collective_out_scaled;
|
||||
if (_swash_type == AP_MOTORS_HELI_SINGLE_SWASH_H1) {
|
||||
_servo1_out += 0.5f;
|
||||
_servo2_out += 0.5f;
|
||||
}
|
||||
_servo3_out = ((_rollFactor[CH_3] * roll_out) + (_pitchFactor[CH_3] * pitch_out))*0.45f + _collectiveFactor[CH_3] * collective_out_scaled;
|
||||
|
||||
// rescale from -1..1, so we can use the pwm calc that includes trim
|
||||
_servo1_out = 2*_servo1_out - 1;
|
||||
_servo2_out = 2*_servo2_out - 1;
|
||||
_servo3_out = 2*_servo3_out - 1;
|
||||
|
||||
// update the yaw rate using the tail rotor/servo
|
||||
move_yaw(yaw_out + yaw_offset);
|
||||
@ -504,6 +445,10 @@ void AP_MotorsHeli_Single::output_to_motors()
|
||||
rc_write_swash(AP_MOTORS_MOT_1, _servo1_out);
|
||||
rc_write_swash(AP_MOTORS_MOT_2, _servo2_out);
|
||||
rc_write_swash(AP_MOTORS_MOT_3, _servo3_out);
|
||||
// get servo positions from swashplate library and write to servo for 4 servo of 4 servo swashplate
|
||||
if (_swashplate_type == SWASHPLATE_TYPE_H4_90 || _swashplate_type == SWASHPLATE_TYPE_H4_45) {
|
||||
rc_write_swash(AP_MOTORS_MOT_5, _servo5_out);
|
||||
}
|
||||
if (_tail_type != AP_MOTORS_HELI_SINGLE_TAILTYPE_DIRECTDRIVE_FIXEDPITCH){
|
||||
rc_write_angle(AP_MOTORS_MOT_4, _servo4_out * YAW_SERVO_MAX_ANGLE);
|
||||
}
|
||||
@ -605,10 +550,10 @@ void AP_MotorsHeli_Single::servo_test()
|
||||
// parameter_check - check if helicopter specific parameters are sensible
|
||||
bool AP_MotorsHeli_Single::parameter_check(bool display_msg) const
|
||||
{
|
||||
// returns false if Phase Angle is outside of range
|
||||
// returns false if Phase Angle is outside of range for H3 swashplate
|
||||
if ((_phase_angle > 30) || (_phase_angle < -30)){
|
||||
if (display_msg) {
|
||||
gcs().send_text(MAV_SEVERITY_CRITICAL, "PreArm: H_PHANG out of range");
|
||||
gcs().send_text(MAV_SEVERITY_CRITICAL, "PreArm: H_H3_PHANG out of range");
|
||||
}
|
||||
return false;
|
||||
}
|
||||
|
||||
@ -7,26 +7,13 @@
|
||||
#include <SRV_Channel/SRV_Channel.h>
|
||||
#include "AP_MotorsHeli.h"
|
||||
#include "AP_MotorsHeli_RSC.h"
|
||||
#include "AP_MotorsHeli_Swash.h"
|
||||
|
||||
// rsc and extgyro function output channels.
|
||||
#define AP_MOTORS_HELI_SINGLE_RSC CH_8
|
||||
#define AP_MOTORS_HELI_SINGLE_EXTGYRO CH_7
|
||||
#define AP_MOTORS_HELI_SINGLE_TAILRSC CH_7
|
||||
|
||||
// servo position defaults
|
||||
#define AP_MOTORS_HELI_SINGLE_SERVO1_POS -60
|
||||
#define AP_MOTORS_HELI_SINGLE_SERVO2_POS 60
|
||||
#define AP_MOTORS_HELI_SINGLE_SERVO3_POS 180
|
||||
|
||||
// swash type definitions
|
||||
#define AP_MOTORS_HELI_SINGLE_SWASH_H3 0
|
||||
#define AP_MOTORS_HELI_SINGLE_SWASH_H1 1
|
||||
#define AP_MOTORS_HELI_SINGLE_SWASH_H3_140 2
|
||||
|
||||
// collective control direction definitions
|
||||
#define AP_MOTORS_HELI_SINGLE_COLLECTIVE_DIRECTION_NORMAL 0
|
||||
#define AP_MOTORS_HELI_SINGLE_COLLECTIVE_DIRECTION_REVERSED 1
|
||||
|
||||
// tail types
|
||||
#define AP_MOTORS_HELI_SINGLE_TAILTYPE_SERVO 0
|
||||
#define AP_MOTORS_HELI_SINGLE_TAILTYPE_SERVO_EXTGYRO 1
|
||||
@ -53,7 +40,8 @@ public:
|
||||
uint16_t speed_hz = AP_MOTORS_HELI_SPEED_DEFAULT) :
|
||||
AP_MotorsHeli(loop_rate, speed_hz),
|
||||
_main_rotor(SRV_Channel::k_heli_rsc, AP_MOTORS_HELI_SINGLE_RSC),
|
||||
_tail_rotor(SRV_Channel::k_heli_tail_rsc, AP_MOTORS_HELI_SINGLE_TAILRSC)
|
||||
_tail_rotor(SRV_Channel::k_heli_tail_rsc, AP_MOTORS_HELI_SINGLE_TAILRSC),
|
||||
_swashplate()
|
||||
{
|
||||
AP_Param::setup_object_defaults(this, var_info);
|
||||
};
|
||||
@ -116,9 +104,6 @@ protected:
|
||||
// update_motor_controls - sends commands to motor controllers
|
||||
void update_motor_control(RotorControlState state) override;
|
||||
|
||||
// calculate_roll_pitch_collective_factors - calculate factors based on swash type and servo position
|
||||
void calculate_roll_pitch_collective_factors() override;
|
||||
|
||||
// heli_move_actuators - moves swash plate and tail rotor
|
||||
void move_actuators(float roll_out, float pitch_out, float coll_in, float yaw_out) override;
|
||||
|
||||
@ -131,6 +116,8 @@ protected:
|
||||
// external objects we depend upon
|
||||
AP_MotorsHeli_RSC _main_rotor; // main rotor
|
||||
AP_MotorsHeli_RSC _tail_rotor; // tail rotor
|
||||
AP_MotorsHeli_Swash _swashplate; // swashplate
|
||||
SwashInt16Param _swash_H3; // H3 servo positions for swash
|
||||
|
||||
// internal variables
|
||||
float _oscillate_angle = 0.0f; // cyclic oscillation angle, used by servo_test function
|
||||
@ -143,14 +130,12 @@ protected:
|
||||
float _servo2_out = 0.0f; // output value sent to motor
|
||||
float _servo3_out = 0.0f; // output value sent to motor
|
||||
float _servo4_out = 0.0f; // output value sent to motor
|
||||
float _servo5_out = 0.0f; // output value sent to motor
|
||||
|
||||
// parameters
|
||||
AP_Int16 _servo1_pos; // Angular location of swash servo #1
|
||||
AP_Int16 _servo2_pos; // Angular location of swash servo #2
|
||||
AP_Int16 _servo3_pos; // Angular location of swash servo #3
|
||||
AP_Int8 _collective_direction; // Collective control direction, normal or reversed
|
||||
AP_Int8 _swash_coll_dir; // Collective control direction, normal or reversed
|
||||
AP_Int16 _tail_type; // Tail type used: Servo, Servo with external gyro, direct drive variable pitch or direct drive fixed pitch
|
||||
AP_Int8 _swash_type; // Swash Type Setting
|
||||
AP_Int8 _swashplate_type; // Swash Type Setting
|
||||
AP_Int16 _ext_gyro_gain_std; // PWM sent to external gyro on ch7 when tail type is Servo w/ ExtGyro
|
||||
AP_Int16 _ext_gyro_gain_acro; // PWM sent to external gyro on ch7 when tail type is Servo w/ ExtGyro in ACRO
|
||||
AP_Int16 _phase_angle; // Phase angle correction for rotor head. If pitching the swash forward induces a roll, this can be correct the problem
|
||||
@ -159,7 +144,4 @@ protected:
|
||||
AP_Int16 _direct_drive_tailspeed; // Direct Drive VarPitch Tail ESC speed (0 ~ 1000)
|
||||
|
||||
bool _acro_tail = false;
|
||||
float _rollFactor[AP_MOTORS_HELI_SINGLE_NUM_SWASHPLATE_SERVOS];
|
||||
float _pitchFactor[AP_MOTORS_HELI_SINGLE_NUM_SWASHPLATE_SERVOS];
|
||||
float _collectiveFactor[AP_MOTORS_HELI_SINGLE_NUM_SWASHPLATE_SERVOS];
|
||||
};
|
||||
|
||||
183
libraries/AP_Motors/AP_MotorsHeli_Swash.cpp
Normal file
183
libraries/AP_Motors/AP_MotorsHeli_Swash.cpp
Normal file
@ -0,0 +1,183 @@
|
||||
/*
|
||||
This program is free software: you can redistribute it and/or modify
|
||||
it under the terms of the GNU General Public License as published by
|
||||
the Free Software Foundation, either version 3 of the License, or
|
||||
(at your option) any later version.
|
||||
|
||||
This program is distributed in the hope that it will be useful,
|
||||
but WITHOUT ANY WARRANTY; without even the implied warranty of
|
||||
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
|
||||
GNU General Public License for more details.
|
||||
|
||||
You should have received a copy of the GNU General Public License
|
||||
along with this program. If not, see <http://www.gnu.org/licenses/>.
|
||||
*/
|
||||
|
||||
#include <stdlib.h>
|
||||
#include <AP_HAL/AP_HAL.h>
|
||||
|
||||
#include "AP_MotorsHeli_Swash.h"
|
||||
|
||||
extern const AP_HAL::HAL& hal;
|
||||
|
||||
const AP_Param::GroupInfo SwashInt16Param::var_info[] = {
|
||||
|
||||
// @Param: ENABLE
|
||||
// @DisplayName: Enable settings for H3
|
||||
// @Description: Automatically set when H3 swash type is selected. Should not be set manually.
|
||||
// @Values: 0:Disabled,1:Enabled
|
||||
// @User: Advanced
|
||||
AP_GROUPINFO_FLAGS("ENABLE", 1, SwashInt16Param, enable, 0, AP_PARAM_FLAG_ENABLE),
|
||||
|
||||
// @Param: SV1_POS
|
||||
// @DisplayName: servo1_pos
|
||||
// @Description: servo 1 position
|
||||
// @Range: -180 to 180
|
||||
// @Units: deg
|
||||
// @User: Advanced
|
||||
AP_GROUPINFO("SV1_POS", 2, SwashInt16Param, servo1_pos, -60),
|
||||
|
||||
// @Param: SV2_POS
|
||||
// @DisplayName: servo2_pos
|
||||
// @Description: servo 2 position
|
||||
// @Range: -180 to 180
|
||||
// @Units: deg
|
||||
// @User: Advanced
|
||||
AP_GROUPINFO("SV2_POS", 3, SwashInt16Param, servo2_pos, 60),
|
||||
|
||||
// @Param: SV3_POS
|
||||
// @DisplayName: servo3_pos
|
||||
// @Description: servo 3 position
|
||||
// @Range: -180 to 180
|
||||
// @Units: deg
|
||||
// @User: Advanced
|
||||
AP_GROUPINFO("SV3_POS", 4, SwashInt16Param, servo3_pos, 180),
|
||||
|
||||
// @Param: PHANG
|
||||
// @DisplayName: Swashplate Phase Angle Compensation
|
||||
// @Description: Only for H3 swashplate. If pitching the swash forward induces a roll, this can be correct the problem
|
||||
// @Range: -30 30
|
||||
// @Units: deg
|
||||
// @User: Advanced
|
||||
// @Increment: 1
|
||||
AP_GROUPINFO("PHANG", 5, SwashInt16Param, phase_angle, 0),
|
||||
|
||||
AP_GROUPEND
|
||||
};
|
||||
|
||||
/*
|
||||
a manual swashplate definition with enable and servo position parameters - constructor
|
||||
*/
|
||||
SwashInt16Param::SwashInt16Param(void)
|
||||
{
|
||||
AP_Param::setup_object_defaults(this, var_info);
|
||||
}
|
||||
|
||||
// CCPM Mixers - calculate mixing scale factors by swashplate type
|
||||
void AP_MotorsHeli_Swash::calculate_roll_pitch_collective_factors()
|
||||
{
|
||||
if (_swash_type == SWASHPLATE_TYPE_H1) {
|
||||
// CCPM mixing not used
|
||||
_collectiveFactor[CH_1] = 0;
|
||||
_collectiveFactor[CH_2] = 0;
|
||||
_collectiveFactor[CH_3] = 1;
|
||||
} else if ((_swash_type == SWASHPLATE_TYPE_H4_90) || (_swash_type == SWASHPLATE_TYPE_H4_45)) {
|
||||
// collective mixer for four-servo CCPM
|
||||
_collectiveFactor[CH_1] = 1;
|
||||
_collectiveFactor[CH_2] = 1;
|
||||
_collectiveFactor[CH_3] = 1;
|
||||
_collectiveFactor[CH_4] = 1;
|
||||
} else {
|
||||
// collective mixer for three-servo CCPM
|
||||
_collectiveFactor[CH_1] = 1;
|
||||
_collectiveFactor[CH_2] = 1;
|
||||
_collectiveFactor[CH_3] = 1;
|
||||
}
|
||||
|
||||
if (_swash_type == SWASHPLATE_TYPE_H3) {
|
||||
// Three-servo roll/pitch mixer for adjustable servo position
|
||||
// can be any style swashplate, phase angle is adjustable
|
||||
_rollFactor[CH_1] = cosf(radians(_servo1_pos + 90 - _phase_angle));
|
||||
_rollFactor[CH_2] = cosf(radians(_servo2_pos + 90 - _phase_angle));
|
||||
_rollFactor[CH_3] = cosf(radians(_servo3_pos + 90 - _phase_angle));
|
||||
_pitchFactor[CH_1] = cosf(radians(_servo1_pos - _phase_angle));
|
||||
_pitchFactor[CH_2] = cosf(radians(_servo2_pos - _phase_angle));
|
||||
_pitchFactor[CH_3] = cosf(radians(_servo3_pos - _phase_angle));
|
||||
|
||||
// defined swashplates, servo1 is always left, servo2 is right,
|
||||
// servo3 is elevator
|
||||
} else if (_swash_type == SWASHPLATE_TYPE_H3_140) { //
|
||||
// Three-servo roll/pitch mixer for H3-140
|
||||
// HR3-140 uses reversed servo and collective direction in heli setup
|
||||
// 1:1 pure input style, phase angle not adjustable
|
||||
_rollFactor[CH_1] = 1;
|
||||
_rollFactor[CH_2] = -1;
|
||||
_rollFactor[CH_3] = 0;
|
||||
_pitchFactor[CH_1] = 1;
|
||||
_pitchFactor[CH_2] = 1;
|
||||
_pitchFactor[CH_3] = -1;
|
||||
} else if (_swash_type == SWASHPLATE_TYPE_H3_120) {
|
||||
// three-servo roll/pitch mixer for H3-120
|
||||
// HR3-120 uses reversed servo and collective direction in heli setup
|
||||
// not a pure mixing swashplate, phase angle is adjustable
|
||||
_rollFactor[CH_1] = 0.866025f;
|
||||
_rollFactor[CH_2] = -0.866025f;
|
||||
_rollFactor[CH_3] = 0;
|
||||
_pitchFactor[CH_1] = 0.5f;
|
||||
_pitchFactor[CH_2] = 0.5f;
|
||||
_pitchFactor[CH_3] = -1;
|
||||
} else if (_swash_type == SWASHPLATE_TYPE_H4_90) {
|
||||
// four-servo roll/pitch mixer for H4-90
|
||||
// 1:1 pure input style, phase angle not adjustable
|
||||
// servos 3 & 7 are elevator
|
||||
// can also be used for all versions of 90 deg three-servo swashplates
|
||||
_rollFactor[CH_1] = 1;
|
||||
_rollFactor[CH_2] = -1;
|
||||
_rollFactor[CH_3] = 0;
|
||||
_rollFactor[CH_4] = 0;
|
||||
_pitchFactor[CH_1] = 0;
|
||||
_pitchFactor[CH_2] = 0;
|
||||
_pitchFactor[CH_3] = -1;
|
||||
_pitchFactor[CH_4] = 1;
|
||||
} else if (_swash_type == SWASHPLATE_TYPE_H4_45) {
|
||||
// four-servo roll/pitch mixer for H4-45
|
||||
// 1:1 pure input style, phase angle not adjustable
|
||||
// for 45 deg plates servos 1&2 are LF&RF, 3&7 are LR&RR.
|
||||
_rollFactor[CH_1] = 0.707107f;
|
||||
_rollFactor[CH_2] = -0.707107f;
|
||||
_rollFactor[CH_3] = 0.707107f;
|
||||
_rollFactor[CH_4] = -0.707107f;
|
||||
_pitchFactor[CH_1] = 0.707107f;
|
||||
_pitchFactor[CH_2] = 0.707107f;
|
||||
_pitchFactor[CH_3] = -0.707f;
|
||||
_pitchFactor[CH_4] = -0.707f;
|
||||
} else {
|
||||
// CCPM mixing not being used, so H1 straight outputs
|
||||
_rollFactor[CH_1] = 1;
|
||||
_rollFactor[CH_2] = 0;
|
||||
_rollFactor[CH_3] = 0;
|
||||
_pitchFactor[CH_1] = 0;
|
||||
_pitchFactor[CH_2] = 1;
|
||||
_pitchFactor[CH_3] = 0;
|
||||
}
|
||||
}
|
||||
|
||||
// get_servo_out - calculates servo output
|
||||
float AP_MotorsHeli_Swash::get_servo_out(int8_t CH_num, float pitch, float roll, float collective)
|
||||
{
|
||||
// Collective control direction. Swash moves up for negative collective pitch, down for positive collective pitch
|
||||
if (_collective_direction == COLLECTIVE_DIRECTION_REVERSED){
|
||||
collective = 1 - collective;
|
||||
}
|
||||
|
||||
float servo = ((_rollFactor[CH_num] * roll) + (_pitchFactor[CH_num] * pitch))*0.45f + _collectiveFactor[CH_num] * collective;
|
||||
if (_swash_type == SWASHPLATE_TYPE_H1 && (CH_num == CH_1 || CH_num == CH_2)) {
|
||||
servo += 0.5f;
|
||||
}
|
||||
|
||||
// rescale from -1..1, so we can use the pwm calc that includes trim
|
||||
servo = 2.0f * servo - 1.0f;
|
||||
|
||||
return servo;
|
||||
}
|
||||
|
||||
88
libraries/AP_Motors/AP_MotorsHeli_Swash.h
Normal file
88
libraries/AP_Motors/AP_MotorsHeli_Swash.h
Normal file
@ -0,0 +1,88 @@
|
||||
/// @file AP_MotorsHeli_Swash.h
|
||||
/// @brief Swashplate Library for traditional heli
|
||||
#pragma once
|
||||
|
||||
#include <AP_Common/AP_Common.h>
|
||||
#include <AP_Math/AP_Math.h> // ArduPilot Mega Vector/Matrix math Library
|
||||
#include <AP_Param/AP_Param.h>
|
||||
|
||||
// swashplate types
|
||||
enum SwashPlateType {
|
||||
SWASHPLATE_TYPE_H3 = 0,
|
||||
SWASHPLATE_TYPE_H1,
|
||||
SWASHPLATE_TYPE_H3_140,
|
||||
SWASHPLATE_TYPE_H3_120,
|
||||
SWASHPLATE_TYPE_H4_90,
|
||||
SWASHPLATE_TYPE_H4_45
|
||||
};
|
||||
|
||||
// collective direction
|
||||
enum CollectiveDirection {
|
||||
COLLECTIVE_DIRECTION_NORMAL = 0,
|
||||
COLLECTIVE_DIRECTION_REVERSED
|
||||
};
|
||||
|
||||
class AP_MotorsHeli_Swash {
|
||||
public:
|
||||
friend class AP_MotorsHeli_Single;
|
||||
friend class AP_MotorsHeli_Dual;
|
||||
|
||||
AP_MotorsHeli_Swash() {};
|
||||
|
||||
// calculate_roll_pitch_collective_factors - calculate factors based on swash type and servo position
|
||||
void calculate_roll_pitch_collective_factors();
|
||||
|
||||
// set_swash_type - sets swashplate type
|
||||
void set_swash_type(SwashPlateType swash_type) { _swash_type = swash_type; }
|
||||
|
||||
// set_collective_direction - sets swashplate collective direction
|
||||
void set_collective_direction(CollectiveDirection collective_direction) { _collective_direction = collective_direction; }
|
||||
|
||||
// set_phase_angle - sets swashplate phase angle
|
||||
void set_phase_angle(int16_t phase_angle) { _phase_angle = phase_angle; }
|
||||
|
||||
// set_phase_angle - sets swashplate phase angle
|
||||
float get_servo_out(int8_t servo_num, float pitch, float roll, float collective);
|
||||
|
||||
void set_servo1_pos(int16_t servo_pos) { _servo1_pos = servo_pos; }
|
||||
void set_servo2_pos(int16_t servo_pos) { _servo2_pos = servo_pos; }
|
||||
void set_servo3_pos(int16_t servo_pos) { _servo3_pos = servo_pos; }
|
||||
void set_servo4_pos(int16_t servo_pos) { _servo4_pos = servo_pos; }
|
||||
|
||||
|
||||
private:
|
||||
// internal variables
|
||||
SwashPlateType _swash_type; // Swashplate type
|
||||
CollectiveDirection _collective_direction; // Collective control direction, normal or reversed
|
||||
int16_t _phase_angle; // Phase angle correction for rotor head. If pitching the swash forward induces a roll, this can be negative depending on mechanics.
|
||||
float _rollFactor[4];
|
||||
float _pitchFactor[4];
|
||||
float _collectiveFactor[4];
|
||||
int16_t _servo1_pos;
|
||||
int16_t _servo2_pos;
|
||||
int16_t _servo3_pos;
|
||||
int16_t _servo4_pos;
|
||||
|
||||
};
|
||||
class SwashInt16Param {
|
||||
public:
|
||||
SwashInt16Param(void);
|
||||
|
||||
static const struct AP_Param::GroupInfo var_info[];
|
||||
|
||||
void set_enable(int8_t setenable) {enable = setenable; }
|
||||
int8_t get_enable() { return enable; }
|
||||
int16_t get_servo1_pos() const { return servo1_pos; }
|
||||
int16_t get_servo2_pos() const { return servo2_pos; }
|
||||
int16_t get_servo3_pos() const { return servo3_pos; }
|
||||
int16_t get_phase_angle() const { return phase_angle; }
|
||||
|
||||
private:
|
||||
AP_Int8 enable;
|
||||
AP_Int16 servo1_pos;
|
||||
AP_Int16 servo2_pos;
|
||||
AP_Int16 servo3_pos;
|
||||
AP_Int16 phase_angle;
|
||||
|
||||
};
|
||||
|
||||
Loading…
Reference in New Issue
Block a user