From ae87759e6da1034f1d9ccb4557d66b82fe564c49 Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Wed, 6 Nov 2013 14:51:28 +0900 Subject: [PATCH] TradHeli: add THROTTLE_MANUAL_HELI Move check_dynamic_flight to run as scheduled task --- ArduCopter/ArduCopter.pde | 57 ++++++++++++++++++++++++--------------- ArduCopter/config.h | 7 ++++- ArduCopter/defines.h | 1 + ArduCopter/flip.pde | 6 ++--- ArduCopter/system.pde | 3 ++- 5 files changed, 47 insertions(+), 27 deletions(-) diff --git a/ArduCopter/ArduCopter.pde b/ArduCopter/ArduCopter.pde index 0468be7481..22a33d8517 100644 --- a/ArduCopter/ArduCopter.pde +++ b/ArduCopter/ArduCopter.pde @@ -869,6 +869,9 @@ static const AP_Scheduler::Task scheduler_tasks[] PROGMEM = { { three_hz_loop, 33, 90 }, { compass_accumulate, 2, 420 }, { barometer_accumulate, 2, 250 }, +#if FRAME_CONFIG == HELI_FRAME + { check_dynamic_flight, 2, 100 }, +#endif { update_notify, 2, 100 }, { one_hz_loop, 100, 420 }, { crash_check, 10, 20 }, @@ -1805,6 +1808,12 @@ void update_super_simple_bearing(bool force_update) } } +// throttle_mode_manual - returns true if the throttle is directly controlled by the pilot +bool throttle_mode_manual(uint8_t thr_mode) +{ + return (thr_mode == THROTTLE_MANUAL || thr_mode == THROTTLE_MANUAL_TILT_COMPENSATED || thr_mode == THROTTLE_MANUAL_HELI); +} + // set_throttle_mode - sets the throttle mode and initialises any variables as required bool set_throttle_mode( uint8_t new_throttle_mode ) { @@ -1829,7 +1838,7 @@ bool set_throttle_mode( uint8_t new_throttle_mode ) case THROTTLE_AUTO: controller_desired_alt = get_initial_alt_hold(current_loc.alt, climb_rate); // reset controller desired altitude to current altitude wp_nav.set_desired_alt(controller_desired_alt); // same as above but for loiter controller - if ( throttle_mode <= THROTTLE_MANUAL_TILT_COMPENSATED ) { // reset the alt hold I terms if previous throttle mode was manual + if (throttle_mode_manual(throttle_mode)) { // reset the alt hold I terms if previous throttle mode was manual reset_throttle_I(); set_accel_throttle_I_from_pilot_throttle(get_pilot_desired_throttle(g.rc_3.control_in)); } @@ -1841,6 +1850,14 @@ bool set_throttle_mode( uint8_t new_throttle_mode ) controller_desired_alt = get_initial_alt_hold(current_loc.alt, climb_rate); // reset controller desired altitude to current altitude throttle_initialised = true; break; + +#if FRAME_CONFIG == HELI_FRAME + case THROTTLE_MANUAL_HELI: + throttle_accel_deactivate(); // this controller does not use accel based throttle controller + altitude_error = 0; // clear altitude error reported to GCS + throttle_initialised = true; + break; +#endif } // update the throttle mode @@ -1850,11 +1867,6 @@ bool set_throttle_mode( uint8_t new_throttle_mode ) // reset some variables used for logging desired_climb_rate = 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 @@ -1871,28 +1883,15 @@ void update_throttle_mode(void) if(ap.do_flip) // this is pretty bad but needed to flip in AP modes. return; -#if FRAME_CONFIG == HELI_FRAME - // check if we are flying - check_dynamic_flight(); - - // allow swash collective to move if we are in manual throttle modes, even if disarmed - if( !motors.armed() ) { - if ( !(throttle_mode == THROTTLE_MANUAL) && !(throttle_mode == THROTTLE_MANUAL_TILT_COMPENSATED)){ - throttle_accel_deactivate(); // do not allow the accel based throttle to override our command - return; - } - } -#else // HELI_FRAME - -// do not run throttle controllers if motors disarmed +#if FRAME_CONFIG != HELI_FRAME + // do not run throttle controllers if motors disarmed if( !motors.armed() ) { set_throttle_out(0, false); throttle_accel_deactivate(); // do not allow the accel based throttle to override our command set_target_alt_for_reporting(0); return; } - -#endif // HELI_FRAME +#endif // FRAME_CONFIG != HELI_FRAME switch(throttle_mode) { @@ -2013,6 +2012,20 @@ void update_throttle_mode(void) get_throttle_land(); set_target_alt_for_reporting(0); break; + +#if FRAME_CONFIG == HELI_FRAME + case THROTTLE_MANUAL_HELI: + // trad heli manual throttle controller + // send pilot's output directly to swash plate + pilot_throttle_scaled = motors.get_pilot_desired_collective(g.rc_3.control_in); + set_throttle_out(pilot_throttle_scaled, false); + + // update estimate of throttle cruise + update_throttle_cruise(motors.get_collective_out()); + set_target_alt_for_reporting(0); + break; +#endif // HELI_FRAME + } } diff --git a/ArduCopter/config.h b/ArduCopter/config.h index 705bb93099..07daf8c56e 100644 --- a/ArduCopter/config.h +++ b/ArduCopter/config.h @@ -111,7 +111,7 @@ # define HELI_PITCH_FF 0 # define HELI_ROLL_FF 0 # define HELI_YAW_FF 0 - # define STABILIZE_THROTTLE THROTTLE_MANUAL + # define STABILIZE_THR THROTTLE_MANUAL_HELI # define MPU6K_FILTER 10 #endif @@ -531,6 +531,11 @@ // Flight mode roll, pitch, yaw, throttle and navigation definitions +// Stabilize Mode +#ifndef STABILIZE_THR + # define STABILIZE_THR THROTTLE_MANUAL_TILT_COMPENSATED +#endif + // Acro Mode #ifndef ACRO_YAW # define ACRO_YAW YAW_ACRO diff --git a/ArduCopter/defines.h b/ArduCopter/defines.h index e3914e6313..548d719058 100644 --- a/ArduCopter/defines.h +++ b/ArduCopter/defines.h @@ -40,6 +40,7 @@ #define THROTTLE_HOLD 2 // alt hold plus pilot input of climb rate #define THROTTLE_AUTO 3 // auto pilot altitude controller with target altitude held in next_WP.alt #define THROTTLE_LAND 4 // landing throttle controller +#define THROTTLE_MANUAL_HELI 5 // pilot manually controlled throttle for traditional helicopters // sonar - for use with CONFIG_SONAR_SOURCE diff --git a/ArduCopter/flip.pde b/ArduCopter/flip.pde index e8c282251d..5d67ed9c61 100644 --- a/ArduCopter/flip.pde +++ b/ArduCopter/flip.pde @@ -35,7 +35,7 @@ void roll_flip() if (roll < 4500) { // Roll control roll_rate_target_bf = 40000 * flip_dir; - if(throttle_mode == THROTTLE_MANUAL || throttle_mode == THROTTLE_MANUAL_TILT_COMPENSATED){ + if (throttle_mode_manual(throttle_mode)){ // increase throttle right before flip set_throttle_out(g.rc_3.control_in + AAP_THR_INC, false); } @@ -52,7 +52,7 @@ void roll_flip() roll_rate_target_bf = 40000 * flip_dir; #endif // decrease throttle while inverted - if(throttle_mode == THROTTLE_MANUAL || throttle_mode == THROTTLE_MANUAL_TILT_COMPENSATED){ + if (throttle_mode_manual(throttle_mode)){ set_throttle_out(g.rc_3.control_in - AAP_THR_DEC, false); } }else{ @@ -68,7 +68,7 @@ void roll_flip() // It will be handled by normal flight control loops // increase throttle to gain any lost alitude - if(throttle_mode == THROTTLE_MANUAL || throttle_mode == THROTTLE_MANUAL_TILT_COMPENSATED){ + if (throttle_mode_manual(throttle_mode)){ set_throttle_out(g.rc_3.control_in + AAP_THR_INC, false); } flip_timer++; diff --git a/ArduCopter/system.pde b/ArduCopter/system.pde index 22faa0678f..0a39a2a7dd 100644 --- a/ArduCopter/system.pde +++ b/ArduCopter/system.pde @@ -327,6 +327,7 @@ static bool manual_flight_mode(uint8_t mode) { } // set_mode - change flight mode and perform any necessary initialisation +// optional force parameter used to force the flight mode change (used only first time mode is set) // returns true if mode was succesfully set // STABILIZE, ACRO, SPORT and LAND can always be set successfully but the return state of other flight modes should be checked and the caller should deal with failures appropriately static bool set_mode(uint8_t mode) @@ -353,7 +354,7 @@ static bool set_mode(uint8_t mode) success = true; set_yaw_mode(YAW_HOLD); set_roll_pitch_mode(ROLL_PITCH_STABLE); - set_throttle_mode(THROTTLE_MANUAL_TILT_COMPENSATED); + set_throttle_mode(STABILIZE_THR); set_nav_mode(NAV_NONE); break;