From 9f03e21f81ae4c2bea723dbb5e369f2183bb433a Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Fri, 28 Feb 2014 10:20:59 +0900 Subject: [PATCH] Copter: remove low-level handling of mission cmds Now handled by AP_Mission --- ArduCopter/commands.pde | 106 ----------------- ArduCopter/commands_process.pde | 199 -------------------------------- ArduCopter/system.pde | 4 - 3 files changed, 309 deletions(-) delete mode 100644 ArduCopter/commands_process.pde diff --git a/ArduCopter/commands.pde b/ArduCopter/commands.pde index aceb5e9037..3b367ed2b1 100644 --- a/ArduCopter/commands.pde +++ b/ArduCopter/commands.pde @@ -1,106 +1,5 @@ // -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*- -static void init_commands() -{ - g.command_index = NO_COMMAND; - command_nav_index = NO_COMMAND; - command_cond_index = NO_COMMAND; - prev_nav_index = NO_COMMAND; - command_cond_queue.id = NO_COMMAND; - command_nav_queue.id = NO_COMMAND; -} - -// Getters -// ------- -static struct Location get_cmd_with_index(int i) -{ - struct Location temp; - - // Find out proper location in memory by using the start_byte position + the index - // -------------------------------------------------------------------------------- - if (i >= g.command_total) { - // we do not have a valid command to load - // return a WP with a "Blank" id - temp.id = CMD_BLANK; - - // no reason to carry on - return temp; - - }else{ - // we can load a command, we don't process it yet - // read WP position - uint16_t mem = (WP_START_BYTE) + (i * WP_SIZE); - - temp.id = hal.storage->read_byte(mem); - - mem++; - temp.options = hal.storage->read_byte(mem); - - mem++; - temp.p1 = hal.storage->read_byte(mem); - - mem++; - temp.alt = hal.storage->read_dword(mem); // alt is stored in CM! Alt is stored relative! - - mem += 4; - temp.lat = hal.storage->read_dword(mem); // lat is stored in decimal * 10,000,000 - - mem += 4; - temp.lng = hal.storage->read_dword(mem); // lon is stored in decimal * 10,000,000 - } - - // 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 & MASK_OPTIONS_RELATIVE_ALT)){ - //temp.alt += home.alt; - //} - - if(temp.options & WP_OPTION_RELATIVE) { - // If were relative, just offset from home - temp.lat += home.lat; - temp.lng += home.lng; - } - - return temp; -} - -// Setters -// ------- -static void set_cmd_with_index(struct Location temp, int i) -{ - - i = constrain_int16(i, 0, g.command_total.get()); - - // store home as 0 altitude!!! - // Home is always a MAV_CMD_NAV_WAYPOINT (16) - if (i == 0) { - temp.alt = 0; - temp.id = MAV_CMD_NAV_WAYPOINT; - } - - uint16_t mem = WP_START_BYTE + (i * WP_SIZE); - - hal.storage->write_byte(mem, temp.id); - - mem++; - hal.storage->write_byte(mem, temp.options); - - mem++; - hal.storage->write_byte(mem, temp.p1); - - mem++; - hal.storage->write_dword(mem, temp.alt); // Alt is stored in CM! - - mem += 4; - hal.storage->write_dword(mem, temp.lat); // Lat is stored in decimal degrees * 10^7 - - mem += 4; - hal.storage->write_dword(mem, temp.lng); // Long is stored in decimal degrees * 10^7 - - // Make sure our WP_total - if(g.command_total < (i+1)) - g.command_total.set_and_save(i+1); -} - static int32_t get_RTL_alt() { if(g.rtl_altitude <= 0) { @@ -119,11 +18,6 @@ static void init_home() set_home_is_set(true); ahrs.set_home(g_gps->latitude, g_gps->longitude, 0); - // Save Home to EEPROM - // ------------------- - // no need to save this to EPROM - set_cmd_with_index(home, 0); - inertial_nav.setup_home_position(); if (g.log_bitmask & MASK_LOG_CMD) diff --git a/ArduCopter/commands_process.pde b/ArduCopter/commands_process.pde deleted file mode 100644 index 3289cbf3b0..0000000000 --- a/ArduCopter/commands_process.pde +++ /dev/null @@ -1,199 +0,0 @@ -/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*- - -// For changing active command mid-mission -//---------------------------------------- -static void change_command(uint8_t cmd_index) -{ - // check we are in AUTO mode - if (control_mode != AUTO) { - return; - } - - // limit range - cmd_index = min(g.command_total - 1, cmd_index); - - // load command - struct Location temp = get_cmd_with_index(cmd_index); - - // verify it's a nav command - if(temp.id > MAV_CMD_NAV_LAST) { - - }else{ - // clear out command queue - init_commands(); - - // copy command to the queue - command_nav_queue = temp; - command_nav_index = cmd_index; - execute_nav_command(); - } -} - -// update_commands - initiates new navigation commands if we have completed the previous command -// called by 10 Hz loop -static void update_commands() -{ - if(g.command_total <= 1) - return; - - if(command_nav_queue.id == NO_COMMAND) { - // Our queue is empty - // fill command queue with a new command if available, or exit mission - // ------------------------------------------------------------------- - - // find next nav command - int16_t tmp_index; - - if(command_nav_index < g.command_total) { - - // what is the next index for a nav command? - tmp_index = find_next_nav_index(command_nav_index + 1); - - if(tmp_index == -1) { - exit_mission(); - return; - }else{ - command_nav_index = tmp_index; - command_nav_queue = get_cmd_with_index(command_nav_index); - execute_nav_command(); - } - }else{ - // we are out of commands - exit_mission(); - return; - } - } - - if(command_cond_queue.id == NO_COMMAND) { - // Our queue is empty - // fill command queue with a new command if available, or do nothing - // ------------------------------------------------------------------- - - // no nav commands completed yet - if(prev_nav_index == NO_COMMAND) - return; - - if(command_cond_index >= command_nav_index) { - // don't process the fututre - return; - - }else if(command_cond_index == NO_COMMAND) { - // start from scratch - // look at command after the most recent completed nav - command_cond_index = prev_nav_index + 1; - - }else{ - // we've completed 1 cond, look at next command for another - command_cond_index++; - } - - if(command_cond_index < (g.command_total -2)) { - // we're OK to load a new command (last command must be a nav command) - command_cond_queue = get_cmd_with_index(command_cond_index); - - if(command_cond_queue.id > MAV_CMD_CONDITION_LAST) { - // this is a do now command - process_now_command(); - - // clear command queue - command_cond_queue.id = NO_COMMAND; - - }else if(command_cond_queue.id > MAV_CMD_NAV_LAST) { - // this is a conditional command - process_cond_command(); - - }else{ - // this is a nav command, don't process - // clear the command conditional queue and index - prev_nav_index = NO_COMMAND; - command_cond_index = NO_COMMAND; - command_cond_queue.id = NO_COMMAND; - } - - } - } -} - -// execute_nav_command - performs minor initialisation and logging before next navigation command in the queue is executed -static void execute_nav_command(void) -{ - // This is what we report to MAVLINK - g.command_index = command_nav_index; - - // Save CMD to Log - if(g.log_bitmask & MASK_LOG_CMD) - Log_Write_Cmd(g.command_index, &command_nav_queue); - - // clear navigation prameters - reset_nav_params(); - - // Act on the new command - process_nav_command(); - - // clear May indexes to force loading of more commands - // existing May commands are tossed. - command_cond_index = NO_COMMAND; -} - -// verify_commands - high level function to check if navigation and conditional commands have completed -static void verify_commands(void) -{ - // check if navigation command completed - if(verify_nav_command()) { - // clear navigation command queue so next command can be loaded - command_nav_queue.id = NO_COMMAND; - - // store our most recent executed nav command - prev_nav_index = command_nav_index; - - // Wipe existing conditionals - command_cond_index = NO_COMMAND; - command_cond_queue.id = NO_COMMAND; - } - - // check if conditional command completed - if(verify_cond_command()) { - // clear conditional command queue so next command can be loaded - command_cond_queue.id = NO_COMMAND; - } -} - -// Finds the next navgation command in EEPROM -static int16_t find_next_nav_index(int16_t search_index) -{ - Location tmp; - while(search_index < g.command_total) { - tmp = get_cmd_with_index(search_index); - if(tmp.id <= MAV_CMD_NAV_LAST) { - return search_index; - }else{ - search_index++; - } - } - return -1; -} - -static void exit_mission() -{ - // we are out of commands - g.command_index = 255; - - // if we are not on the ground switch to loiter or land - if(!ap.land_complete) { - // try to enter loiter but if that fails land - if (!set_mode(LOITER)) { - set_mode(LAND); - } - }else{ -#if LAND_REQUIRE_MIN_THROTTLE_TO_DISARM == ENABLED - // disarm when the landing detector says we've landed and throttle is at minimum - if (g.rc_3.control_in == 0 || failsafe.radio) { - init_disarm_motors(); - } -#else - // if we've landed it's safe to disarm - init_disarm_motors(); -#endif - } -} - diff --git a/ArduCopter/system.pde b/ArduCopter/system.pde index 4d4648ac1d..68ece585db 100644 --- a/ArduCopter/system.pde +++ b/ArduCopter/system.pde @@ -287,10 +287,6 @@ static void init_ardupilot() init_sonar(); #endif - // initialize commands - // ------------------- - init_commands(); - // initialise the flight mode and aux switch // --------------------------- reset_control_switch();