From 37abfdc65aab6bd0fc0948a2456e6f9ad23499a0 Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Sat, 20 Apr 2013 12:03:55 +0900 Subject: [PATCH] Copter: make CIRCLE_RATE a tunable parameter Also bug fix to check of how many time it has rotated during a loiter turns mission command --- ArduCopter/ArduCopter.pde | 8 ++++++-- ArduCopter/Parameters.h | 4 +++- ArduCopter/Parameters.pde | 11 ++++++++++- ArduCopter/commands_logic.pde | 2 +- ArduCopter/config.h | 4 ++++ ArduCopter/defines.h | 1 + ArduCopter/navigation.pde | 2 +- 7 files changed, 26 insertions(+), 6 deletions(-) diff --git a/ArduCopter/ArduCopter.pde b/ArduCopter/ArduCopter.pde index f3e2e3985d..8016c6c726 100644 --- a/ArduCopter/ArduCopter.pde +++ b/ArduCopter/ArduCopter.pde @@ -580,8 +580,6 @@ static LowPassFilterFloat rate_pitch_filter; // Rate Pitch filter //////////////////////////////////////////////////////////////////////////////// // Circle Mode / Loiter control //////////////////////////////////////////////////////////////////////////////// -// used to control the speed of Circle mode in radians/second, default is 5° per second -static const float circle_rate = 0.0872664625; Vector3f circle_center; // circle position expressed in cm from home location. x = lat, y = lon // angle from the circle center to the copter's desired location. Incremented at circle_rate / second static float circle_angle; @@ -2153,6 +2151,12 @@ static void tuning(){ // set declination to +-20degrees compass.set_declination(ToRad(20-g.rc_6.control_in/25), false); // 2nd parameter is false because we do not want to save to eeprom because this would have a performance impact break; + + case CH6_CIRCLE_RATE: + // set circle rate + g.circle_rate.set(g.rc_6.control_in/25-20); // allow approximately 45 degree turn rate in either direction + //cliSerial->printf_P(PSTR("\nRate:%4.2f"),(float)g.circle_rate); + break; } } diff --git a/ArduCopter/Parameters.h b/ArduCopter/Parameters.h index 68a021d45c..5def028e05 100644 --- a/ArduCopter/Parameters.h +++ b/ArduCopter/Parameters.h @@ -80,7 +80,8 @@ public: k_param_throttle_accel_enabled, k_param_wp_yaw_behavior, k_param_acro_trainer_enabled, - k_param_pilot_velocity_z_max, // 28 + k_param_pilot_velocity_z_max, + k_param_circle_rate, // 29 // 65: AP_Limits Library k_param_limits = 65, @@ -289,6 +290,7 @@ public: AP_Int8 command_total; AP_Int8 command_index; AP_Int16 circle_radius; + AP_Float circle_rate; // Circle mode's turn rate in deg/s. positive to rotate clockwise, negative for counter clockwise AP_Int32 rtl_loiter_time; AP_Int16 land_speed; AP_Int16 pilot_velocity_z_max; // maximum vertical velocity the pilot may request diff --git a/ArduCopter/Parameters.pde b/ArduCopter/Parameters.pde index f29e841aad..20e3c51127 100644 --- a/ArduCopter/Parameters.pde +++ b/ArduCopter/Parameters.pde @@ -210,6 +210,15 @@ const AP_Param::Info var_info[] PROGMEM = { // @User: Standard GSCALAR(circle_radius, "CIRCLE_RADIUS", CIRCLE_RADIUS), + // @Param: CIRCLE_RATE + // @DisplayName: Circle rate + // @Description: Circle mode's turn rate in degrees / second. Positive to turn clockwise, negative for counter clockwise + // @Units: Degrees / second + // @Range: -90 90 + // @Increment: 1 + // @User: Standard + GSCALAR(circle_rate, "CIRCLE_RATE", CIRCLE_RATE), + // @Param: RTL_LOIT_TIME // @DisplayName: RTL loiter time // @Description: Time (in milliseconds) to loiter above home before begining final descent @@ -350,7 +359,7 @@ const AP_Param::Info var_info[] PROGMEM = { // @DisplayName: Channel 6 Tuning // @Description: Controls which parameters (normally PID gains) are being tuned with transmitter's channel 6 knob // @User: Standard - // @Values: 0:CH6_NONE,1:CH6_STABILIZE_KP,2:CH6_STABILIZE_KI,3:CH6_YAW_KP,4:CH6_RATE_KP,5:CH6_RATE_KI,6:CH6_YAW_RATE_KP,7:CH6_THROTTLE_KP,8:CH6_TOP_BOTTOM_RATIO,9:CH6_RELAY,10:CH6_WP_SPEED,12:CH6_LOITER_KP,13:CH6_HELI_EXTERNAL_GYRO,14:CH6_THR_HOLD_KP,17:CH6_OPTFLOW_KP,18:CH6_OPTFLOW_KI,19:CH6_OPTFLOW_KD,21:CH6_RATE_KD,22:CH6_LOITER_RATE_KP,23:CH6_LOITER_RATE_KD,24:CH6_YAW_KI,25:CH6_ACRO_KP,26:CH6_YAW_RATE_KD,27:CH6_LOITER_KI,28:CH6_LOITER_RATE_KI,29:CH6_STABILIZE_KD,30:CH6_AHRS_YAW_KP,31:CH6_AHRS_KP,32:CH6_INAV_TC,33:CH6_THROTTLE_KI,34:CH6_THR_ACCEL_KP,35:CH6_THR_ACCEL_KI,36:CH6_THR_ACCEL_KD,38:CH6_DECLINATION + // @Values: 0:CH6_NONE,1:CH6_STABILIZE_KP,2:CH6_STABILIZE_KI,3:CH6_YAW_KP,4:CH6_RATE_KP,5:CH6_RATE_KI,6:CH6_YAW_RATE_KP,7:CH6_THROTTLE_KP,8:CH6_TOP_BOTTOM_RATIO,9:CH6_RELAY,10:CH6_WP_SPEED,12:CH6_LOITER_KP,13:CH6_HELI_EXTERNAL_GYRO,14:CH6_THR_HOLD_KP,17:CH6_OPTFLOW_KP,18:CH6_OPTFLOW_KI,19:CH6_OPTFLOW_KD,21:CH6_RATE_KD,22:CH6_LOITER_RATE_KP,23:CH6_LOITER_RATE_KD,24:CH6_YAW_KI,25:CH6_ACRO_KP,26:CH6_YAW_RATE_KD,27:CH6_LOITER_KI,28:CH6_LOITER_RATE_KI,29:CH6_STABILIZE_KD,30:CH6_AHRS_YAW_KP,31:CH6_AHRS_KP,32:CH6_INAV_TC,33:CH6_THROTTLE_KI,34:CH6_THR_ACCEL_KP,35:CH6_THR_ACCEL_KI,36:CH6_THR_ACCEL_KD,38:CH6_DECLINATION,39:CH6_CIRCLE_RATE GSCALAR(radio_tuning, "TUNE", 0), // @Param: TUNE_LOW diff --git a/ArduCopter/commands_logic.pde b/ArduCopter/commands_logic.pde index 0ca6214c4f..ad6300c3e3 100644 --- a/ArduCopter/commands_logic.pde +++ b/ArduCopter/commands_logic.pde @@ -482,7 +482,7 @@ static bool verify_loiter_time() static bool verify_circle() { // have we rotated around the center enough times? - return fabs(circle_desired_rotations/M_PI) > circle_desired_rotations; + return fabsf(circle_angle_total/(2*M_PI)) >= circle_desired_rotations; } // verify_RTL - handles any state changes required to implement RTL diff --git a/ArduCopter/config.h b/ArduCopter/config.h index a8c64aa33d..af98d77a3b 100644 --- a/ArduCopter/config.h +++ b/ArduCopter/config.h @@ -608,6 +608,10 @@ # define CIRCLE_NAV NAV_CIRCLE #endif +#ifndef CIRCLE_RATE + # define CIRCLE_RATE 5.0f // degrees per second turn rate +#endif + // Guided Mode #ifndef GUIDED_YAW # define GUIDED_YAW YAW_LOOK_AT_NEXT_WP diff --git a/ArduCopter/defines.h b/ArduCopter/defines.h index a2e3b82179..d206b41fed 100644 --- a/ArduCopter/defines.h +++ b/ArduCopter/defines.h @@ -171,6 +171,7 @@ #define CH6_AHRS_KP 31 // accelerometer effect on roll/pitch angle (0=low) #define CH6_INAV_TC 32 // inertial navigation baro/accel and gps/accel time constant (1.5 = strong baro/gps correction on accel estimatehas very strong does not correct accel estimate, 7 = very weak correction) #define CH6_DECLINATION 38 // compass declination in radians +#define CH6_CIRCLE_RATE 39 // circle turn rate in degrees (hard coded to about 45 degrees in either direction) // Commands - Note that APM now uses a subset of the MAVLink protocol diff --git a/ArduCopter/navigation.pde b/ArduCopter/navigation.pde index 646c9922c2..5cdfc1243e 100644 --- a/ArduCopter/navigation.pde +++ b/ArduCopter/navigation.pde @@ -238,7 +238,7 @@ circle_set_center(const Vector3f current_position, float heading_in_radians) static void update_circle(float dt) { - float angle_delta = circle_rate * dt; + float angle_delta = ToRad(g.circle_rate) * dt; float cir_radius = g.circle_radius * 100; Vector3f circle_target;