mirror of https://github.com/ArduPilot/ardupilot
uncrustify ArduCopter/commands_process.pde
This commit is contained in:
uncrustify
Pat Hickey
parent
b6dd8aa592
commit
1e2c01d8f6
|
|
@ -4,220 +4,220 @@
|
|||
//----------------------------------------
|
||||
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);
|
||||
//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);
|
||||
// load command
|
||||
struct Location temp = get_cmd_with_index(cmd_index);
|
||||
|
||||
//Serial.printf("loading cmd: %d with id:%d\n", cmd_index, temp.id);
|
||||
//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"));
|
||||
// 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();
|
||||
}else{
|
||||
// clear out command queue
|
||||
init_commands();
|
||||
|
||||
// copy command to the queue
|
||||
command_nav_queue = temp;
|
||||
command_nav_index = cmd_index;
|
||||
execute_nav_command();
|
||||
}
|
||||
// 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);
|
||||
//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(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
|
||||
// -------------------------------------------------------------------
|
||||
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;
|
||||
// find next nav command
|
||||
int16_t tmp_index;
|
||||
|
||||
if(command_nav_index < g.command_total){
|
||||
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);
|
||||
// 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();
|
||||
}
|
||||
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);
|
||||
// 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);
|
||||
// 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){
|
||||
fast_corner = false;
|
||||
}else{
|
||||
int32_t temp = get_bearing_cd(&next_WP, &tmp_loc) - original_target_bearing;
|
||||
temp = wrap_180(temp);
|
||||
fast_corner = labs(temp) < 6000;
|
||||
}
|
||||
}
|
||||
}else{
|
||||
// we are out of commands
|
||||
exit_mission();
|
||||
return;
|
||||
}
|
||||
}
|
||||
if(tmp_loc.lat == 0) {
|
||||
fast_corner = false;
|
||||
}else{
|
||||
int32_t temp = get_bearing_cd(&next_WP, &tmp_loc) - original_target_bearing;
|
||||
temp = wrap_180(temp);
|
||||
fast_corner = labs(temp) < 6000;
|
||||
}
|
||||
}
|
||||
}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
|
||||
// -------------------------------------------------------------------
|
||||
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;
|
||||
// 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;
|
||||
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 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++;
|
||||
}
|
||||
}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_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();
|
||||
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;
|
||||
// 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 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;
|
||||
}
|
||||
}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;
|
||||
// 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);
|
||||
// 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();
|
||||
// clear navigation prameters
|
||||
reset_nav_params();
|
||||
|
||||
// Act on the new command
|
||||
process_nav_command();
|
||||
// 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;
|
||||
// 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;
|
||||
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;
|
||||
// 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;
|
||||
// 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);
|
||||
}
|
||||
}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;
|
||||
}
|
||||
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 - 1){
|
||||
tmp = get_cmd_with_index(search_index);
|
||||
if(tmp.id <= MAV_CMD_NAV_LAST){
|
||||
return search_index;
|
||||
}else{
|
||||
search_index++;
|
||||
}
|
||||
}
|
||||
return -1;
|
||||
Location tmp;
|
||||
while(search_index < g.command_total - 1) {
|
||||
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;
|
||||
// we are out of commands
|
||||
g.command_index = 255;
|
||||
|
||||
// if we are on the ground, enter stabilize, else Land
|
||||
if(land_complete == true){
|
||||
// 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);
|
||||
}
|
||||
}
|
||||
// if we are on the ground, enter stabilize, else Land
|
||||
if(land_complete == true) {
|
||||
// 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);
|
||||
}
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
|
|
|
|||
Loading…
Reference in New Issue