ardupilot/ArduCopterMega/commands_process.pde

166 lines
4.4 KiB
Plaintext
Raw Normal View History

/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: t -*-
// called by 10 Hz loop
// --------------------
void update_commands(void)
{
// This function loads commands into three buffers
// when a new command is loaded, it is processed with process_XXX()
// ----------------------------------------------------------------
if(home_is_set == false){
return; // don't do commands
}
if(control_mode == AUTO){
load_next_command_from_EEPROM();
process_next_command();
}else if(control_mode == GCS_AUTO){
/*if( there is a command recieved )
process_next_command();
*/
}
}
void verify_commands(void)
{
if(verify_must()){
command_must_index = NO_COMMAND;
}
if(verify_may()){
command_may_index = NO_COMMAND;
}
}
void load_next_command_from_EEPROM()
{
// fetch next command if the next command queue is empty
// -----------------------------------------------------
if(next_command.id == NO_COMMAND){
next_command = get_wp_with_index(g.waypoint_index + 1);
//if(next_command.id != NO_COMMAND){
//SendDebug("MSG <load_next_command> fetch found new cmd from list at index ");
//SendDebug((g.waypoint_index + 1),DEC);
//SendDebug(" with cmd id ");
//SendDebugln(next_command.id,DEC);
//}
}
// If the preload failed, return or just Loiter
// generate a dynamic command for RTL
// --------------------------------------------
if(next_command.id == NO_COMMAND){
// we are out of commands!
gcs.send_text(SEVERITY_LOW,"out of commands!");
//SendDebug("MSG <load_next_command> out of commands, g.waypoint_index: ");
//SendDebugln(g.waypoint_index,DEC);
handle_no_commands();
}
}
void process_next_command()
{
// these are waypoint/Must commands
// ---------------------------------
if (command_must_index == 0){ // no current command loaded
if (next_command.id < MAV_CMD_NAV_LAST ){
increment_WP_index();
//save_command_index(); // to Recover from in air Restart
command_must_index = g.waypoint_index;
//SendDebug("MSG <process_next_command> new command_must_id ");
//SendDebug(next_command.id,DEC);
//SendDebug(" index:");
//SendDebugln(command_must_index,DEC);
if (g.log_bitmask & MASK_LOG_CMD)
Log_Write_Cmd(g.waypoint_index, &next_command);
process_must();
}
}
// these are May commands
// ----------------------
if (command_may_index == 0){
if (next_command.id > MAV_CMD_NAV_LAST && next_command.id < MAV_CMD_CONDITION_LAST ){
increment_WP_index();// this command is from the command list in EEPROM
command_may_index = g.waypoint_index;
//Serial.print("new command_may_index ");
//Serial.println(command_may_index,DEC);
if (g.log_bitmask & MASK_LOG_CMD)
Log_Write_Cmd(g.waypoint_index, &next_command);
process_may();
}
}
// these are do it now commands
// ---------------------------
if (next_command.id > MAV_CMD_CONDITION_LAST){
increment_WP_index();// this command is from the command list in EEPROM
if (g.log_bitmask & MASK_LOG_CMD)
Log_Write_Cmd(g.waypoint_index, &next_command);
process_now();
}
}
/*
These functions implement the waypoint commands.
*/
void process_must()
{
//SendDebug("process must index: ");
//SendDebugln(command_must_index,DEC);
gcs.send_text(SEVERITY_LOW,"New cmd: <process_must>");
gcs.send_message(MSG_COMMAND_LIST, g.waypoint_index);
// clear May indexes
command_may_index = 0;
command_may_ID = 0;
command_must_ID = next_command.id;
// loads the waypoint into Next_WP struct
// --------------------------------------
//set_next_WP(&next_command);
// invalidate command so a new one is loaded
// -----------------------------------------
handle_process_must();
next_command.id = NO_COMMAND;
}
void process_may()
{
//Serial.print("process_may cmd# ");
//Serial.println(g.waypoint_index,DEC);
command_may_ID = next_command.id;
gcs.send_text(SEVERITY_LOW,"<process_may> New may command loaded:");
gcs.send_message(MSG_COMMAND_LIST, g.waypoint_index);
handle_process_may();
// invalidate command so a new one is loaded
// -----------------------------------------
next_command.id = NO_COMMAND;
}
void process_now()
{
const float t5 = 100000.0;
//Serial.print("process_now cmd# ");
//Serial.println(g.waypoint_index,DEC);
gcs.send_text(SEVERITY_LOW, "<process_now> New now command loaded: ");
gcs.send_message(MSG_COMMAND_LIST, g.waypoint_index);
handle_process_now();
// invalidate command so a new one is loaded
// -----------------------------------------
next_command.id = NO_COMMAND;
}