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

TradHeli: move pilot desired rotor speed to heli.pde

This commit is contained in:
Randy Mackay 2013-11-08 17:29:54 +09:00
parent 527f05ddec
commit 197fc67897
4 changed files with 53 additions and 59 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

3
ArduCopter/ArduCopter.pde
View File

@ -1086,6 +1086,9 @@ static void throttle_loop()
update_auto_armed();
#if FRAME_CONFIG == HELI_FRAME
// update rotor speed
heli_update_rotor_speed_targets();
// update trad heli swash plate movement
heli_update_landing_swash();
#endif

25
ArduCopter/heli.pde
View File

@ -225,4 +225,29 @@ static void heli_update_landing_swash()
}
}
// heli_update_rotor_speed_targets - reads pilot input and passes new rotor speed targets to heli motors object
static void heli_update_rotor_speed_targets()
{
// get rotor control method
uint8_t rsc_control_mode = motors.get_rsc_mode();
switch (rsc_control_mode) {
case AP_MOTORS_HELI_RSC_MODE_NONE:
// even though pilot passes rotors speed directly to rotor ESC via receiver, motor lib needs to know if
// rotor is spinning in case we are using direct drive tailrotor which must be spun up at same time
case AP_MOTORS_HELI_RSC_MODE_CH8_PASSTHROUGH:
// pass through pilot desired rotor speed
motors.set_desired_rotor_speed(g.rc_8.control_in);
break;
case AP_MOTORS_HELI_RSC_MODE_SETPOINT:
// pass setpoint through as desired rotor speed
if (g.rc_8.control_in > 0) {
motors.set_desired_rotor_speed(motors.get_rsc_setpoint());
}else{
motors.set_desired_rotor_speed(0);
}
break;
}
}
#endif // FRAME_CONFIG == HELI_FRAME

69
libraries/AP_Motors/AP_MotorsHeli.cpp
View File

@ -361,6 +361,12 @@ int16_t AP_MotorsHeli::get_pilot_desired_collective(int16_t control_in)
return collective_out;
}
// set_desired_rotor_speed - sets target rotor speed as a number from 0 ~ 1000
void AP_MotorsHeli::set_desired_rotor_speed(int16_t desired_speed)
{
_rotor_desired = desired_speed;
}
// return true if the main rotor is up to speed
bool AP_MotorsHeli::motor_runup_complete()
{
@ -398,7 +404,6 @@ void AP_MotorsHeli::output_armed()
_rc_yaw->servo_out = _rc_yaw->control_in;
}
//static int counter = 0;
_rc_roll->calc_pwm();
_rc_pitch->calc_pwm();
_rc_throttle->calc_pwm();
@ -406,8 +411,8 @@ void AP_MotorsHeli::output_armed()
move_swash( _rc_roll->servo_out, _rc_pitch->servo_out, _rc_throttle->servo_out, _rc_yaw->servo_out );
// To-Do: clean-up the passing in of the desired rotor speed
rsc_control(_servo_rsc->control_in);
// update rotor and direct drive esc speeds
rsc_control();
}
// output_disarmed - sends commands to the motors
@ -657,7 +662,7 @@ void AP_MotorsHeli::move_swash(int16_t roll_out, int16_t pitch_out, int16_t coll
// rsc_control - update value to send to tail and main rotor's ESC
// desired_rotor_speed is a desired speed from 0 to 1000
void AP_MotorsHeli::rsc_control(int16_t desired_rotor_speed)
void AP_MotorsHeli::rsc_control()
{
// if disarmed output minimums
if (!armed()) {
@ -674,63 +679,13 @@ void AP_MotorsHeli::rsc_control(int16_t desired_rotor_speed)
return;
}
// handle simpler case of pilot directly controlling main rotor
if (_rsc_mode == AP_MOTORS_HELI_RSC_MODE_NONE) {
switch (_tail_type) {
// return immediately if no direct drive tail motor
case AP_MOTORS_HELI_TAILTYPE_SERVO:
case AP_MOTORS_HELI_TAILTYPE_SERVO_EXTGYRO:
default:
return;
break;
// direct drive variable pitch tail
case AP_MOTORS_HELI_TAILTYPE_DIRECTDRIVE_VARPITCH:
if (desired_rotor_speed > 0) {
// ramp up tail if main rotor on
tail_ramp(_direct_drive_setpoint);
}else{
// ramp down tail rotor if main rotor off
tail_ramp(0);
}
return;
break;
// direct drive fixed pitch tail
case AP_MOTORS_HELI_TAILTYPE_DIRECTDRIVE_FIXEDPITCH:
// output fixed-pitch speed control if Ch8 is high
if (desired_rotor_speed > 0) {
// copy yaw output to tail esc
write_aux(_servo_4->servo_out);
}else{
write_aux(0);
}
return;
break;
}
return;
}
// handle more complex case of channel 8 passthrough and external governor
int16_t temp_rotor_speed;
if (_rsc_mode == AP_MOTORS_HELI_RSC_MODE_CH8_PASSTHROUGH) {
temp_rotor_speed = desired_rotor_speed;
}else{
// rotor speed is predefined set-point
if (desired_rotor_speed > 0) {
temp_rotor_speed = _rsc_setpoint;
}else{
temp_rotor_speed = 0;
}
}
// ramp up or down main rotor and tail
if (temp_rotor_speed > 0) {
if (_rotor_desired > 0) {
// ramp up tail rotor (this does nothing if not using direct drive variable pitch tail)
tail_ramp(_direct_drive_tailspeed);
// note: this always returns true if not using direct drive variable pitch tail
if (tail_rotor_runup_complete()) {
rotor_ramp(temp_rotor_speed);
rotor_ramp(_rotor_desired);
}
}else{
// shutting down main rotor
@ -744,7 +699,7 @@ void AP_MotorsHeli::rsc_control(int16_t desired_rotor_speed)
// 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 (temp_rotor_speed > 0 || _rotor_out > 0) {
if (_rotor_desired > 0 || _rotor_out > 0) {
// copy yaw output to tail esc
write_aux(_servo_4->servo_out);
}else{

15
libraries/AP_Motors/AP_MotorsHeli.h
View File

@ -114,6 +114,7 @@ public:
_collective_scalar_manual(1),
_collective_out(0),
_collective_mid_pwm(0),
_rotor_desired(0),
_rotor_out(0),
_rsc_ramp_increment(0.0f),
_tail_direct_drive_out(0)
@ -175,6 +176,15 @@ public:
// set_collective_for_landing - limits collective from going too low if we know we are landed
void set_collective_for_landing(bool landing) { _heliflags.landing_collective = landing; }
// get_rsc_mode - gets the rotor speed control method (AP_MOTORS_HELI_RSC_MODE_NONE, AP_MOTORS_HELI_RSC_MODE_CH8_PASSTHROUGH or AP_MOTORS_HELI_RSC_MODE_SETPOINT)
uint8_t get_rsc_mode() { return _rsc_mode; }
// get_rsc_setpoint - gets contents of _rsc_setpoint parameter (0~1000)
int16_t get_rsc_setpoint() { return _rsc_setpoint; }
// set_desired_rotor_speed - sets target rotor speed as a number from 0 ~ 1000
void set_desired_rotor_speed(int16_t desired_speed);
// return true if the main rotor is up to speed
bool motor_runup_complete();
@ -205,7 +215,7 @@ private:
void calculate_roll_pitch_collective_factors();
// rsc_control - main function to update values to send to main rotor and tail rotor ESCs
void rsc_control(int16_t desired_rotor_speed);
void rsc_control();
// rotor_ramp - ramps rotor towards target. result put rotor_out and sent to ESC
void rotor_ramp(int16_t rotor_target);
@ -272,7 +282,8 @@ private:
float _collective_scalar_manual; // collective scalar to reduce the range of the collective movement while collective is being controlled manually (i.e. directly by the pilot)
int16_t _collective_out; // actual collective pitch value. Required by the main code for calculating cruise throttle
int16_t _collective_mid_pwm; // collective mid parameter value converted to pwm form (i.e. 0 ~ 1000)
float _rotor_out; // output to the rotor (0 ~ 1000)
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
int16_t _tail_direct_drive_out; // current ramped speed of output on ch7 when using direct drive variable pitch tail type
};