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

Copter: remove low-level handling of mission cmds 

Now handled by AP_Mission
This commit is contained in:
Randy Mackay 2014-02-28 10:20:59 +09:00
parent 5d568502a0
commit 9f03e21f81
3 changed files with 0 additions and 309 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

106
ArduCopter/commands.pde
View File

@ -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)

199
ArduCopter/commands_process.pde
View File

@ -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
}
}

4
ArduCopter/system.pde
View File

@ -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();