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

TradHeli: integrate motor lib changes

This commit is contained in:
Randy Mackay 2013-11-04 20:54:47 +09:00
parent 13a412ee21
commit 46118b59d7
6 changed files with 34 additions and 36 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

24
ArduCopter/ArduCopter.pde
View File

@ -1626,7 +1626,7 @@ void update_roll_pitch_mode(void)
#if FRAME_CONFIG == HELI_FRAME #if FRAME_CONFIG == HELI_FRAME
// ACRO does not get SIMPLE mode ability // ACRO does not get SIMPLE mode ability
if (motors.flybar_mode == 1) { if (motors.has_flybar()) {
g.rc_1.servo_out = g.rc_1.control_in; g.rc_1.servo_out = g.rc_1.control_in;
g.rc_2.servo_out = g.rc_2.control_in; g.rc_2.servo_out = g.rc_2.control_in;
}else{ }else{
@ -1850,6 +1850,11 @@ bool set_throttle_mode( uint8_t new_throttle_mode )
// reset some variables used for logging // reset some variables used for logging
desired_climb_rate = 0; desired_climb_rate = 0;
nav_throttle = 0; nav_throttle = 0;
#if FRAME_CONFIG == HELI_FRAME
// use reduced collective range if in manual throttle mode
motors.set_collective_for_manual_control(throttle_mode == THROTTLE_MANUAL || throttle_mode == THROTTLE_MANUAL_TILT_COMPENSATED);
#endif
} }
// return success or failure // return success or failure
@ -1867,13 +1872,9 @@ void update_throttle_mode(void)
return; return;
#if FRAME_CONFIG == HELI_FRAME #if FRAME_CONFIG == HELI_FRAME
// check if we are flying
if (control_mode == STABILIZE){
motors.stab_throttle = true;
} else {
motors.stab_throttle = false;
}
check_dynamic_flight(); check_dynamic_flight();
// allow swash collective to move if we are in manual throttle modes, even if disarmed // allow swash collective to move if we are in manual throttle modes, even if disarmed
if( !motors.armed() ) { if( !motors.armed() ) {
if ( !(throttle_mode == THROTTLE_MANUAL) && !(throttle_mode == THROTTLE_MANUAL_TILT_COMPENSATED)){ if ( !(throttle_mode == THROTTLE_MANUAL) && !(throttle_mode == THROTTLE_MANUAL_TILT_COMPENSATED)){
@ -1881,7 +1882,6 @@ void update_throttle_mode(void)
return; return;
} }
} }
#else // HELI_FRAME #else // HELI_FRAME
// do not run throttle controllers if motors disarmed // do not run throttle controllers if motors disarmed
@ -1907,7 +1907,7 @@ void update_throttle_mode(void)
// update estimate of throttle cruise // update estimate of throttle cruise
#if FRAME_CONFIG == HELI_FRAME #if FRAME_CONFIG == HELI_FRAME
update_throttle_cruise(motors.coll_out); update_throttle_cruise(motors.get_collective_out());
#else #else
update_throttle_cruise(pilot_throttle_scaled); update_throttle_cruise(pilot_throttle_scaled);
// check if we've taken off yet // check if we've taken off yet
@ -1932,7 +1932,7 @@ void update_throttle_mode(void)
// update estimate of throttle cruise // update estimate of throttle cruise
#if FRAME_CONFIG == HELI_FRAME #if FRAME_CONFIG == HELI_FRAME
update_throttle_cruise(motors.coll_out); update_throttle_cruise(motors.get_collective_out());
#else #else
update_throttle_cruise(pilot_throttle_scaled); update_throttle_cruise(pilot_throttle_scaled);
if (!ap.takeoff_complete && motors.armed()) { if (!ap.takeoff_complete && motors.armed()) {
@ -1997,7 +1997,7 @@ void update_throttle_mode(void)
}else{ }else{
#if FRAME_CONFIG == HELI_FRAME #if FRAME_CONFIG == HELI_FRAME
// collective pitch should not be full negative to avoid harshing the mechanics. Use Stab Coll Min. // collective pitch should not be full negative to avoid harshing the mechanics. Use Stab Coll Min.
set_throttle_out(motors.stab_col_min*10, false); set_throttle_out(motors.get_manual_collective_min(), false);
#else #else
// pilot's throttle must be at zero so keep motors off // pilot's throttle must be at zero so keep motors off
set_throttle_out(0, false); set_throttle_out(0, false);
@ -2212,7 +2212,7 @@ static void tuning(){
#if FRAME_CONFIG == HELI_FRAME #if FRAME_CONFIG == HELI_FRAME
case CH6_HELI_EXTERNAL_GYRO: case CH6_HELI_EXTERNAL_GYRO:
motors.ext_gyro_gain = tuning_value; motors.ext_gyro_gain(tuning_value);
break; break;
#endif #endif

14
ArduCopter/Attitude.pde
View File

@ -83,7 +83,7 @@ get_stabilize_yaw(int32_t target_angle)
// do not use rate controllers for helicotpers with external gyros // do not use rate controllers for helicotpers with external gyros
#if FRAME_CONFIG == HELI_FRAME #if FRAME_CONFIG == HELI_FRAME
if(motors.ext_gyro_enabled) { if(motors.ext_gyro_enabled()) {
g.rc_4.servo_out = constrain_int32(target_rate, -4500, 4500); g.rc_4.servo_out = constrain_int32(target_rate, -4500, 4500);
} }
#endif #endif
@ -294,7 +294,7 @@ get_roll_rate_stabilized_ef(int32_t stick_angle)
} }
#if FRAME_CONFIG == HELI_FRAME #if FRAME_CONFIG == HELI_FRAME
if (!motors.motor_runup_complete) { if (!motors.motor_runup_complete()) {
angle_error = 0; angle_error = 0;
} }
#else #else
@ -335,7 +335,7 @@ get_pitch_rate_stabilized_ef(int32_t stick_angle)
} }
#if FRAME_CONFIG == HELI_FRAME #if FRAME_CONFIG == HELI_FRAME
if (!motors.motor_runup_complete) { if (!motors.motor_runup_complete()) {
angle_error = 0; angle_error = 0;
} }
#else #else
@ -373,7 +373,7 @@ get_yaw_rate_stabilized_ef(int32_t stick_angle)
angle_error = constrain_int32(angle_error, -MAX_YAW_OVERSHOOT, MAX_YAW_OVERSHOOT); angle_error = constrain_int32(angle_error, -MAX_YAW_OVERSHOOT, MAX_YAW_OVERSHOOT);
#if FRAME_CONFIG == HELI_FRAME #if FRAME_CONFIG == HELI_FRAME
if (!motors.motor_runup_complete) { if (!motors.motor_runup_complete()) {
angle_error = 0; angle_error = 0;
} }
#else #else
@ -443,7 +443,7 @@ void
run_rate_controllers() run_rate_controllers()
{ {
#if FRAME_CONFIG == HELI_FRAME // helicopters only use rate controllers for yaw and only when not using an external gyro #if FRAME_CONFIG == HELI_FRAME // helicopters only use rate controllers for yaw and only when not using an external gyro
if(!motors.ext_gyro_enabled) { if(!motors.ext_gyro_enabled()) {
heli_integrated_swash_controller(roll_rate_target_bf, pitch_rate_target_bf); heli_integrated_swash_controller(roll_rate_target_bf, pitch_rate_target_bf);
g.rc_4.servo_out = get_heli_rate_yaw(yaw_rate_target_bf); g.rc_4.servo_out = get_heli_rate_yaw(yaw_rate_target_bf);
} }
@ -780,7 +780,7 @@ static int16_t get_angle_boost(int16_t throttle)
{ {
float angle_boost_factor = cos_pitch_x * cos_roll_x; float angle_boost_factor = cos_pitch_x * cos_roll_x;
angle_boost_factor = 1.0f - constrain_float(angle_boost_factor, .5f, 1.0f); angle_boost_factor = 1.0f - constrain_float(angle_boost_factor, .5f, 1.0f);
int16_t throttle_above_mid = max(throttle - motors.throttle_mid,0); int16_t throttle_above_mid = max(throttle - motors.get_collective_mid(),0);
// to allow logging of angle boost // to allow logging of angle boost
angle_boost = throttle_above_mid*angle_boost_factor; angle_boost = throttle_above_mid*angle_boost_factor;
@ -900,7 +900,7 @@ get_throttle_accel(int16_t z_target_accel)
} else { } else {
// Avoid harshing the mechanics pushing into the ground // Avoid harshing the mechanics pushing into the ground
// stab_col_min should gently push down on the ground // stab_col_min should gently push down on the ground
output = constrain_float(p+i+d+g.throttle_cruise, motors.stab_col_min*10, motors.stab_col_max*10); output = constrain_float(p+i+d+g.throttle_cruise, motors.get_manual_collective_min(), motors.get_manual_collective_max());
} }
#else #else
output = constrain_float(p+i+d+g.throttle_cruise, g.throttle_min, g.throttle_max); output = constrain_float(p+i+d+g.throttle_cruise, g.throttle_min, g.throttle_max);

2
ArduCopter/Log.pde
View File

@ -271,7 +271,7 @@ static void Log_Write_Motors()
motors.motor_out[AP_MOTORS_MOT_2], motors.motor_out[AP_MOTORS_MOT_2],
motors.motor_out[AP_MOTORS_MOT_3], motors.motor_out[AP_MOTORS_MOT_3],
motors.motor_out[AP_MOTORS_MOT_4]}, motors.motor_out[AP_MOTORS_MOT_4]},
ext_gyro_gain : motors.ext_gyro_gain ext_gyro_gain : motors.ext_gyro_gain()
#elif FRAME_CONFIG == TRI_FRAME #elif FRAME_CONFIG == TRI_FRAME
motor_out : {motors.motor_out[AP_MOTORS_MOT_1], motor_out : {motors.motor_out[AP_MOTORS_MOT_1],
motors.motor_out[AP_MOTORS_MOT_2], motors.motor_out[AP_MOTORS_MOT_2],

6
ArduCopter/heli.pde
View File

@ -20,7 +20,7 @@ static struct {
// should be called at 50hz // should be called at 50hz
static void check_dynamic_flight(void) static void check_dynamic_flight(void)
{ {
if (!motors.armed() || throttle_mode == THROTTLE_LAND || !motors.motor_runup_complete) { if (!motors.armed() || throttle_mode == THROTTLE_LAND || !motors.motor_runup_complete()) {
heli_dynamic_flight_counter = 0; heli_dynamic_flight_counter = 0;
heli_flags.dynamic_flight = false; heli_flags.dynamic_flight = false;
return; return;
@ -89,7 +89,7 @@ static void heli_integrated_swash_controller(int32_t target_roll_rate, int32_t t
if (roll_pid_saturated){ if (roll_pid_saturated){
roll_i = g.pid_rate_roll.get_integrator(); // Locked Integrator due to PID saturation on previous cycle roll_i = g.pid_rate_roll.get_integrator(); // Locked Integrator due to PID saturation on previous cycle
} else { } else {
if (motors.flybar_mode == 1) { // Mechanical Flybars get regular integral for rate auto trim if (motors.has_flybar()) { // Mechanical Flybars get regular integral for rate auto trim
if (target_roll_rate > -50 && target_roll_rate < 50){ // Frozen at high rates if (target_roll_rate > -50 && target_roll_rate < 50){ // Frozen at high rates
roll_i = g.pid_rate_roll.get_i(roll_rate_error, G_Dt); roll_i = g.pid_rate_roll.get_i(roll_rate_error, G_Dt);
} else { } else {
@ -107,7 +107,7 @@ static void heli_integrated_swash_controller(int32_t target_roll_rate, int32_t t
if (pitch_pid_saturated){ if (pitch_pid_saturated){
pitch_i = g.pid_rate_pitch.get_integrator(); // Locked Integrator due to PID saturation on previous cycle pitch_i = g.pid_rate_pitch.get_integrator(); // Locked Integrator due to PID saturation on previous cycle
} else { } else {
if (motors.flybar_mode == 1) { // Mechanical Flybars get regular integral for rate auto trim if (motors.has_flybar()) { // Mechanical Flybars get regular integral for rate auto trim
if (target_pitch_rate > -50 && target_pitch_rate < 50){ // Frozen at high rates if (target_pitch_rate > -50 && target_pitch_rate < 50){ // Frozen at high rates
pitch_i = g.pid_rate_pitch.get_i(pitch_rate_error, G_Dt); pitch_i = g.pid_rate_pitch.get_i(pitch_rate_error, G_Dt);
} else { } else {

13
ArduCopter/motors.pde
View File

@ -34,12 +34,13 @@ static void arm_motors_check()
return; return;
} }
#if FRAME_CONFIG == HELI_FRAME #if FRAME_CONFIG == HELI_FRAME
if ((motors.rsc_mode > 0) && (g.rc_8.control_in >= 10)){ // heli specific arming check
arming_counter = 0; if (!motors.allow_arming()){
return; arming_counter = 0;
} return;
#endif // HELI_FRAME }
#endif // HELI_FRAME
int16_t tmp = g.rc_4.control_in; int16_t tmp = g.rc_4.control_in;

11
ArduCopter/system.pde
View File

@ -170,11 +170,6 @@ static void init_ardupilot()
} }
#endif #endif
#if FRAME_CONFIG == HELI_FRAME
motors.servo_manual = false;
motors.init_swash(); // heli initialisation
#endif
init_rc_in(); // sets up rc channels from radio init_rc_in(); // sets up rc channels from radio
init_rc_out(); // sets up motors and output to escs init_rc_out(); // sets up motors and output to escs
@ -498,13 +493,15 @@ static void update_auto_armed()
#if FRAME_CONFIG == HELI_FRAME #if FRAME_CONFIG == HELI_FRAME
// for tradheli if motors are armed and throttle is above zero and the motor is started, auto_armed should be true // for tradheli if motors are armed and throttle is above zero and the motor is started, auto_armed should be true
if(motors.armed() && g.rc_3.control_in != 0 && motors.motor_runup_complete) { if(motors.armed() && g.rc_3.control_in != 0 && motors.motor_runup_complete()) {
set_auto_armed(true);
}
#else #else
// if motors are armed and throttle is above zero auto_armed should be true // if motors are armed and throttle is above zero auto_armed should be true
if(motors.armed() && g.rc_3.control_in != 0) { if(motors.armed() && g.rc_3.control_in != 0) {
#endif // HELI_FRAME
set_auto_armed(true); set_auto_armed(true);
} }
#endif // HELI_FRAME
} }
} }