From c812d079930c9e421de48ba5986faed738f27b49 Mon Sep 17 00:00:00 2001 From: Robert Lefebvre Date: Fri, 5 Jul 2013 14:56:03 -0400 Subject: [PATCH] TradHeli: Creating motor_runup_complete bool which is to confirm that the heli motor is running. This will be used for advanced features in the future. --- ArduCopter/Attitude.pde | 18 ++++++++++++++++++ libraries/AP_Motors/AP_MotorsHeli.cpp | 13 +++++++++++++ libraries/AP_Motors/AP_MotorsHeli.h | 6 +++++- 3 files changed, 36 insertions(+), 1 deletion(-) diff --git a/ArduCopter/Attitude.pde b/ArduCopter/Attitude.pde index 5415c43d75..b76faece99 100644 --- a/ArduCopter/Attitude.pde +++ b/ArduCopter/Attitude.pde @@ -259,10 +259,16 @@ get_roll_rate_stabilized_ef(int32_t stick_angle) angle_error = constrain_int32(angle_error, -MAX_ROLL_OVERSHOOT, MAX_ROLL_OVERSHOOT); } +#if FRAME_CONFIG == HELI_FRAME + if (!motors.motor_runup_complete) { + angle_error = 0; + } +#else // reset target angle to current angle if motors not spinning if (!motors.armed() || g.rc_3.servo_out == 0) { angle_error = 0; } +#endif // HELI_FRAME // update roll_axis to be within max_angle_overshoot of our current heading roll_axis = wrap_180_cd(angle_error + ahrs.roll_sensor); @@ -294,10 +300,16 @@ get_pitch_rate_stabilized_ef(int32_t stick_angle) angle_error = constrain_int32(angle_error, -MAX_PITCH_OVERSHOOT, MAX_PITCH_OVERSHOOT); } +#if FRAME_CONFIG == HELI_FRAME + if (!motors.motor_runup_complete) { + angle_error = 0; + } +#else // reset target angle to current angle if motors not spinning if (!motors.armed() || g.rc_3.servo_out == 0) { angle_error = 0; } +#endif // HELI_FRAME // update pitch_axis to be within max_angle_overshoot of our current heading pitch_axis = wrap_180_cd(angle_error + ahrs.pitch_sensor); @@ -326,10 +338,16 @@ get_yaw_rate_stabilized_ef(int32_t stick_angle) // limit the maximum overshoot angle_error = constrain_int32(angle_error, -MAX_YAW_OVERSHOOT, MAX_YAW_OVERSHOOT); +#if FRAME_CONFIG == HELI_FRAME + if (!motors.motor_runup_complete) { + angle_error = 0; + } +#else // reset target angle to current heading if motors not spinning if (!motors.armed() || g.rc_3.servo_out == 0) { angle_error = 0; } +#endif // HELI_FRAME // update nav_yaw to be within max_angle_overshoot of our current heading nav_yaw = wrap_360_cd(angle_error + ahrs.yaw_sensor); diff --git a/libraries/AP_Motors/AP_MotorsHeli.cpp b/libraries/AP_Motors/AP_MotorsHeli.cpp index 821dc4a304..419c0d5137 100644 --- a/libraries/AP_Motors/AP_MotorsHeli.cpp +++ b/libraries/AP_Motors/AP_MotorsHeli.cpp @@ -532,6 +532,19 @@ static long map(long x, long in_min, long in_max, long out_min, long out_max) void AP_MotorsHeli::rsc_control() { + + if (armed() && (rsc_ramp >= rsc_ramp_up_rate)){ // rsc_ramp will never increase if rsc_mode = 0 + if (motor_runup_timer < MOTOR_RUNUP_TIME){ // therefore motor_runup_complete can never be true + motor_runup_timer++; + } else { + motor_runup_complete = true; + } + } else { + motor_runup_complete = false; // motor_runup_complete will go to false if we + motor_runup_timer = 0; // disarm or wind down the motor + } + + switch ( rsc_mode ) { case AP_MOTORSHELI_RSC_MODE_CH8_PASSTHROUGH: diff --git a/libraries/AP_Motors/AP_MotorsHeli.h b/libraries/AP_Motors/AP_MotorsHeli.h index 86cec44168..e9a6302f95 100644 --- a/libraries/AP_Motors/AP_MotorsHeli.h +++ b/libraries/AP_Motors/AP_MotorsHeli.h @@ -34,6 +34,8 @@ #define FLYBARLESS_HEAD 0 #define FLYBAR_HEAD 1 +// motor run-up timer +#define MOTOR_RUNUP_TIME 500 // 500 = 5 seconds class AP_HeliControls; @@ -67,6 +69,7 @@ public: _stab_throttle_scalar = 1; _swash_initialised = false; stab_throttle = false; + motor_runup_complete = false; }; RC_Channel *_servo_1; @@ -96,6 +99,7 @@ public: AP_Int8 stab_col_min; // collective pitch minimum in Stabilize Mode AP_Int8 stab_col_max; // collective pitch maximum in Stabilize Mode bool stab_throttle; // true if we are in Stabilize Mode for reduced Swash Range + bool motor_runup_complete; // true if the rotors have had enough time to wind up int16_t coll_out; // returns the actual collective in use to the main code // init @@ -150,7 +154,7 @@ protected: bool _swash_initialised; // true if swash has been initialised int16_t rsc_output; // final output to the external motor governor 1000-2000 int16_t rsc_ramp; // current state of ramping - + int16_t motor_runup_timer; // timer to determine if motor has run up fully };