Trying to make Alt hold more aggressively change altitudes.

This commit is contained in:
Jason Short 2011-09-23 13:58:39 -07:00
parent 7c72f13702
commit 0f5c74f03a
2 changed files with 5 additions and 14 deletions

View File

@ -296,8 +296,6 @@ static boolean motor_auto_armed; // if true,
// PIDs // PIDs
// ---- // ----
//int max_stabilize_dampener; //
//int max_yaw_dampener; //
static Vector3f omega; static Vector3f omega;
float tuning_value; float tuning_value;
@ -319,13 +317,8 @@ static bool did_ground_start = false; // have we ground started after first ar
// --------------------- // ---------------------
static const float radius_of_earth = 6378100; // meters static const float radius_of_earth = 6378100; // meters
static const float gravity = 9.81; // meters/ sec^2 static const float gravity = 9.81; // meters/ sec^2
//static long nav_bearing; // deg * 100 : 0 to 360 current desired bearing to navigate
static long target_bearing; // deg * 100 : 0 to 360 location of the plane to the target static long target_bearing; // deg * 100 : 0 to 360 location of the plane to the target
//static bool xtrack_enabled = false;
//static long crosstrack_bearing; // deg * 100 : 0 to 360 desired angle of plane to target
//static long crosstrack_correction; // deg * 100 : 0 to 360 desired angle of plane to target
static int climb_rate; // m/s * 100 - For future implementation of controlled ascent/descent by rate static int climb_rate; // m/s * 100 - For future implementation of controlled ascent/descent by rate
static byte wp_control; // used to control - navgation or loiter static byte wp_control; // used to control - navgation or loiter
@ -380,8 +373,7 @@ static int ground_temperature;
// Altitude Sensor variables // Altitude Sensor variables
// ---------------------- // ----------------------
static int sonar_alt; static int sonar_alt;
static int baro_alt; static int baro_alt;;
//static int baro_alt_offset;
static byte altitude_sensor = BARO; // used to know which sensor is active, BARO or SONAR static byte altitude_sensor = BARO; // used to know which sensor is active, BARO or SONAR
static int altitude_rate; static int altitude_rate;
@ -393,7 +385,6 @@ static byte throttle_mode;
static boolean takeoff_complete; // Flag for using take-off controls static boolean takeoff_complete; // Flag for using take-off controls
static boolean land_complete; static boolean land_complete;
//static int landing_distance; // meters;
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
@ -1113,7 +1104,7 @@ void update_throttle_mode(void)
altitude_error = get_altitude_error(); altitude_error = get_altitude_error();
// get the AP throttle // get the AP throttle
nav_throttle = get_nav_throttle(altitude_error, 200); //150 = target speed of 1.5m/s nav_throttle = get_nav_throttle(altitude_error, 250); //150 = target speed of 1.5m/s
//Serial.printf("in:%d, cr:%d, NT:%d, I:%1.4f\n", g.rc_3.control_in,altitude_error, nav_throttle, g.pi_throttle.get_integrator()); //Serial.printf("in:%d, cr:%d, NT:%d, I:%1.4f\n", g.rc_3.control_in,altitude_error, nav_throttle, g.pi_throttle.get_integrator());
// clear the new data flag // clear the new data flag

View File

@ -85,7 +85,7 @@ get_stabilize_yaw(long target_angle)
return (int)constrain(rate, -2500, 2500); return (int)constrain(rate, -2500, 2500);
} }
#define ALT_ERROR_MAX 300 #define ALT_ERROR_MAX 350
static int static int
get_nav_throttle(long z_error, int target_speed) get_nav_throttle(long z_error, int target_speed)
{ {
@ -97,7 +97,7 @@ get_nav_throttle(long z_error, int target_speed)
target_speed = z_error * scaler; target_speed = z_error * scaler;
rate_error = target_speed - altitude_rate; rate_error = target_speed - altitude_rate;
rate_error = constrain(rate_error, -110, 110); rate_error = constrain(rate_error, -120, 140);
float delta_throttle; float delta_throttle;