From 7b4ed2d227513eddb4cab0dfde357fe2e22cde2d Mon Sep 17 00:00:00 2001 From: rmackay9 Date: Sat, 24 Nov 2012 22:50:09 +0900 Subject: [PATCH] ArduCopter: restored landing detector to landing throttle mode. Added LAND_SPEED parameter to control the rate of descent for the last 10m of a landing. Added CH6 tuning for accel based throttle controller. --- ArduCopter/ArduCopter.pde | 21 ++++++++++++++++----- ArduCopter/Attitude.pde | 39 ++++++++++++++++++++++++++++++++------- ArduCopter/Parameters.h | 2 ++ ArduCopter/Parameters.pde | 9 +++++++++ ArduCopter/config.h | 3 +++ ArduCopter/defines.h | 5 ++++- 6 files changed, 66 insertions(+), 13 deletions(-) diff --git a/ArduCopter/ArduCopter.pde b/ArduCopter/ArduCopter.pde index a5fcef17947..8f2d8c2a13b 100644 --- a/ArduCopter/ArduCopter.pde +++ b/ArduCopter/ArduCopter.pde @@ -720,9 +720,8 @@ static byte throttle_mode; //////////////////////////////////////////////////////////////////////////////// // An additional throttle added to keep the copter at the same altitude when banking static int16_t angle_boost; -// for controlling the landing throttle curve -//verifies landings -static int16_t ground_detector; +// counter to verify landings +static uint16_t land_detector; //////////////////////////////////////////////////////////////////////////////// @@ -1691,7 +1690,7 @@ bool set_throttle_mode( uint8_t new_throttle_mode ) case THROTTLE_HOLD: case THROTTLE_AUTO: set_new_altitude(current_loc.alt); // by default hold the current altitude - if ( throttle_mode <= THROTTLE_HOLD ) { // reset the alt hold I terms if previous throttle mode was manual + if ( throttle_mode < THROTTLE_HOLD ) { // reset the alt hold I terms if previous throttle mode was manual reset_throttle_I(); } throttle_accel_controller_active = true; // this controller uses accel based throttle controller @@ -1700,7 +1699,7 @@ bool set_throttle_mode( uint8_t new_throttle_mode ) case THROTTLE_LAND: set_land_complete(false); // mark landing as incomplete - ground_detector = 0; // A counter that goes up if our climb rate stalls out. + land_detector = 0; // A counter that goes up if our climb rate stalls out. set_new_altitude(0); // Set a new target altitude throttle_accel_controller_active = true; // this controller uses accel based throttle controller throttle_initialised = true; @@ -2153,6 +2152,18 @@ static void tuning(){ inertial_nav.set_time_constant_z(tuning_value); break; #endif + + case CH6_THR_ACCEL_KP: + g.pid_throttle_accel.kP(tuning_value); + break; + + case CH6_THR_ACCEL_KI: + g.pid_throttle_accel.kI(tuning_value); + break; + + case CH6_THR_ACCEL_KD: + g.pid_throttle_accel.kD(tuning_value); + break; } } diff --git a/ArduCopter/Attitude.pde b/ArduCopter/Attitude.pde index aca3a08149c..619fba578c6 100644 --- a/ArduCopter/Attitude.pde +++ b/ArduCopter/Attitude.pde @@ -711,8 +711,8 @@ static void update_throttle_cruise(int16_t throttle) if( throttle_avg == 0 ) { throttle_avg = g.throttle_cruise; } - // calc average throttle - if ((throttle > g.throttle_min) && (abs(climb_rate) < 60) && (g_gps->ground_speed < 200)) { + // calc average throttle if we are in a level hover + if (throttle > g.throttle_min && abs(climb_rate) < 60 && labs(ahrs.roll_sensor) < 500 && labs(ahrs.pitch_sensor) < 500) { throttle_avg = throttle_avg * .99 + (float)throttle * .01; g.throttle_cruise = throttle_avg; } @@ -963,6 +963,11 @@ get_throttle_rate(int16_t z_target_speed) // set target for accel based throttle controller set_throttle_accel_target(target_accel); + + // update throttle cruise + if( z_target_speed == 0 ) { + update_throttle_cruise(g.rc_3.servo_out); + } } // get_throttle_rate_stabilized - rate controller with additional 'stabilizer' @@ -1037,18 +1042,38 @@ get_throttle_althold(int32_t target_alt, int16_t max_climb_rate) // get_throttle_land - high level landing logic // sends the desired acceleration in the accel based throttle controller // called at 50hz -#define LAND_START_ALT 1000 // altitude in cm where land controller switches to slow rate of descent -#define LAND_DESCENT_SPEED -50 // minimum descent speed in cm/s +#define LAND_START_ALT 1000 // altitude in cm where land controller switches to slow rate of descent +#define LAND_DETECTOR_TRIGGER 250 // number of 50hz iterations with near zero climb rate that triggers landing complete. static void get_throttle_land() { // if we are above 10m perform regular alt hold descent if (current_loc.alt >= LAND_START_ALT) { - get_throttle_althold(LAND_START_ALT, LAND_DESCENT_SPEED); + get_throttle_althold(LAND_START_ALT, -abs(g.land_speed)); }else{ - get_throttle_rate_stabilized(LAND_DESCENT_SPEED); + get_throttle_rate_stabilized(-abs(g.land_speed)); + + // detect whether we have landed +#if INERTIAL_NAV == ENABLED + if (abs(inertial_nav._velocity.z) < 20) { +#else + if (abs(climb_rate) < 20) { +#endif + if( land_detector < LAND_DETECTOR_TRIGGER ) { + land_detector++; + }else{ + set_land_complete(true); + if( g.rc_3.control_in == 0 ) { + init_disarm_motors(); + } + } + }else{ + // we've sensed movement up or down so decrease land_detector + if (land_detector > 0 ) { + land_detector--; + } + } } - // TO-DO: add detection of whether we've landed or not and set ap.land_complete } /* diff --git a/ArduCopter/Parameters.h b/ArduCopter/Parameters.h index 479e21ca168..5c77c42510d 100644 --- a/ArduCopter/Parameters.h +++ b/ArduCopter/Parameters.h @@ -201,6 +201,7 @@ public: k_param_waypoint_radius, k_param_loiter_radius, k_param_waypoint_speed_max, + k_param_land_speed, // 217 // // 220: PI/D Controllers @@ -281,6 +282,7 @@ public: AP_Float crosstrack_gain; AP_Int16 crosstrack_min_distance; AP_Int32 auto_land_timeout; + AP_Int16 land_speed; // Throttle diff --git a/ArduCopter/Parameters.pde b/ArduCopter/Parameters.pde index 8d447115e46..b6daaad67cc 100644 --- a/ArduCopter/Parameters.pde +++ b/ArduCopter/Parameters.pde @@ -193,6 +193,15 @@ const AP_Param::Info var_info[] PROGMEM = { GSCALAR(auto_land_timeout, "AUTO_LAND", AUTO_LAND_TIME*1000), + // @Param: LAND_SPEED + // @DisplayName: Land speed + // @Description: The descent speed for the final stage of landing in cm/s + // @Units: cm/s + // @Range: 10 200 + // @Increment: 10 + // @User: Standard + GSCALAR(land_speed, "LAND_SPEED", LAND_SPEED), + // @Param: THR_MIN // @DisplayName: Minimum Throttle // @Description: The minimum throttle which the autopilot will apply. diff --git a/ArduCopter/config.h b/ArduCopter/config.h index 46f1be4cbfe..99cefd9f9c3 100644 --- a/ArduCopter/config.h +++ b/ArduCopter/config.h @@ -437,6 +437,9 @@ # define AUTO_LAND_TIME 5 #endif +#ifndef LAND_SPEED + # define LAND_SPEED 50 // the descent speed for the final stage of landing in cm/s +#endif ////////////////////////////////////////////////////////////////////////////// diff --git a/ArduCopter/defines.h b/ArduCopter/defines.h index 4dfe2e7320e..9f93ff70da2 100644 --- a/ArduCopter/defines.h +++ b/ArduCopter/defines.h @@ -167,8 +167,11 @@ #define CH6_RATE_KD 21 #define CH6_YAW_RATE_KP 6 #define CH6_YAW_RATE_KD 26 -// Altitude rate controller +// Throttle #define CH6_THROTTLE_KP 7 +#define CH6_THR_ACCEL_KP 33 +#define CH6_THR_ACCEL_KI 34 +#define CH6_THR_ACCEL_KD 35 // Extras #define CH6_TOP_BOTTOM_RATIO 8 #define CH6_RELAY 9