Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

Added climb rate control 

removed unused vars
This commit is contained in:
Jason Short 2012-01-10 23:31:38 -08:00
parent 7890b784b2
commit 5a91396f63
1 changed files with 67 additions and 56 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

123
ArduCopter/ArduCopter.pde
View File

@ -279,7 +279,6 @@ ModeFilter sonar_mode_filter;
////////////////////////////////////////////////////////////////////////////////
// Global variables
////////////////////////////////////////////////////////////////////////////////
static const char *comma = ",";
static const char* flight_mode_strings[] = {
"STABILIZE",
@ -328,7 +327,7 @@ static int16_t y_rate_error;
////////////////////////////////////////////////////////////////////////////////
// This is the state of the flight control system
// There are multiple states defined such as STABILIZE, ACRO,
static byte control_mode = STABILIZE;
static int8_t control_mode = STABILIZE;
// This is the state of simple mode.
// Set in the control_mode.pde file when the control switch is read
static bool do_simple = false;
@ -444,8 +443,6 @@ static int32_t target_bearing;
// This is the angle from the copter to the "next_WP" location
// with the addition of Crosstrack error in degrees * 100
static int32_t nav_bearing;
// This is the angle from the copter to the "home" location in degrees * 100
static int32_t home_bearing;
// Status of the Waypoint tracking mode. Options include:
// NO_NAV_MODE, WP_MODE, LOITER_MODE, CIRCLE_MODE
static byte wp_control;
@ -573,7 +570,8 @@ static int32_t baro_alt;
static int32_t old_baro_alt;
// The climb_rate as reported by Baro in cm/s
static int16_t baro_rate;
//
static boolean reset_throttle_flag;
////////////////////////////////////////////////////////////////////////////////
// flight modes
@ -682,12 +680,23 @@ static int16_t nav_throttle; // 0-1000 for throttle control
static uint32_t throttle_integrator;
// This is a future value for replacing the throttle_cruise setup procedure. It's an average of throttle control
// that is generated when the climb rate is within a certain threshold
static float throttle_avg = THROTTLE_CRUISE;
//static float throttle_avg = THROTTLE_CRUISE;
// This is a flag used to trigger the updating of nav_throttle at 10hz
static bool invalid_throttle;
// Used to track the altitude offset for climbrate control
static int32_t target_altitude;
//static int32_t target_altitude;
////////////////////////////////////////////////////////////////////////////////
// Climb rate control
////////////////////////////////////////////////////////////////////////////////
// Time when we intiated command in millis - used for controlling decent rate
// The orginal altitude used to base our new altitude during decent
static int32_t original_altitude;
// Used to track the altitude offset for climbrate control
static int32_t target_altitude;
static uint32_t alt_change_timer;
static int8_t alt_change_flag;
static uint32_t alt_change;
////////////////////////////////////////////////////////////////////////////////
// Navigation Yaw control
@ -742,16 +751,6 @@ static int32_t condition_value; // used in condition commands (eg delay, change
static uint32_t condition_start;
////////////////////////////////////////////////////////////////////////////////
// Auto Landing
////////////////////////////////////////////////////////////////////////////////
// Time when we intiated command in millis - used for controlling decent rate
static int32_t land_start;
// The orginal altitude used to base our new altitude during decent
static int32_t original_alt;
////////////////////////////////////////////////////////////////////////////////
// IMU variables
////////////////////////////////////////////////////////////////////////////////
@ -795,6 +794,8 @@ static float dTnav;
static int16_t superslow_loopCounter;
// RTL Autoland Timer
static uint32_t auto_land_timer;
// disarms the copter while in Acro or Stabilzie mode after 30 seconds of no flight
static uint8_t auto_disarming_counter;
// Tracks if GPS is enabled based on statup routine
@ -803,7 +804,7 @@ static bool GPS_enabled = false;
// Set true if we have new PWM data to act on from the Radio
static bool new_radio_frame;
// Used to auto exit the in-flight leveler
static int16_t auto_level_counter;
static int16_t auto_level_counter;
// Reference to the AP relay object - APM1 only
AP_Relay relay;
@ -866,6 +867,7 @@ void loop()
counter_one_herz++;
// trgger our 1 hz loop
if(counter_one_herz >= 50){
super_slow_loop();
counter_one_herz = 0;
@ -1194,16 +1196,28 @@ static void slow_loop()
default:
slow_loopCounter = 0;
break;
}
}
#define AUTO_ARMING_DELAY 60
// 1Hz loop
static void super_slow_loop()
{
if (g.log_bitmask & MASK_LOG_CUR)
Log_Write_Current();
// this function disarms the copter if it has been sitting on the ground for any moment of time greater than 30s
// but only of the control mode is manual
if((control_mode <= ACRO) && (g.rc_3.control_in == 0)){
auto_disarming_counter++;
if(auto_disarming_counter == AUTO_ARMING_DELAY){
init_disarm_motors();
}else if (auto_disarming_counter > AUTO_ARMING_DELAY){
auto_disarming_counter = AUTO_ARMING_DELAY + 1;
}
}else{
auto_disarming_counter = 0;
}
gcs_send_message(MSG_HEARTBEAT);
gcs_data_stream_send(1,3);
// agmatthews - USERHOOKS
@ -1559,18 +1573,27 @@ void update_throttle_mode(void)
throttle_out = g.throttle_cruise + angle_boost + manual_boost;
#endif
// reset next_WP.alt and don't go below 1 meter
next_WP.alt = max(current_loc.alt, 100);
//force a reset of the altitude change
clear_new_altitude();
/*
Serial.printf("tar_alt: %d, actual_alt: %d \talt_err: %d, \t manb: %d\n",
int16_t iterm = g.pi_alt_hold.get_integrator();
Serial.printf("tar_alt: %d, actual_alt: %d \talt_err: %d, \t manb: %d, iterm %d\n",
next_WP.alt,
current_loc.alt,
altitude_error,
manual_boost);
manual_boost,
iterm);
//*/
reset_throttle_flag = true;
}else{
if(reset_throttle_flag) {
set_new_altitude(max(current_loc.alt, 100));
reset_throttle_flag = false;
}
// 10hz, don't run up i term
if(invalid_throttle && motor_auto_armed == true){
@ -1583,14 +1606,13 @@ void update_throttle_mode(void)
// clear the new data flag
invalid_throttle = false;
/*
Serial.printf("tar_alt: %d, actual_alt: %d \talt_err: %d, \tnav_thr: %d, \talt Int: %d, \trate_int %d \n",
Serial.printf("tar_alt: %d, actual_alt: %d \talt_err: %d, \tnav_thr: %d, \talt Int: %d\n",
next_WP.alt,
current_loc.alt,
altitude_error,
nav_throttle,
(int16_t)g.pi_alt_hold.get_integrator(),
(int16_t) g.pi_throttle.get_integrator());
*/
(int16_t)g.pi_alt_hold.get_integrator());
//*/
}
#if FRAME_CONFIG == HELI_FRAME
@ -1638,23 +1660,26 @@ static void update_navigation()
break;
case RTL:
// We have reached Home
if((wp_distance <= g.waypoint_radius) || check_missed_wp()){
// if this value > 0, we are set to trigger auto_land after 30 seconds
set_mode(LOITER);
auto_land_timer = millis();
break;
}
}else if(current_loc.alt < (next_WP.alt - 300)){
// don't navigate if we are below our target alt
wp_control = LOITER_MODE;
// We wait until we've reached out new altitude before coming home
// Arg doesn't work, it
//if(alt_change_flag != REACHED_ALT){
// wp_control = NO_NAV_MODE;
//}else{
wp_control = WP_MODE;
}else{
// calculates desired Yaw
#if FRAME_CONFIG == HELI_FRAME
update_auto_yaw();
#endif
wp_control = WP_MODE;
}
//}
// calculates the desired Roll and Pitch
update_nav_wp();
@ -1690,12 +1715,9 @@ static void update_navigation()
case LAND:
wp_control = LOITER_MODE;
if(verify_land()) { // JLN fix for auto land in RTL
set_mode(STABILIZE);
} else {
// calculates the desired Roll and Pitch
update_nav_wp();
}
// verify land will override WP_control if we are below
// a certain altitude
verify_land();
// calculates the desired Roll and Pitch
update_nav_wp();
@ -1828,7 +1850,8 @@ static void update_altitude()
scale = (sonar_alt - 400) / 200;
scale = constrain(scale, 0, 1);
current_loc.alt = ((float)sonar_alt * (1.0 - scale)) + ((float)baro_alt * scale) + home.alt;
// solve for a blended altitude
current_loc.alt = ((float)sonar_alt * (1.0 - scale)) + ((float)baro_alt * scale) + home.alt;
// solve for a blended climb_rate
climb_rate = ((float)sonar_rate * (1.0 - scale)) + (float)baro_rate * scale;
@ -1841,7 +1864,6 @@ static void update_altitude()
}
}else{
// NO Sonar case
current_loc.alt = baro_alt + home.alt;
climb_rate = baro_rate;
@ -1849,25 +1871,14 @@ static void update_altitude()
// manage bad data
climb_rate = constrain(climb_rate, -300, 300);
// update the target altitude
next_WP.alt = get_new_altitude();
}
static void
adjust_altitude()
{
/*
// old vert control
if(g.rc_3.control_in <= 200){
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;
}*/
if(g.rc_3.control_in <= 180){
// we remove 0 to 100 PWM from hover
manual_boost = g.rc_3.control_in - 180;