From 4a2f58b013999aa71364fd215eba3abddda67560 Mon Sep 17 00:00:00 2001 From: Jason Short Date: Mon, 12 Sep 2011 12:57:36 -0700 Subject: [PATCH] Cleaned up guided mode Reduced ADC filter to 3 from 6 to increase speed of filter. --- ArduCopter/ArduCopter.pde | 1 - ArduCopter/commands.pde | 43 ++++++++++++++++--------------- ArduCopter/commands_logic.pde | 11 +++----- ArduCopter/system.pde | 4 ++- libraries/AP_ADC/AP_ADC_ADS7844.h | 2 +- 5 files changed, 30 insertions(+), 31 deletions(-) diff --git a/ArduCopter/ArduCopter.pde b/ArduCopter/ArduCopter.pde index d377ee26bf..6ad4e24031 100644 --- a/ArduCopter/ArduCopter.pde +++ b/ArduCopter/ArduCopter.pde @@ -1464,7 +1464,6 @@ static void tuning(){ static void update_nav_wp() { - // XXX Guided mode!!! if(wp_control == LOITER_MODE){ // calc a pitch to the target diff --git a/ArduCopter/commands.pde b/ArduCopter/commands.pde index 427fc04b6c..ec1da8f3ff 100644 --- a/ArduCopter/commands.pde +++ b/ArduCopter/commands.pde @@ -1,5 +1,18 @@ // -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*- + +/*static void init_auto() +{ + //if (g.waypoint_index == g.waypoint_total) { + // do_RTL(); + //} + + // initialize commands + // ------------------- + init_commands(); +} +*/ + static void init_commands() { // zero is home, but we always load the next command (1), in the code. @@ -20,18 +33,6 @@ static void clear_command_queue(){ next_command.id = NO_COMMAND; } -static void init_auto() -{ - //if (g.waypoint_index == g.waypoint_total) { - // Serial.println("ia_f"); - // do_RTL(); - //} - - // initialize commands - // ------------------- - init_commands(); -} - // Getters // ------- static struct Location get_command_with_index(int i) @@ -41,8 +42,6 @@ static struct Location get_command_with_index(int i) // Find out proper location in memory by using the start_byte position + the index // -------------------------------------------------------------------------------- if (i > g.waypoint_total) { - Serial.println("XCD"); - // we do not have a valid command to load // return a WP with a "Blank" id temp.id = CMD_BLANK; @@ -51,7 +50,6 @@ static struct Location get_command_with_index(int i) return temp; }else{ - //Serial.println("LD"); // we can load a command, we don't process it yet // read WP position long mem = (WP_START_BYTE) + (i * WP_SIZE); @@ -75,10 +73,9 @@ static struct Location get_command_with_index(int i) } // Add on home altitude if we are a nav command (or other command with altitude) and stored alt is relative - if((temp.id < MAV_CMD_NAV_LAST || temp.id == MAV_CMD_CONDITION_CHANGE_ALT) && temp.options & WP_OPTION_ALT_RELATIVE){ + //if((temp.id < MAV_CMD_NAV_LAST || temp.id == MAV_CMD_CONDITION_CHANGE_ALT) && temp.options & WP_OPTION_ALT_RELATIVE){ //temp.alt += home.alt; - } - //Serial.println("ADD ALT"); + //} if(temp.options & WP_OPTION_RELATIVE){ // If were relative, just offset from home @@ -217,14 +214,12 @@ static void init_home() home.lng = g_gps->longitude; // Lon * 10**7 home.lat = g_gps->latitude; // Lat * 10**7 //home.alt = max(g_gps->altitude, 0); // we sometimes get negatives from GPS, not valid - home.alt = 0; // this is a test + home.alt = 0; // Home is always 0 home_is_set = true; // to point yaw towards home until we set it with Mavlink target_WP = home; - //Serial.printf_P(PSTR("gps alt: %ld\n"), home.alt); - // Save Home to EEPROM // ------------------- // no need to save this to EPROM @@ -233,8 +228,14 @@ static void init_home() // Save prev loc this makes the calcs look better before commands are loaded prev_WP = home; + // this is dangerous since we can get GPS lock at any time. //next_WP = home; + + // Load home for a default guided_WP + // ------------- + guided_WP = home; + guided_WP.alt += g.RTL_altitude; } diff --git a/ArduCopter/commands_logic.pde b/ArduCopter/commands_logic.pde index f544719c45..68dbce1c55 100644 --- a/ArduCopter/commands_logic.pde +++ b/ArduCopter/commands_logic.pde @@ -105,17 +105,14 @@ static void handle_process_now() static void handle_no_commands() { - // we don't want to RTL yet. Maybe this will change in the future. RTL is kinda dangerous. - // use landing commands /* switch (control_mode){ default: - //set_mode(RTL); + set_mode(RTL); break; - } - return; - */ - Serial.println("Handle No CMDs"); + }*/ + //return; + //Serial.println("Handle No CMDs"); } /********************************************************************************/ diff --git a/ArduCopter/system.pde b/ArduCopter/system.pde index f9ce432993..6eb3c83e8f 100644 --- a/ArduCopter/system.pde +++ b/ArduCopter/system.pde @@ -407,7 +407,8 @@ static void set_mode(byte mode) init_throttle_cruise(); // loads the commands from where we left off - init_auto(); + //init_auto(); + init_commands(); // do crosstrack correction // XXX move to flight commands @@ -440,6 +441,7 @@ static void set_mode(byte mode) //xtrack_enabled = true; init_throttle_cruise(); next_WP = current_loc; + set_next_WP(&guided_WP); break; case RTL: diff --git a/libraries/AP_ADC/AP_ADC_ADS7844.h b/libraries/AP_ADC/AP_ADC_ADS7844.h index 532dfd97de..ca748e4e1f 100644 --- a/libraries/AP_ADC/AP_ADC_ADS7844.h +++ b/libraries/AP_ADC/AP_ADC_ADS7844.h @@ -9,7 +9,7 @@ #define ADC_DATAIN 50 // MISO #define ADC_SPICLOCK 52 // SCK #define ADC_CHIP_SELECT 33 // PC4 9 // PH6 Puerto:0x08 Bit mask : 0x40 -#define ADC_FILTER_SIZE 6 +#define ADC_FILTER_SIZE 3 #include "AP_ADC.h" #include