Added a slew rate to the alt change rate.
Added better reporting of throttle control upped constraints for alt hold Additional logging for CTUN lay hold
This commit is contained in:
Jason Short
parent
fb0e0f02e7
commit
07daf34ccc
@ -334,6 +334,9 @@ static int waypoint_speed_gov;
|
||||
static bool do_flip = false;
|
||||
#endif
|
||||
|
||||
static boolean trim_flag;
|
||||
static int CH7_wp_index = 0;
|
||||
|
||||
// Airspeed
|
||||
// --------
|
||||
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 old_altitude;
|
||||
static int old_rate;
|
||||
static long yaw_error; // how off are we pointed
|
||||
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 int velocity_land;
|
||||
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
|
||||
// -----------------
|
||||
@ -1022,7 +1028,8 @@ void update_throttle_mode(void)
|
||||
|
||||
case THROTTLE_MANUAL:
|
||||
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{
|
||||
g.pi_stabilize_roll.reset_I();
|
||||
g.pi_stabilize_pitch.reset_I();
|
||||
@ -1051,8 +1058,8 @@ void update_throttle_mode(void)
|
||||
// clear the new data flag
|
||||
invalid_throttle = false;
|
||||
}
|
||||
|
||||
g.rc_3.servo_out = g.throttle_cruise + nav_throttle + get_angle_boost(g.throttle_cruise);
|
||||
angle_boost = get_angle_boost(g.throttle_cruise);
|
||||
g.rc_3.servo_out = g.throttle_cruise + nav_throttle + angle_boost + manual_boost;
|
||||
break;
|
||||
}
|
||||
}
|
||||
@ -1213,7 +1220,19 @@ static void update_altitude()
|
||||
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
|
||||
|
||||
// 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;
|
||||
#endif
|
||||
}
|
||||
@ -1225,9 +1244,12 @@ adjust_altitude()
|
||||
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, 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){
|
||||
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
|
||||
//manual_boost = (g.rc_3.control_in == 800) ? 20 : 0;
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
@ -83,7 +83,7 @@ get_stabilize_yaw(long target_angle)
|
||||
return (int)constrain(rate, -2500, 2500);
|
||||
}
|
||||
|
||||
#define ALT_ERROR_MAX 400
|
||||
#define ALT_ERROR_MAX 500
|
||||
static int
|
||||
get_nav_throttle(long z_error)
|
||||
{
|
||||
@ -94,33 +94,10 @@ get_nav_throttle(long z_error)
|
||||
rate_error = rate_error - altitude_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);
|
||||
}
|
||||
|
||||
#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
|
||||
get_rate_roll(long target_rate)
|
||||
{
|
||||
|
||||
@ -680,19 +680,23 @@ static void Log_Write_Control_Tuning()
|
||||
DataFlash.WriteByte(HEAD_BYTE2);
|
||||
DataFlash.WriteByte(LOG_CONTROL_TUNING_MSG);
|
||||
|
||||
|
||||
// yaw
|
||||
DataFlash.WriteInt((int)(dcm.yaw_sensor/100)); //2
|
||||
DataFlash.WriteInt((int)(nav_yaw/100)); //2
|
||||
DataFlash.WriteInt((int)yaw_error/100); //3
|
||||
DataFlash.WriteInt((int)(dcm.yaw_sensor/100)); //1
|
||||
DataFlash.WriteInt((int)(nav_yaw/100)); //2
|
||||
DataFlash.WriteInt((int)yaw_error/100); //3
|
||||
|
||||
// Alt hold
|
||||
DataFlash.WriteInt(g.rc_3.servo_out); //4
|
||||
DataFlash.WriteInt(sonar_alt); //5
|
||||
DataFlash.WriteInt(baro_alt); //6
|
||||
DataFlash.WriteInt(sonar_alt); //4
|
||||
DataFlash.WriteInt(baro_alt); //5
|
||||
DataFlash.WriteInt((int)next_WP.alt); //6
|
||||
|
||||
DataFlash.WriteInt((int)next_WP.alt); //7
|
||||
DataFlash.WriteInt((int)g.pi_throttle.get_integrator());//8
|
||||
DataFlash.WriteInt(nav_throttle); //7
|
||||
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);
|
||||
}
|
||||
@ -701,27 +705,30 @@ static void Log_Write_Control_Tuning()
|
||||
// Read an control tuning packet
|
||||
static void Log_Read_Control_Tuning()
|
||||
{
|
||||
Serial.printf_P(PSTR( "CTUN, "
|
||||
"%d, %d, %d, "
|
||||
"%d, %d, %d, "
|
||||
"%d, %d\n"),
|
||||
// 1 2 3 4 5 6 7 8 9 10 11 12
|
||||
Serial.printf_P(PSTR( "CTUN, %d, %d, %d, %d, %d, %d, %d, %d, %d, %d, %d, %d\n"),
|
||||
|
||||
// Control
|
||||
//DataFlash.ReadByte(),
|
||||
//DataFlash.ReadInt(),
|
||||
|
||||
// yaw
|
||||
DataFlash.ReadInt(),
|
||||
DataFlash.ReadInt(),
|
||||
DataFlash.ReadInt(),
|
||||
DataFlash.ReadInt(), //1
|
||||
DataFlash.ReadInt(), //2
|
||||
DataFlash.ReadInt(), //3
|
||||
|
||||
// Alt Hold
|
||||
DataFlash.ReadInt(),
|
||||
DataFlash.ReadInt(),
|
||||
DataFlash.ReadInt(),
|
||||
DataFlash.ReadInt(), //4
|
||||
DataFlash.ReadInt(), //5
|
||||
DataFlash.ReadInt(), //6
|
||||
|
||||
DataFlash.ReadInt(),
|
||||
DataFlash.ReadInt());
|
||||
DataFlash.ReadInt(), //7
|
||||
DataFlash.ReadInt(), //8
|
||||
DataFlash.ReadByte(), //9
|
||||
|
||||
DataFlash.ReadInt(), //10
|
||||
DataFlash.ReadInt(), //11
|
||||
DataFlash.ReadInt()); //12
|
||||
}
|
||||
|
||||
// Write a performance monitoring packet. Total length : 19 bytes
|
||||
|
||||
@ -520,7 +520,7 @@
|
||||
|
||||
// RATE control
|
||||
#ifndef THROTTLE_P
|
||||
# define THROTTLE_P 0.6 //
|
||||
# define THROTTLE_P 0.8 //
|
||||
#endif
|
||||
#ifndef THROTTLE_I
|
||||
# define THROTTLE_I 0.10 // with 4m error, 12.5s windup
|
||||
|
||||
@ -42,10 +42,6 @@ static void reset_control_switch()
|
||||
read_control_switch();
|
||||
}
|
||||
|
||||
#if CH7_OPTION == CH7_SET_HOVER
|
||||
static boolean trim_flag;
|
||||
#endif
|
||||
|
||||
// read at 10 hz
|
||||
// set this to your trainer switch
|
||||
static void read_trim_switch()
|
||||
|
||||
@ -348,6 +348,12 @@ static void set_mode(byte mode)
|
||||
// used to stop fly_aways
|
||||
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]);
|
||||
|
||||
// report the GPS and Motor arming status
|
||||
|
||||
Loading…
Reference in New Issue
Block a user