ardupilot/ArduCopter/commands_process.pde

/// -*- 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)
{
    //Serial.printf("change_command: %d\n",cmd_index );
    // limit range
    cmd_index = min(g.command_total - 1, cmd_index);

    // load command
    struct Location temp = get_cmd_with_index(cmd_index);

    //Serial.printf("loading cmd: %d with id:%d\n", cmd_index, temp.id);

    // verify it's a nav command
    if(temp.id > MAV_CMD_NAV_LAST) {
        //gcs_send_text_P(SEVERITY_LOW,PSTR("error: non-Nav cmd"));

    }else{
        // clear out command queue
        init_commands();

        // copy command to the queue
        command_nav_queue               = temp;
        command_nav_index               = cmd_index;
        execute_nav_command();
    }
}

// called by 10 Hz loop
// --------------------
static void update_commands()
{
    //Serial.printf("update_commands: %d\n",increment );
    // A: if we do not have any commands there is nothing to do
    // B: We have completed the mission, don't redo the mission
    // XXX debug
    //uint8_t tmp = g.command_index.get();
    //Serial.printf("command_index %u \n", tmp);

    if(g.command_total <= 1 || g.command_index >= 255)
        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();
            }

            // try to load the next nav for better speed control
            // find_next_nav_index takes the next guess to start the search
            tmp_index = find_next_nav_index(command_nav_index + 1);

            // Fast corner management
            // ----------------------
            if(tmp_index == -1) {
                // there are no more commands left
            }else{
                // we have at least one more cmd left
                Location tmp_loc = get_cmd_with_index(tmp_index);

                if(tmp_loc.lat == 0) {
                    ap.fast_corner = false;
                }else{
                    int32_t temp = get_bearing_cd(&next_WP, &tmp_loc) - original_target_bearing;
                    temp = wrap_180(temp);
                    ap.fast_corner = labs(temp) < 6000;
                }

                // If we try and stop at a corner, lets reset our desired speed to prevent
                // too much jerkyness.
				if(false == ap.fast_corner){
					reset_desired_speed();
				}
            }

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

        }
    }
}

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

// called with GPS navigation update - not constantly
static void verify_commands(void)
{
    if(verify_must()) {
        //Serial.printf("verified must cmd %d\n" , command_nav_index);
        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;

    }else{
        //Serial.printf("verified must false %d\n" , command_nav_index);
    }

    if(verify_may()) {
        //Serial.printf("verified may cmd %d\n" , command_cond_index);
        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 on the ground, enter stabilize, else Land
    if(ap.land_complete) {
        // we will disarm the motors after landing.
    }else{
        // If the approach altitude is valid (above 1m), do approach, else land
        if(g.rtl_approach_alt == 0) {
            set_mode(LAND);
        }else{
            set_mode(LOITER);
            set_new_altitude(g.rtl_approach_alt);
        }
    }

}