mirror of https://github.com/ArduPilot/ardupilot
parent
7890b784b2
commit
5a91396f63
|
@ -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
|
||||
|
@ -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,6 +1850,7 @@ static void update_altitude()
|
|||
scale = (sonar_alt - 400) / 200;
|
||||
scale = constrain(scale, 0, 1);
|
||||
|
||||
// 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
|
||||
|
@ -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;
|
||||
|
|
Loading…
Reference in New Issue