Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

TradHeli: add landing collective min

This commit is contained in:
Randy Mackay 2013-11-06 21:39:10 +09:00
parent 633e91b7d4
commit 7ae0d3320b
2 changed files with 85 additions and 8 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

85
libraries/AP_Motors/AP_MotorsHeli.cpp
View File

@ -175,8 +175,8 @@ const AP_Param::GroupInfo AP_MotorsHeli::var_info[] PROGMEM = {
// @Range: 0:NoFlybar 1:Flybar
// @User: Standard
AP_GROUPINFO("FLYBAR_MODE", 18, AP_MotorsHeli, _flybar_mode, AP_MOTORS_HELI_NOFLYBAR),
// @Param: STAB_COL_MIN
// @Param: STAB_COL_MIN
// @DisplayName: Stabilize Throttle Minimum
// @Description: Minimum collective position while pilot directly controls collective
// @Range: 0 50
@ -185,7 +185,7 @@ const AP_Param::GroupInfo AP_MotorsHeli::var_info[] PROGMEM = {
// @User: Standard
AP_GROUPINFO("STAB_COL_MIN", 19, AP_MotorsHeli, _manual_collective_min, AP_MOTORS_HELI_MANUAL_COLLECTIVE_MIN),
// @Param: STAB_COL_MAX
// @Param: STAB_COL_MAX
// @DisplayName: Stabilize Throttle Maximum
// @Description: Maximum collective position while pilot directly controls collective
// @Range: 50 100
@ -194,6 +194,15 @@ const AP_Param::GroupInfo AP_MotorsHeli::var_info[] PROGMEM = {
// @User: Standard
AP_GROUPINFO("STAB_COL_MAX", 20, AP_MotorsHeli, _manual_collective_max, AP_MOTORS_HELI_MANUAL_COLLECTIVE_MAX),
// @Param: LAND_COL_MIN
// @DisplayName: Landing Collective Minimum
// @Description: Minimum collective position while landed or landing
// @Range: 0 500
// @Units: pwm
// @Increment: 1
// @User: Standard
AP_GROUPINFO("LAND_COL_MIN", 21, AP_MotorsHeli, _land_collective_min, AP_MOTORS_HELI_LAND_COLLECTIVE_MIN),
AP_GROUPEND
};
@ -246,6 +255,12 @@ void AP_MotorsHeli::output_min()
{
// move swash to mid
move_swash(0,0,500,0);
// override limits flags
limit.roll_pitch = true;
limit.yaw = true;
limit.throttle_lower = true;
limit.throttle_upper = false;
}
@ -332,6 +347,17 @@ int16_t AP_MotorsHeli::get_pilot_desired_collective(int16_t control_in)
return collective_out;
}
// return true if the main rotor is up to speed
bool AP_MotorsHeli::motor_runup_complete()
{
// if we have no control of motors, assume pilot has spun them up
if (_rsc_mode == AP_MOTORS_HELI_RSC_MODE_NONE) {
return true;
}
return _heliflags.motor_runup_complete;
}
//
// protected methods
//
@ -486,6 +512,12 @@ void AP_MotorsHeli::move_swash(int16_t roll_out, int16_t pitch_out, int16_t coll
int16_t yaw_offset = 0;
int16_t coll_out_scaled;
// initialize limits flag
limit.roll_pitch = false;
limit.yaw = false;
limit.throttle_lower = false;
limit.throttle_upper = false;
if (_servo_manual == 1) { // are we in manual servo mode? (i.e. swash set-up mode)?
// check if we need to free up the swash
if (_heliflags.swash_initialised) {
@ -505,13 +537,44 @@ void AP_MotorsHeli::move_swash(int16_t roll_out, int16_t pitch_out, int16_t coll
// 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;
roll_out = constrain_int16(roll_out, (int16_t)-_roll_max, (int16_t)_roll_max);
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;
pitch_out = constrain_int16(pitch_out, (int16_t)-_pitch_max, (int16_t)_pitch_max);
if (pitch_out < -_pitch_max) {
pitch_out = -_pitch_max;
limit.roll_pitch = true;
}
if (pitch_out > _pitch_max) {
pitch_out = _pitch_max;
limit.roll_pitch = true;
}
// constrain collective input
_collective_out = coll_in;
if (_collective_out <= 0) {
_collective_out = 0;
limit.throttle_lower = true;
}
if (_collective_out >= 1000) {
_collective_out = 1000;
limit.throttle_upper = true;
}
// ensure not below landed/landing collective
if (_heliflags.landing_collective && _collective_out < _land_collective_min) {
_collective_out = _land_collective_min;
limit.throttle_lower = true;
}
// scale collective pitch
_collective_out = constrain_int16(coll_in, 0, 1000);
coll_out_scaled = _collective_out * _collective_scalar + _collective_min - 1000;
// rudder feed forward based on collective
@ -530,6 +593,16 @@ void AP_MotorsHeli::move_swash(int16_t roll_out, int16_t pitch_out, int16_t coll
_servo_3->servo_out = (_rollFactor[CH_3] * roll_out + _pitchFactor[CH_3] * pitch_out)/10 + _collectiveFactor[CH_3] * coll_out_scaled + (_servo_3->radio_trim-1500);
_servo_4->servo_out = yaw_out + yaw_offset;
// constrain yaw and update limits
if (_servo_4->servo_out < -4500) {
_servo_4->servo_out = -4500;
limit.yaw = true;
}
if (_servo_4->servo_out > 4500) {
_servo_4->servo_out = 4500;
limit.yaw = true;
}
// use servo_out to calculate pwm_out and radio_out
_servo_1->calc_pwm();
_servo_2->calc_pwm();

8
libraries/AP_Motors/AP_MotorsHeli.h
View File

@ -40,10 +40,13 @@
#define AP_MOTORS_HELI_COLLECTIVE_MAX 1750
#define AP_MOTORS_HELI_COLLECTIVE_MID 1500
// swash min and max position while in stabilize mode (as a number from 0 ~ 1000)
// swash min and max position while in stabilize mode (as a number from 0 ~ 100)
#define AP_MOTORS_HELI_MANUAL_COLLECTIVE_MIN 0
#define AP_MOTORS_HELI_MANUAL_COLLECTIVE_MAX 100
// swash min while landed or landing (as a number from 0 ~ 1000
#define AP_MOTORS_HELI_LAND_COLLECTIVE_MIN 0
// default external gyro gain (ch7 out)
#define AP_MOTORS_HELI_EXT_GYRO_GAIN 1350
@ -155,7 +158,7 @@ public:
void set_collective_for_landing(bool landing) { _heliflags.landing_collective = landing; }
// return true if the main rotor is up to speed
bool motor_runup_complete() { return _heliflags.motor_runup_complete; }
bool motor_runup_complete();
// var_info for holding Parameter information
static const struct AP_Param::GroupInfo var_info[];
@ -218,6 +221,7 @@ private:
AP_Int8 _flybar_mode; // Flybar present or not. Affects attitude controller used during ACRO flight mode
AP_Int8 _manual_collective_min; // Minimum collective position while pilot directly controls the collective
AP_Int8 _manual_collective_max; // Maximum collective position while pilot directly controls the collective
AP_Int16 _land_collective_min; // Minimum collective when landed or landing
// internal variables
float _rollFactor[AP_MOTORS_HELI_NUM_SWASHPLATE_SERVOS];