Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-08 17:08:28 -04:00
Code Issues Packages Projects Releases Wiki Activity

Copter: make CIRCLE_RATE a tunable parameter

Browse Source 
Also bug fix to check of how many time it has rotated during a loiter
turns mission command
This commit is contained in:
Randy Mackay 2013-04-20 12:03:55 +09:00
parent 947b64dfc4
commit 37abfdc65a
7 changed files with 26 additions and 6 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

8
ArduCopter/ArduCopter.pde
View File

@ -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;
}
}

4
ArduCopter/Parameters.h
View File

@ -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

11
ArduCopter/Parameters.pde
View File

@ -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

2
ArduCopter/commands_logic.pde
View File

@ -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

4
ArduCopter/config.h
View File

@ -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

1
ArduCopter/defines.h
View File

@ -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

2
ArduCopter/navigation.pde
View File

@ -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;