mirror of https://github.com/ArduPilot/ardupilot
AP_Motors: tradheli-fix parameters for generic swashplate and other minor fixes
This commit is contained in:
bnsgeyer
Randy Mackay
parent
9f547cc328
commit
8c46fe1c61
|
|
@ -196,6 +196,7 @@ const AP_Param::GroupInfo AP_MotorsHeli::var_info[] = {
|
|||
// @Param: LIN_SW_SERVO
|
||||
// @DisplayName: Linearize swashplate servo mechanical throw
|
||||
// @Description: This linearizes the swashplate servo's mechanical output to account for nonlinear output due to arm rotation. This requires a specific setup procedure to work properly. The servo arm must be centered on the mechanical throw at the servo trim position and the servo trim position kept as close to 1500 as possible. Leveling the swashplate can only be done through the pitch links. See the ardupilot wiki for more details on setup.
|
||||
// @Values: 0:Disabled,1:Enabled
|
||||
// @User: Standard
|
||||
AP_GROUPINFO("LIN_SW_SERVO", 25, AP_MotorsHeli, _linear_swash_servo, 0),
|
||||
AP_GROUPEND
|
||||
|
|
|
|||
|
|
@ -15,8 +15,8 @@
|
|||
|
||||
#include <stdlib.h>
|
||||
#include <AP_HAL/AP_HAL.h>
|
||||
|
||||
#include "AP_MotorsHeli_Dual.h"
|
||||
#include <GCS_MAVLink/GCS.h>
|
||||
|
||||
extern const AP_HAL::HAL& hal;
|
||||
|
||||
|
|
@ -89,37 +89,36 @@ const AP_Param::GroupInfo AP_MotorsHeli_Dual::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_Dual, _swash1_coll_dir, (int8_t)COLLECTIVE_DIRECTION_NORMAL),
|
||||
AP_GROUPINFO("COL_CTRL_DIR", 19, AP_MotorsHeli_Dual, _swash1_coll_dir, 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),
|
||||
AP_GROUPINFO("COL_CTRL_DIR2", 20, AP_MotorsHeli_Dual, _swash2_coll_dir, COLLECTIVE_DIRECTION_NORMAL),
|
||||
|
||||
// @Param: SWASH1_TYPE
|
||||
// @DisplayName: Swashplate Type for swashplate 1
|
||||
// @Description: H3 is generic, three-servo only. H3_120/H3_140 plates have Motor1 left side, Motor2 right side, Motor3 elevator in rear. HR3_120/HR3_140 have Motor1 right side, Motor2 left side, Motor3 elevator in front - use H3_120/H3_140 and reverse servo and collective directions as necessary. For all H3_90 swashplates use H4_90 and don't use servo output for the missing servo. For H4-90 Motors1&2 are left/right respectively, Motors3&4 are rear/front respectively. For H4-45 Motors1&2 are LF/RF, Motors3&4 are LR/RR
|
||||
// @Values: 0:H3 Generic, 1:H1 non-CPPM, 2:H3_140, 3:H3_120, 4:H4_90, 5:H4_45
|
||||
// @User: Standard
|
||||
AP_GROUPINFO("SWASH1_TYPE", 21, AP_MotorsHeli_Dual, _swashplate1_type, (int8_t)SWASHPLATE_TYPE_H3),
|
||||
AP_GROUPINFO("SWASH1_TYPE", 21, AP_MotorsHeli_Dual, _swashplate1_type, SWASHPLATE_TYPE_H3),
|
||||
|
||||
// @Param: SWASH2_TYPE
|
||||
// @DisplayName: Swashplate Type for swashplate 2
|
||||
// @Description: H3 is generic, three-servo only. H3_120/H3_140 plates have Motor1 left side, Motor2 right side, Motor3 elevator in rear. HR3_120/HR3_140 have Motor1 right side, Motor2 left side, Motor3 elevator in front - use H3_120/H3_140 and reverse servo and collective directions as necessary. For all H3_90 swashplates use H4_90 and don't use servo output for the missing servo. For H4-90 Motors1&2 are left/right respectively, Motors3&4 are rear/front respectively. For H4-45 Motors1&2 are LF/RF, Motors3&4 are LR/RR
|
||||
// @Values: 0:H3 Generic, 1:H1 non-CPPM, 2:H3_140, 3:H3_120, 4:H4_90, 5:H4_45
|
||||
// @User: Standard
|
||||
AP_GROUPINFO("SWASH2_TYPE", 22, AP_MotorsHeli_Dual, _swashplate2_type, (int8_t)SWASHPLATE_TYPE_H3),
|
||||
AP_GROUPINFO("SWASH2_TYPE", 22, AP_MotorsHeli_Dual, _swashplate2_type, SWASHPLATE_TYPE_H3),
|
||||
|
||||
// @Group: SW1_H3_
|
||||
// @Path: Swash.cpp
|
||||
AP_SUBGROUPINFO(_swash1_H3, "SW1_H3_", 23, AP_MotorsHeli_Dual, SwashInt16Param),
|
||||
// @Path: AP_MotorsHeli_Swash.cpp
|
||||
AP_SUBGROUPINFO(_swashplate1, "SW1_H3_", 23, AP_MotorsHeli_Dual, AP_MotorsHeli_Swash),
|
||||
|
||||
// @Group: SW2_H3_
|
||||
// @Path: Swash.cpp
|
||||
AP_SUBGROUPINFO(_swash2_H3, "SW2_H3_", 24, AP_MotorsHeli_Dual, SwashInt16Param),
|
||||
|
||||
// @Path: AP_MotorsHeli_Swash.cpp
|
||||
AP_SUBGROUPINFO(_swashplate2, "SW2_H3_", 24, AP_MotorsHeli_Dual, AP_MotorsHeli_Swash),
|
||||
|
||||
AP_GROUPEND
|
||||
};
|
||||
|
|
@ -271,40 +270,31 @@ void AP_MotorsHeli_Dual::calculate_scalars()
|
|||
_collective2_mid_pct = ((float)(_collective2_mid-_collective2_min))/((float)(_collective2_max-_collective2_min));
|
||||
|
||||
// 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);
|
||||
if (_swashplate1.get_enable() == 0) {
|
||||
_swashplate1.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);
|
||||
if (_swashplate1.get_enable() == 1) {
|
||||
_swashplate1.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.set_swash_type(static_cast<SwashPlateType>(_swashplate1_type.get()));
|
||||
_swashplate1.set_collective_direction(static_cast<CollectiveDirection>(_swash1_coll_dir.get()));
|
||||
_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);
|
||||
if (_swashplate2.get_enable() == 0) {
|
||||
_swashplate2.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);
|
||||
if (_swashplate2.get_enable() == 1) {
|
||||
_swashplate2.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.set_swash_type(static_cast<SwashPlateType>(_swashplate2_type.get()));
|
||||
_swashplate2.set_collective_direction(static_cast<CollectiveDirection>(_swash2_coll_dir.get()));
|
||||
_swashplate2.calculate_roll_pitch_collective_factors();
|
||||
|
||||
// set mode of main rotor controller and trigger recalculation of scalars
|
||||
|
|
@ -615,3 +605,26 @@ void AP_MotorsHeli_Dual::servo_test()
|
|||
_roll_in = constrain_float(_roll_test, -1.0f, 1.0f);
|
||||
_pitch_in = constrain_float(_pitch_test, -1.0f, 1.0f);
|
||||
}
|
||||
|
||||
// parameter_check - check if helicopter specific parameters are sensible
|
||||
bool AP_MotorsHeli_Dual::parameter_check(bool display_msg) const
|
||||
{
|
||||
// returns false if Phase Angle is outside of range for H3 swashplate 1
|
||||
if (_swashplate1_type == SWASHPLATE_TYPE_H3 && (_swashplate1.get_phase_angle() > 30 || _swashplate1.get_phase_angle() < -30)){
|
||||
if (display_msg) {
|
||||
gcs().send_text(MAV_SEVERITY_CRITICAL, "PreArm: H_SW1_H3_PHANG out of range");
|
||||
}
|
||||
return false;
|
||||
}
|
||||
|
||||
// returns false if Phase Angle is outside of range for H3 swashplate 2
|
||||
if (_swashplate2_type == SWASHPLATE_TYPE_H3 && (_swashplate2.get_phase_angle() > 30 || _swashplate2.get_phase_angle() < -30)){
|
||||
if (display_msg) {
|
||||
gcs().send_text(MAV_SEVERITY_CRITICAL, "PreArm: H_SW2_H3_PHANG out of range");
|
||||
}
|
||||
return false;
|
||||
}
|
||||
|
||||
// check parent class parameters
|
||||
return AP_MotorsHeli::parameter_check(display_msg);
|
||||
}
|
||||
|
|
|
|||
|
|
@ -88,6 +88,9 @@ public:
|
|||
// servo_test - move servos through full range of movement
|
||||
void servo_test() override;
|
||||
|
||||
// parameter_check - returns true if helicopter specific parameters are sensible, used for pre-arm check
|
||||
bool parameter_check(bool display_msg) const override;
|
||||
|
||||
// var_info for holding Parameter information
|
||||
static const struct AP_Param::GroupInfo var_info[];
|
||||
|
||||
|
|
@ -99,8 +102,8 @@ protected:
|
|||
// update_motor_controls - sends commands to motor controllers
|
||||
void update_motor_control(RotorControlState state) 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);
|
||||
// get_swashplate - calculate movement 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;
|
||||
|
|
@ -109,8 +112,6 @@ protected:
|
|||
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
|
||||
|
|
@ -128,8 +129,6 @@ protected:
|
|||
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
|
||||
AP_Float _dcp_scaler; // scaling factor applied to the differential-collective-pitch
|
||||
AP_Float _dcp_yaw_effect; // feed-forward compensation to automatically add yaw input when differential collective pitch is applied.
|
||||
|
|
|
|||
|
|
@ -38,7 +38,7 @@ const AP_Param::GroupInfo AP_MotorsHeli_Single::var_info[] = {
|
|||
// @Description: H3 is generic, three-servo only. H3_120/H3_140 plates have Motor1 left side, Motor2 right side, Motor3 elevator in rear. HR3_120/HR3_140 have Motor1 right side, Motor2 left side, Motor3 elevator in front - use H3_120/H3_140 and reverse servo and collective directions as necessary. For all H3_90 swashplates use H4_90 and don't use servo output for the missing servo. For H4-90 Motors1&2 are left/right respectively, Motors3&4 are rear/front respectively. For H4-45 Motors1&2 are LF/RF, Motors3&4 are LR/RR
|
||||
// @Values: 0:H3 Generic, 1:H1 non-CPPM, 2:H3_140, 3:H3_120, 4:H4_90, 5:H4_45
|
||||
// @User: Standard
|
||||
AP_GROUPINFO("SWASH_TYPE", 5, AP_MotorsHeli_Single, _swashplate_type, (int8_t)SWASHPLATE_TYPE_H3),
|
||||
AP_GROUPINFO("SWASH_TYPE", 5, AP_MotorsHeli_Single, _swashplate_type, SWASHPLATE_TYPE_H3),
|
||||
|
||||
// @Param: GYR_GAIN
|
||||
// @DisplayName: External Gyro Gain
|
||||
|
|
@ -91,13 +91,11 @@ 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, _swash_coll_dir, (int8_t)COLLECTIVE_DIRECTION_NORMAL),
|
||||
AP_GROUPINFO("COL_CTRL_DIR", 19, AP_MotorsHeli_Single, _swash_coll_dir, 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),
|
||||
// @Group: H3_SW_
|
||||
// @Path: AP_MotorsHeli_Swash.cpp
|
||||
AP_SUBGROUPINFO(_swashplate, "SW_H3_", 20, AP_MotorsHeli_Single, AP_MotorsHeli_Swash),
|
||||
|
||||
AP_GROUPEND
|
||||
};
|
||||
|
|
@ -253,20 +251,16 @@ void AP_MotorsHeli_Single::calculate_scalars()
|
|||
|
||||
// configure swashplate and update scalars
|
||||
if (_swashplate_type == SWASHPLATE_TYPE_H3) {
|
||||
if (_swash_H3.get_enable() == 0) {
|
||||
_swash_H3.set_enable(1);
|
||||
if (_swashplate.get_enable() == 0) {
|
||||
_swashplate.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);
|
||||
if (_swashplate.get_enable() == 1) {
|
||||
_swashplate.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.set_swash_type(static_cast<SwashPlateType>(_swashplate_type.get()));
|
||||
_swashplate.set_collective_direction(static_cast<CollectiveDirection>(_swash_coll_dir.get()));
|
||||
_swashplate.calculate_roll_pitch_collective_factors();
|
||||
_swashplate.set_linear_servo_out(_linear_swash_servo);
|
||||
|
||||
|
|
@ -552,7 +546,7 @@ void AP_MotorsHeli_Single::servo_test()
|
|||
bool AP_MotorsHeli_Single::parameter_check(bool display_msg) const
|
||||
{
|
||||
// returns false if Phase Angle is outside of range for H3 swashplate
|
||||
if ((_phase_angle > 30) || (_phase_angle < -30)){
|
||||
if (_swashplate_type == SWASHPLATE_TYPE_H3 && (_swashplate.get_phase_angle() > 30 || _swashplate.get_phase_angle() < -30)){
|
||||
if (display_msg) {
|
||||
gcs().send_text(MAV_SEVERITY_CRITICAL, "PreArm: H_H3_PHANG out of range");
|
||||
}
|
||||
|
|
|
|||
|
|
@ -117,7 +117,6 @@ protected:
|
|||
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
|
||||
|
|
@ -138,7 +137,6 @@ protected:
|
|||
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
|
||||
AP_Float _collective_yaw_effect; // Feed-forward compensation to automatically add rudder input when collective pitch is increased. Can be positive or negative depending on mechanics.
|
||||
AP_Int8 _flybar_mode; // Flybar present or not. Affects attitude controller used during ACRO flight mode
|
||||
AP_Int16 _direct_drive_tailspeed; // Direct Drive VarPitch Tail ESC speed (0 ~ 1000)
|
||||
|
|
|
|||
|
|
@ -20,38 +20,38 @@
|
|||
|
||||
extern const AP_HAL::HAL& hal;
|
||||
|
||||
const AP_Param::GroupInfo SwashInt16Param::var_info[] = {
|
||||
const AP_Param::GroupInfo AP_MotorsHeli_Swash::var_info[] = {
|
||||
|
||||
// @Param: ENABLE
|
||||
// @DisplayName: Enable settings for H3
|
||||
// @Description: Automatically set when H3 swash type is selected. Should not be set manually.
|
||||
// @DisplayName: Enable generic H3 swashplate settings
|
||||
// @Description: Automatically set when H3 generic swash type is selected. Do not set manually.
|
||||
// @Values: 0:Disabled,1:Enabled
|
||||
// @User: Advanced
|
||||
AP_GROUPINFO_FLAGS("ENABLE", 1, SwashInt16Param, enable, 0, AP_PARAM_FLAG_ENABLE),
|
||||
AP_GROUPINFO_FLAGS("ENABLE", 1, AP_MotorsHeli_Swash, enable, 0, AP_PARAM_FLAG_ENABLE),
|
||||
|
||||
// @Param: SV1_POS
|
||||
// @DisplayName: servo1_pos
|
||||
// @Description: servo 1 position
|
||||
// @Range: -180 to 180
|
||||
// @DisplayName: servo 1 position
|
||||
// @Description: Azimuth position on swashplate for servo 1 with the front of the heli being 0 deg
|
||||
// @Range: -180 180
|
||||
// @Units: deg
|
||||
// @User: Advanced
|
||||
AP_GROUPINFO("SV1_POS", 2, SwashInt16Param, servo1_pos, -60),
|
||||
AP_GROUPINFO("SV1_POS", 2, AP_MotorsHeli_Swash, _servo1_pos, -60),
|
||||
|
||||
// @Param: SV2_POS
|
||||
// @DisplayName: servo2_pos
|
||||
// @Description: servo 2 position
|
||||
// @Range: -180 to 180
|
||||
// @DisplayName: servo 2 position
|
||||
// @Description: Azimuth position on swashplate for servo 2 with the front of the heli being 0 deg
|
||||
// @Range: -180 180
|
||||
// @Units: deg
|
||||
// @User: Advanced
|
||||
AP_GROUPINFO("SV2_POS", 3, SwashInt16Param, servo2_pos, 60),
|
||||
AP_GROUPINFO("SV2_POS", 3, AP_MotorsHeli_Swash, _servo2_pos, 60),
|
||||
|
||||
// @Param: SV3_POS
|
||||
// @DisplayName: servo3_pos
|
||||
// @Description: servo 3 position
|
||||
// @Range: -180 to 180
|
||||
// @DisplayName: servo 3 position
|
||||
// @Description: Azimuth position on swashplate for servo 3 with the front of the heli being 0 deg
|
||||
// @Range: -180 180
|
||||
// @Units: deg
|
||||
// @User: Advanced
|
||||
AP_GROUPINFO("SV3_POS", 4, SwashInt16Param, servo3_pos, 180),
|
||||
AP_GROUPINFO("SV3_POS", 4, AP_MotorsHeli_Swash, _servo3_pos, 180),
|
||||
|
||||
// @Param: PHANG
|
||||
// @DisplayName: Swashplate Phase Angle Compensation
|
||||
|
|
@ -60,19 +60,11 @@ const AP_Param::GroupInfo SwashInt16Param::var_info[] = {
|
|||
// @Units: deg
|
||||
// @User: Advanced
|
||||
// @Increment: 1
|
||||
AP_GROUPINFO("PHANG", 5, SwashInt16Param, phase_angle, 0),
|
||||
AP_GROUPINFO("PHANG", 5, AP_MotorsHeli_Swash, _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()
|
||||
{
|
||||
|
|
@ -163,15 +155,15 @@ void AP_MotorsHeli_Swash::calculate_roll_pitch_collective_factors()
|
|||
}
|
||||
|
||||
// get_servo_out - calculates servo output
|
||||
float AP_MotorsHeli_Swash::get_servo_out(int8_t CH_num, float pitch, float roll, float collective)
|
||||
float AP_MotorsHeli_Swash::get_servo_out(int8_t ch_num, float pitch, float roll, float collective) const
|
||||
{
|
||||
// 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)) {
|
||||
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;
|
||||
}
|
||||
|
||||
|
|
@ -185,7 +177,8 @@ float AP_MotorsHeli_Swash::get_servo_out(int8_t CH_num, float pitch, float roll,
|
|||
return servo;
|
||||
}
|
||||
|
||||
float AP_MotorsHeli_Swash::get_linear_servo_output(float input)
|
||||
// set_linear_servo_out - sets swashplate servo output to be linear
|
||||
float AP_MotorsHeli_Swash::get_linear_servo_output(float input) const
|
||||
{
|
||||
float ret;
|
||||
|
||||
|
|
|
|||
|
|
@ -27,9 +27,12 @@ public:
|
|||
friend class AP_MotorsHeli_Single;
|
||||
friend class AP_MotorsHeli_Dual;
|
||||
|
||||
AP_MotorsHeli_Swash() {};
|
||||
AP_MotorsHeli_Swash()
|
||||
{
|
||||
AP_Param::setup_object_defaults(this, var_info);
|
||||
};
|
||||
|
||||
// calculate_roll_pitch_collective_factors - calculate factors based on swash type and servo position
|
||||
// CCPM Mixers - calculate mixing scale factors by swashplate type
|
||||
void calculate_roll_pitch_collective_factors();
|
||||
|
||||
// set_swash_type - sets swashplate type
|
||||
|
|
@ -38,57 +41,41 @@ public:
|
|||
// 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; }
|
||||
// get_servo_out - calculates servo output
|
||||
float get_servo_out(int8_t servo_num, float pitch, float roll, float collective) const;
|
||||
|
||||
// set_linear_servo_out - sets swashplate servo output to be linear
|
||||
void set_linear_servo_out(int8_t linear_servo) { _make_servo_linear = linear_servo; }
|
||||
|
||||
//linearize mechanical output of swashplate servo
|
||||
float get_linear_servo_output(float input);
|
||||
// linearize mechanical output of swashplate servo
|
||||
float get_linear_servo_output(float input) const;
|
||||
|
||||
// allow parameters to be enabled
|
||||
void set_enable(int8_t setenable) {enable = setenable; }
|
||||
int8_t get_enable() { return enable; }
|
||||
|
||||
//
|
||||
int16_t get_phase_angle() const { return _phase_angle; }
|
||||
|
||||
// var_info
|
||||
static const struct AP_Param::GroupInfo var_info[];
|
||||
|
||||
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;
|
||||
int8_t _make_servo_linear;
|
||||
float _rollFactor[4]; // Roll axis scaling of servo output based on servo position
|
||||
float _pitchFactor[4]; // Pitch axis scaling of servo output based on servo position
|
||||
float _collectiveFactor[4]; // Collective axis scaling of servo output based on servo position
|
||||
int8_t _make_servo_linear; // Sets servo output to be linearized
|
||||
|
||||
};
|
||||
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:
|
||||
// parameters
|
||||
AP_Int8 enable;
|
||||
AP_Int16 servo1_pos;
|
||||
AP_Int16 servo2_pos;
|
||||
AP_Int16 servo3_pos;
|
||||
AP_Int16 phase_angle;
|
||||
AP_Int16 _servo1_pos; // servo1 azimuth position on swashplate with front of heli being 0 deg
|
||||
AP_Int16 _servo2_pos; // servo2 azimuth position on swashplate with front of heli being 0 deg
|
||||
AP_Int16 _servo3_pos; // servo3 azimuth position on swashplate with front of heli being 0 deg
|
||||
AP_Int16 _phase_angle; // Phase angle correction for rotor head. If pitching the swash forward induces
|
||||
// a roll, this can be negative depending on mechanics.
|
||||
|
||||
};
|
||||
|
||||
|
|
|
|||
Loading…
Reference in New Issue