aepko/Ardupilot2
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Added a slew rate to the alt change rate.

Browse Source 
Added better reporting of throttle control
upped constraints for alt hold
Additional logging for CTUN lay hold
This commit is contained in:
Jason Short 2011-10-28 21:29:10 -07:00
parent fb0e0f02e7
commit 07daf34ccc
6 changed files with 62 additions and 54 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

28
ArduCopter/ArduCopter.pde
View File

@ -334,6 +334,9 @@ static int waypoint_speed_gov;
static bool do_flip = false; static bool do_flip = false;
#endif #endif
static boolean trim_flag;
static int CH7_wp_index = 0;
// Airspeed // Airspeed
// -------- // --------
static int airspeed; // m/s * 100 static int airspeed; // m/s * 100
@ -342,6 +345,7 @@ static int airspeed; // m/s * 100
// --------------- // ---------------
static long altitude_error; // meters * 100 we are off in altitude static long altitude_error; // meters * 100 we are off in altitude
static long old_altitude; static long old_altitude;
static int old_rate;
static long yaw_error; // how off are we pointed static long yaw_error; // how off are we pointed
static long long_error, lat_error; // temp for debugging static long long_error, lat_error; // temp for debugging
@ -381,6 +385,8 @@ static boolean land_complete;
static long old_alt; // used for managing altitude rates static long old_alt; // used for managing altitude rates
static int velocity_land; static int velocity_land;
static byte yaw_tracking = MAV_ROI_WPNEXT; // no tracking, point at next wp, or at a target static byte yaw_tracking = MAV_ROI_WPNEXT; // no tracking, point at next wp, or at a target
static int manual_boost; // used in adjust altitude to make changing alt faster
static int angle_boost; // used in adjust altitude to make changing alt faster
// Loiter management // Loiter management
// ----------------- // -----------------
@ -1022,7 +1028,8 @@ void update_throttle_mode(void)
case THROTTLE_MANUAL: case THROTTLE_MANUAL:
if (g.rc_3.control_in > 0){ if (g.rc_3.control_in > 0){
g.rc_3.servo_out = g.rc_3.control_in + get_angle_boost(g.rc_3.control_in); angle_boost = get_angle_boost(g.rc_3.control_in);
g.rc_3.servo_out = g.rc_3.control_in + angle_boost;
}else{ }else{
g.pi_stabilize_roll.reset_I(); g.pi_stabilize_roll.reset_I();
g.pi_stabilize_pitch.reset_I(); g.pi_stabilize_pitch.reset_I();
@ -1051,8 +1058,8 @@ void update_throttle_mode(void)
// clear the new data flag // clear the new data flag
invalid_throttle = false; invalid_throttle = false;
} }
angle_boost = get_angle_boost(g.throttle_cruise);
g.rc_3.servo_out = g.throttle_cruise + nav_throttle + get_angle_boost(g.throttle_cruise); g.rc_3.servo_out = g.throttle_cruise + nav_throttle + angle_boost + manual_boost;
break; break;
} }
} }
@ -1213,7 +1220,19 @@ static void update_altitude()
current_loc.alt = baro_alt + home.alt; current_loc.alt = baro_alt + home.alt;
} }
// calc the accel rate limit to 2m/s
altitude_rate = (current_loc.alt - old_altitude) * 10; // 10 hz timer altitude_rate = (current_loc.alt - old_altitude) * 10; // 10 hz timer
// rate limiter to reduce some of the motor pulsing
if (altitude_rate > 0){
// going up
altitude_rate = min(altitude_rate, old_rate + 20);
}else{
// going down
altitude_rate = max(altitude_rate, old_rate - 20);
}
old_rate = altitude_rate;
old_altitude = current_loc.alt; old_altitude = current_loc.alt;
#endif #endif
} }
@ -1225,9 +1244,12 @@ adjust_altitude()
next_WP.alt -= 1; // 1 meter per second next_WP.alt -= 1; // 1 meter per second
next_WP.alt = max(next_WP.alt, (current_loc.alt - 500)); // don't go less than 4 meters below current location next_WP.alt = max(next_WP.alt, (current_loc.alt - 500)); // don't go less than 4 meters below current location
next_WP.alt = max(next_WP.alt, 100); // don't go less than 1 meter next_WP.alt = max(next_WP.alt, 100); // don't go less than 1 meter
//manual_boost = (g.rc_3.control_in == 0) ? -20 : 0;
}else if (g.rc_3.control_in > 700){ }else if (g.rc_3.control_in > 700){
next_WP.alt += 1; // 1 meter per second next_WP.alt += 1; // 1 meter per second
next_WP.alt = min(next_WP.alt, (current_loc.alt + 500)); // don't go more than 4 meters below current location next_WP.alt = min(next_WP.alt, (current_loc.alt + 500)); // don't go more than 4 meters below current location
//manual_boost = (g.rc_3.control_in == 800) ? 20 : 0;
} }
} }

27
ArduCopter/Attitude.pde
View File

@ -83,7 +83,7 @@ get_stabilize_yaw(long target_angle)
return (int)constrain(rate, -2500, 2500); return (int)constrain(rate, -2500, 2500);
} }
#define ALT_ERROR_MAX 400 #define ALT_ERROR_MAX 500
static int static int
get_nav_throttle(long z_error) get_nav_throttle(long z_error)
{ {
@ -94,33 +94,10 @@ get_nav_throttle(long z_error)
rate_error = rate_error - altitude_rate; rate_error = rate_error - altitude_rate;
// limit the rate // limit the rate
rate_error = constrain(rate_error, -100, 120); rate_error = constrain(rate_error, -120, 140);
return (int)g.pi_throttle.get_pi(rate_error, .1); return (int)g.pi_throttle.get_pi(rate_error, .1);
} }
#define ALT_ERROR_MAX2 300
static int
get_nav_throttle2(long z_error)
{
if (z_error > ALT_ERROR_MAX2){
return g.pi_throttle.kP() * 80;
}else if (z_error < -ALT_ERROR_MAX2){
return g.pi_throttle.kP() * -60;
} else{
// limit error to prevent I term run up
z_error = constrain(z_error, -ALT_ERROR_MAX2, ALT_ERROR_MAX2);
int rate_error = g.pi_alt_hold.get_pi(z_error, .1); //_p = .85
rate_error = rate_error - altitude_rate;
// limit the rate
rate_error = constrain(rate_error, -100, 120);
return (int)g.pi_throttle.get_pi(rate_error, .1) + alt_hold_velocity();
}
}
static int static int
get_rate_roll(long target_rate) get_rate_roll(long target_rate)
{ {

49
ArduCopter/Log.pde
View File

@ -680,19 +680,23 @@ static void Log_Write_Control_Tuning()
DataFlash.WriteByte(HEAD_BYTE2); DataFlash.WriteByte(HEAD_BYTE2);
DataFlash.WriteByte(LOG_CONTROL_TUNING_MSG); DataFlash.WriteByte(LOG_CONTROL_TUNING_MSG);
// yaw // yaw
DataFlash.WriteInt((int)(dcm.yaw_sensor/100)); //2 DataFlash.WriteInt((int)(dcm.yaw_sensor/100)); //1
DataFlash.WriteInt((int)(nav_yaw/100)); //2 DataFlash.WriteInt((int)(nav_yaw/100)); //2
DataFlash.WriteInt((int)yaw_error/100); //3 DataFlash.WriteInt((int)yaw_error/100); //3
// Alt hold // Alt hold
DataFlash.WriteInt(g.rc_3.servo_out); //4 DataFlash.WriteInt(sonar_alt); //4
DataFlash.WriteInt(sonar_alt); //5 DataFlash.WriteInt(baro_alt); //5
DataFlash.WriteInt(baro_alt); //6 DataFlash.WriteInt((int)next_WP.alt); //6
DataFlash.WriteInt((int)next_WP.alt); //7 DataFlash.WriteInt(nav_throttle); //7
DataFlash.WriteInt((int)g.pi_throttle.get_integrator());//8 DataFlash.WriteInt(angle_boost); //8
DataFlash.WriteByte(manual_boost); //9
DataFlash.WriteInt(g.rc_3.servo_out); //10
DataFlash.WriteInt((int)g.pi_alt_hold.get_integrator()); //11
DataFlash.WriteInt((int)g.pi_throttle.get_integrator()); //12
DataFlash.WriteByte(END_BYTE); DataFlash.WriteByte(END_BYTE);
} }
@ -701,27 +705,30 @@ static void Log_Write_Control_Tuning()
// Read an control tuning packet // Read an control tuning packet
static void Log_Read_Control_Tuning() static void Log_Read_Control_Tuning()
{ {
Serial.printf_P(PSTR( "CTUN, " // 1 2 3 4 5 6 7 8 9 10 11 12
"%d, %d, %d, " Serial.printf_P(PSTR( "CTUN, %d, %d, %d, %d, %d, %d, %d, %d, %d, %d, %d, %d\n"),
"%d, %d, %d, "
"%d, %d\n"),
// Control // Control
//DataFlash.ReadByte(), //DataFlash.ReadByte(),
//DataFlash.ReadInt(), //DataFlash.ReadInt(),
// yaw // yaw
DataFlash.ReadInt(), DataFlash.ReadInt(), //1
DataFlash.ReadInt(), DataFlash.ReadInt(), //2
DataFlash.ReadInt(), DataFlash.ReadInt(), //3
// Alt Hold // Alt Hold
DataFlash.ReadInt(), DataFlash.ReadInt(), //4
DataFlash.ReadInt(), DataFlash.ReadInt(), //5
DataFlash.ReadInt(), DataFlash.ReadInt(), //6
DataFlash.ReadInt(), DataFlash.ReadInt(), //7
DataFlash.ReadInt()); DataFlash.ReadInt(), //8
DataFlash.ReadByte(), //9
DataFlash.ReadInt(), //10
DataFlash.ReadInt(), //11
DataFlash.ReadInt()); //12
} }
// Write a performance monitoring packet. Total length : 19 bytes // Write a performance monitoring packet. Total length : 19 bytes

2
ArduCopter/config.h
View File

@ -520,7 +520,7 @@
// RATE control // RATE control
#ifndef THROTTLE_P #ifndef THROTTLE_P
# define THROTTLE_P 0.6 // # define THROTTLE_P 0.8 //
#endif #endif
#ifndef THROTTLE_I #ifndef THROTTLE_I
# define THROTTLE_I 0.10 // with 4m error, 12.5s windup # define THROTTLE_I 0.10 // with 4m error, 12.5s windup

4
ArduCopter/control_modes.pde
View File

@ -42,10 +42,6 @@ static void reset_control_switch()
read_control_switch(); read_control_switch();
} }
#if CH7_OPTION == CH7_SET_HOVER
static boolean trim_flag;
#endif
// read at 10 hz // read at 10 hz
// set this to your trainer switch // set this to your trainer switch
static void read_trim_switch() static void read_trim_switch()

6
ArduCopter/system.pde
View File

@ -348,6 +348,12 @@ static void set_mode(byte mode)
// used to stop fly_aways // used to stop fly_aways
motor_auto_armed = (g.rc_3.control_in > 0); motor_auto_armed = (g.rc_3.control_in > 0);
// clearing value used in interactive alt hold
manual_boost = 0;
// clearing value used to set WP's dynamically.
CH7_wp_index = 0;
Serial.println(flight_mode_strings[control_mode]); Serial.println(flight_mode_strings[control_mode]);
// report the GPS and Motor arming status // report the GPS and Motor arming status