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

TradHeli: add RSC_RUNUP_TIME param and rotor speed estimate

This commit is contained in:
Randy Mackay 2013-11-09 13:38:33 +09:00
parent bc4dba0eea
commit b71c6bfd76
2 changed files with 49 additions and 20 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

59
libraries/AP_Motors/AP_MotorsHeli.cpp
View File

@ -199,12 +199,20 @@ const AP_Param::GroupInfo AP_MotorsHeli::var_info[] PROGMEM = {
// @Param: RSC_RAMP_TIME
// @DisplayName: RSC Ramp Time
// @Description: Time in seconds for the main rotor to reach full speed
// @Description: Time in seconds for the output to the main rotor's ESC to reach full speed
// @Range: 0 60
// @Units: Seconds
// @User: Standard
AP_GROUPINFO("RSC_RAMP_TIME", 22, AP_MotorsHeli,_rsc_ramp_time, AP_MOTORS_HELI_RSC_RAMP_TIME),
// @Param: RSC_RUNUP_TIME
// @DisplayName: RSC Runup Time
// @Description: Time in seconds for the main rotor to reach full speed. Must be longer than RSC_RAMP_TIME
// @Range: 0 60
// @Units: Seconds
// @User: Standard
AP_GROUPINFO("RSC_RUNUP_TIME", 23, AP_MotorsHeli,_rsc_runup_time, AP_MOTORS_HELI_RSC_RUNUP_TIME),
// @Param: TAIL_SPEED
// @DisplayName: Direct Drive VarPitch Tail ESC speed
// @Description: Direct Drive VarPitch Tail ESC speed. Only used when TailType is DirectDrive VarPitch
@ -387,6 +395,14 @@ void AP_MotorsHeli::recalc_scalers()
}
_rsc_ramp_increment = 1000.0f / (_rsc_ramp_time * 100.0f);
// recalculate rotor runup increment
if (_rsc_runup_time <= 0 ) {
_rsc_runup_time = 1;
}
if (_rsc_runup_time < _rsc_ramp_time) {
_rsc_runup_time = _rsc_ramp_time;
}
_rsc_runup_increment = 1000.0f / (_rsc_runup_time * 100.0f);
}
//
@ -674,6 +690,7 @@ void AP_MotorsHeli::rsc_control()
// shut down main rotor
if (_rsc_mode != AP_MOTORS_HELI_RSC_MODE_NONE) {
_rotor_out = 0;
_rotor_speed_estimate = 0;
write_rsc(_rotor_out);
}
return;
@ -691,7 +708,7 @@ void AP_MotorsHeli::rsc_control()
// shutting down main rotor
rotor_ramp(0);
// if completed shutting down main motor then shut-down tail rotor. Note: this does nothing if not using direct drive vairable pitch tail
if (_rotor_out == 0) {
if (_rotor_speed_estimate <= 0) {
tail_ramp(0);
}
}
@ -699,7 +716,7 @@ void AP_MotorsHeli::rsc_control()
// direct drive fixed pitch tail servo gets copy of yaw servo out (ch4) while main rotor is running
if (_tail_type == AP_MOTORS_HELI_TAILTYPE_DIRECTDRIVE_FIXEDPITCH) {
// output fixed-pitch speed control if Ch8 is high
if (_rotor_desired > 0 || _rotor_out > 0) {
if (_rotor_desired > 0 || _rotor_speed_estimate > 0) {
// copy yaw output to tail esc
write_aux(_servo_4->servo_out);
}else{
@ -712,8 +729,6 @@ void AP_MotorsHeli::rsc_control()
// result put in _rotor_out and sent to ESC
void AP_MotorsHeli::rotor_ramp(int16_t rotor_target)
{
bool ramp_down = false; // true if we are ramping down so that we immediately output the target to the rotor ESC
// return immediately if not ramping required
if (_rsc_mode == AP_MOTORS_HELI_RSC_MODE_NONE) {
_rotor_out = rotor_target;
@ -723,36 +738,44 @@ void AP_MotorsHeli::rotor_ramp(int16_t rotor_target)
// range check rotor_target
rotor_target = constrain_int16(rotor_target,0,1000);
// ramp towards target
// initialise rotor_out to our estimated rotor speed
_rotor_out = _rotor_speed_estimate;
// ramp rotor esc output towards target
if (_rotor_out < rotor_target) {
// ramp up slowly to target
_rotor_out += _rsc_ramp_increment;
if (_rotor_out > rotor_target) {
_rotor_out = rotor_target;
}
}else if(_rotor_out > rotor_target) {
_rotor_out -= _rsc_ramp_increment;
if (_rotor_out < rotor_target) {
}else{
// ramping down happens instantly
_rotor_out = rotor_target;
}
ramp_down = true;
// ramp rotor speed estimate towards rotor out
if (_rotor_speed_estimate < _rotor_out) {
_rotor_speed_estimate += _rsc_runup_increment;
if (_rotor_speed_estimate > _rotor_out) {
_rotor_speed_estimate = _rotor_out;
}
}else{
_rotor_speed_estimate -= _rsc_runup_increment;
if (_rotor_speed_estimate < _rotor_out) {
_rotor_speed_estimate = _rotor_out;
}
}
// set runup complete flag
if (!_heliflags.motor_runup_complete && rotor_target > 0 && _rotor_out >= rotor_target) {
if (!_heliflags.motor_runup_complete && rotor_target > 0 && _rotor_speed_estimate >= rotor_target) {
_heliflags.motor_runup_complete = true;
}
if (_heliflags.motor_runup_complete && rotor_target == 0 && _rotor_out <= 0) {
if (_heliflags.motor_runup_complete && rotor_target == 0 && _rotor_speed_estimate <= 0) {
_heliflags.motor_runup_complete = false;
}
// output to rsc servo
if (ramp_down) {
// if ramping down we immediately output the target to the motors
// the _rotor_out is just an estimate of how fast the rotor is actually spinning
write_rsc(rotor_target);
}else{
write_rsc(_rotor_out);
}
}
// tail_ramp - ramps tail motor towards target. Only used for direct drive variable pitch tails

10
libraries/AP_Motors/AP_MotorsHeli.h
View File

@ -73,7 +73,8 @@
#define AP_MOTORS_HELI_RSC_SETPOINT 500
// default main rotor ramp up time in seconds
#define AP_MOTORS_HELI_RSC_RAMP_TIME 1 // 1 seconds (most people use exterrnal govenors so we can ramp up output to rotor quickly)
#define AP_MOTORS_HELI_RSC_RAMP_TIME 1 // 1 second to ramp output to main rotor ESC to full power (most people use exterrnal govenors so we can ramp up quickly)
#define AP_MOTORS_HELI_RSC_RUNUP_TIME 10 // 10 seconds for rotor to reach full speed
#define AP_MOTORS_HELI_TAIL_RAMP_INCREMENT 5 // 5 is 2 seconds for direct drive tail rotor to reach to full speed (5 = (2sec*100hz)/1000)
// motor run-up time default in 100th of seconds
@ -117,6 +118,8 @@ public:
_rotor_desired(0),
_rotor_out(0),
_rsc_ramp_increment(0.0f),
_rsc_runup_increment(0.0f),
_rotor_speed_estimate(0.0f),
_tail_direct_drive_out(0)
{
AP_Param::setup_object_defaults(this, var_info);
@ -265,7 +268,8 @@ private:
AP_Int16 _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_Int16 _rsc_setpoint; // rotor speed when RSC mode is set to is enabledv
AP_Int8 _rsc_mode; // Which main rotor ESC control mode is active
AP_Int8 _rsc_ramp_time; // Time in seconds to ramp up the main rotor to full speed
AP_Int8 _rsc_ramp_time; // Time in seconds for the output to the main rotor's ESC to reach full speed
AP_Int8 _rsc_runup_time; // Time in seconds for the main rotor to reach full speed. Must be longer than _rsc_ramp_time
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
@ -285,6 +289,8 @@ private:
int16_t _rotor_desired; // latest desired rotor speed from pilot
float _rotor_out; // latest output sent to the main rotor or an estimate of the rotors actual speed (whichever is higher) (0 ~ 1000)
float _rsc_ramp_increment; // the amount we can increase the rotor output during each 100hz iteration
float _rsc_runup_increment; // the amount we can increase the rotor's estimated speed during each 100hz iteration
float _rotor_speed_estimate; // estimated speed of the main rotor (0~1000)
int16_t _tail_direct_drive_out; // current ramped speed of output on ch7 when using direct drive variable pitch tail type
};