Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-02-23 00:04:02 -04:00
Code Issues Packages Projects Releases Wiki Activity

AP_Motors: Circular swash-plate limits for AP_MotorsHeli

Browse Source
This commit is contained in:
Jolyon Saunders 2014-09-19 11:54:26 +10:00 committed by Randy Mackay
parent bdbfd8fd5e
commit 0df3af4e42
3 changed files with 24 additions and 42 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

27
libraries/AP_Motors/AP_MotorsHeli.cpp
View File

@ -27,23 +27,9 @@ extern const AP_HAL::HAL& hal;
const AP_Param::GroupInfo AP_MotorsHeli::var_info[] PROGMEM = {
// @Param: ROL_MAX
// @DisplayName: Swash Roll Angle Max
// @Description: Maximum roll angle of the swash plate
// @Range: 0 18000
// @Units: Centi-Degrees
// @Increment: 100
// @User: Advanced
AP_GROUPINFO("ROL_MAX", 1, AP_MotorsHeli, _roll_max, AP_MOTORS_HELI_SWASH_ROLL_MAX),
// 1 was ROL_MAX which has been replaced by CYC_MAX
// @Param: PIT_MAX
// @DisplayName: Swash Pitch Angle Max
// @Description: Maximum pitch angle of the swash plate
// @Range: 0 18000
// @Units: Centi-Degrees
// @Increment: 100
// @User: Advanced
AP_GROUPINFO("PIT_MAX", 2, AP_MotorsHeli, _pitch_max, AP_MOTORS_HELI_SWASH_PITCH_MAX),
// 2 was PIT_MAX which has been replaced by CYC_MAX
// @Param: COL_MIN
// @DisplayName: Collective Pitch Minimum
@ -152,6 +138,15 @@ const AP_Param::GroupInfo AP_MotorsHeli::var_info[] PROGMEM = {
// @User: Standard
AP_GROUPINFO("RSC_POWER_HIGH", 15, AP_MotorsHeli, _rsc_power_high, AP_MOTORS_HELI_RSC_POWER_HIGH_DEFAULT),
// @Param: CYC_MAX
// @DisplayName: Cyclic Pitch Angle Max
// @Description: Maximum pitch angle of the swash plate
// @Range: 0 18000
// @Units: Centi-Degrees
// @Increment: 100
// @User: Advanced
AP_GROUPINFO("CYC_MAX", 16, AP_MotorsHeli, _cyclic_max, AP_MOTORS_HELI_SWASH_CYCLIC_MAX),
AP_GROUPEND
};

6
libraries/AP_Motors/AP_MotorsHeli.h
View File

@ -23,8 +23,7 @@
#define AP_MOTORS_HELI_SPEED_ANALOG_SERVOS 125 // update rate for analog servos
// default swash min and max angles and positions
#define AP_MOTORS_HELI_SWASH_ROLL_MAX 2500
#define AP_MOTORS_HELI_SWASH_PITCH_MAX 2500
#define AP_MOTORS_HELI_SWASH_CYCLIC_MAX 2500
#define AP_MOTORS_HELI_COLLECTIVE_MIN 1250
#define AP_MOTORS_HELI_COLLECTIVE_MAX 1750
#define AP_MOTORS_HELI_COLLECTIVE_MID 1500
@ -205,8 +204,7 @@ protected:
} _heliflags;
// parameters
AP_Int16 _roll_max; // Maximum roll angle of the swash plate in centi-degrees
AP_Int16 _pitch_max; // Maximum pitch angle of the swash plate in centi-degrees
AP_Int16 _cyclic_max; // Maximum cyclic angle of the swash plate in centi-degrees
AP_Int16 _collective_min; // Lowest possible servo position for the swashplate
AP_Int16 _collective_max; // Highest possible servo position for the swashplate
AP_Int16 _collective_mid; // Swash servo position corresponding to zero collective pitch (or zero lift for Assymetrical blades)

33
libraries/AP_Motors/AP_MotorsHeli_Single.cpp
View File

@ -233,8 +233,8 @@ void AP_MotorsHeli_Single::calculate_scalars()
_collective_range = 1000 - _collective_mid_pwm;
// determine roll, pitch and collective input scaling
_roll_scaler = (float)_roll_max/4500.0f;
_pitch_scaler = (float)_pitch_max/4500.0f;
_roll_scaler = (float)_cyclic_max/4500.0f;
_pitch_scaler = (float)_cyclic_max/4500.0f;
_collective_scalar = ((float)(_collective_max-_collective_min))/1000.0f;
// calculate factors based on swash type and servo position
@ -352,29 +352,18 @@ void AP_MotorsHeli_Single::move_actuators(int16_t roll_out, int16_t pitch_out, i
limit.throttle_lower = false;
limit.throttle_upper = false;
// rescale roll_out and pitch-out into the min and max ranges to provide linear motion
// rescale roll_out and pitch_out into the min and max ranges to provide linear motion
// across the input range instead of stopping when the input hits the constrain value
// these calculations are based on an assumption of the user specified roll_max and pitch_max
// these calculations are based on an assumption of the user specified cyclic_max
// coming into this equation at 4500 or less, and based on the original assumption of the
// total _servo_x.servo_out range being -4500 to 4500.
roll_out = roll_out * _roll_scaler;
if (roll_out < -_roll_max) {
roll_out = -_roll_max;
limit.roll_pitch = true;
}
if (roll_out > _roll_max) {
roll_out = _roll_max;
limit.roll_pitch = true;
}
// scale pitch and update limits
pitch_out = pitch_out * _pitch_scaler;
if (pitch_out < -_pitch_max) {
pitch_out = -_pitch_max;
limit.roll_pitch = true;
}
if (pitch_out > _pitch_max) {
pitch_out = _pitch_max;
float total_out = pythagorous2((float)pitch_out, (float)roll_out);
if (total_out > _cyclic_max) {
float ratio = (float)_cyclic_max / total_out;
roll_out *= ratio;
pitch_out *= ratio;
limit.roll_pitch = true;
}
@ -470,4 +459,4 @@ void AP_MotorsHeli_Single::write_aux(int16_t servo_out)
_servo_aux.servo_out = servo_out;
_servo_aux.calc_pwm();
hal.rcout->write(AP_MOTORS_HELI_SINGLE_AUX, _servo_aux.radio_out);
}
}