mirror of https://github.com/ArduPilot/ardupilot
uncrustify ArduCopter/commands_process.pde
This commit is contained in:
parent
b6dd8aa592
commit
1e2c01d8f6
|
@ -14,7 +14,7 @@ static void change_command(uint8_t 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
|
// verify it's a nav command
|
||||||
if(temp.id > MAV_CMD_NAV_LAST){
|
if(temp.id > MAV_CMD_NAV_LAST) {
|
||||||
//gcs_send_text_P(SEVERITY_LOW,PSTR("error: non-Nav cmd"));
|
//gcs_send_text_P(SEVERITY_LOW,PSTR("error: non-Nav cmd"));
|
||||||
|
|
||||||
}else{
|
}else{
|
||||||
|
@ -42,7 +42,7 @@ static void update_commands()
|
||||||
if(g.command_total <= 1 || g.command_index >= 255)
|
if(g.command_total <= 1 || g.command_index >= 255)
|
||||||
return;
|
return;
|
||||||
|
|
||||||
if(command_nav_queue.id == NO_COMMAND){
|
if(command_nav_queue.id == NO_COMMAND) {
|
||||||
// Our queue is empty
|
// Our queue is empty
|
||||||
// fill command queue with a new command if available, or exit mission
|
// fill command queue with a new command if available, or exit mission
|
||||||
// -------------------------------------------------------------------
|
// -------------------------------------------------------------------
|
||||||
|
@ -50,12 +50,12 @@ static void update_commands()
|
||||||
// find next nav command
|
// find next nav command
|
||||||
int16_t tmp_index;
|
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?
|
// what is the next index for a nav command?
|
||||||
tmp_index = find_next_nav_index(command_nav_index + 1);
|
tmp_index = find_next_nav_index(command_nav_index + 1);
|
||||||
|
|
||||||
if(tmp_index == -1){
|
if(tmp_index == -1) {
|
||||||
exit_mission();
|
exit_mission();
|
||||||
return;
|
return;
|
||||||
}else{
|
}else{
|
||||||
|
@ -70,13 +70,13 @@ static void update_commands()
|
||||||
|
|
||||||
// Fast corner management
|
// Fast corner management
|
||||||
// ----------------------
|
// ----------------------
|
||||||
if(tmp_index == -1){
|
if(tmp_index == -1) {
|
||||||
// there are no more commands left
|
// there are no more commands left
|
||||||
}else{
|
}else{
|
||||||
// we have at least one more cmd left
|
// we have at least one more cmd left
|
||||||
Location tmp_loc = get_cmd_with_index(tmp_index);
|
Location tmp_loc = get_cmd_with_index(tmp_index);
|
||||||
|
|
||||||
if(tmp_loc.lat == 0){
|
if(tmp_loc.lat == 0) {
|
||||||
fast_corner = false;
|
fast_corner = false;
|
||||||
}else{
|
}else{
|
||||||
int32_t temp = get_bearing_cd(&next_WP, &tmp_loc) - original_target_bearing;
|
int32_t temp = get_bearing_cd(&next_WP, &tmp_loc) - original_target_bearing;
|
||||||
|
@ -91,7 +91,7 @@ static void update_commands()
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
if(command_cond_queue.id == NO_COMMAND){
|
if(command_cond_queue.id == NO_COMMAND) {
|
||||||
// Our queue is empty
|
// Our queue is empty
|
||||||
// fill command queue with a new command if available, or do nothing
|
// fill command queue with a new command if available, or do nothing
|
||||||
// -------------------------------------------------------------------
|
// -------------------------------------------------------------------
|
||||||
|
@ -100,11 +100,11 @@ static void update_commands()
|
||||||
if(prev_nav_index == NO_COMMAND)
|
if(prev_nav_index == NO_COMMAND)
|
||||||
return;
|
return;
|
||||||
|
|
||||||
if(command_cond_index >= command_nav_index){
|
if(command_cond_index >= command_nav_index) {
|
||||||
// don't process the fututre
|
// don't process the fututre
|
||||||
return;
|
return;
|
||||||
|
|
||||||
}else if(command_cond_index == NO_COMMAND){
|
}else if(command_cond_index == NO_COMMAND) {
|
||||||
// start from scratch
|
// start from scratch
|
||||||
// look at command after the most recent completed nav
|
// look at command after the most recent completed nav
|
||||||
command_cond_index = prev_nav_index + 1;
|
command_cond_index = prev_nav_index + 1;
|
||||||
|
@ -114,18 +114,18 @@ static void update_commands()
|
||||||
command_cond_index++;
|
command_cond_index++;
|
||||||
}
|
}
|
||||||
|
|
||||||
if(command_cond_index < (g.command_total -2)){
|
if(command_cond_index < (g.command_total -2)) {
|
||||||
// we're OK to load a new command (last command must be a nav command)
|
// 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);
|
command_cond_queue = get_cmd_with_index(command_cond_index);
|
||||||
|
|
||||||
if(command_cond_queue.id > MAV_CMD_CONDITION_LAST){
|
if(command_cond_queue.id > MAV_CMD_CONDITION_LAST) {
|
||||||
// this is a do now command
|
// this is a do now command
|
||||||
process_now_command();
|
process_now_command();
|
||||||
|
|
||||||
// clear command queue
|
// clear command queue
|
||||||
command_cond_queue.id = NO_COMMAND;
|
command_cond_queue.id = NO_COMMAND;
|
||||||
|
|
||||||
}else if(command_cond_queue.id > MAV_CMD_NAV_LAST){
|
}else if(command_cond_queue.id > MAV_CMD_NAV_LAST) {
|
||||||
// this is a conditional command
|
// this is a conditional command
|
||||||
process_cond_command();
|
process_cond_command();
|
||||||
|
|
||||||
|
@ -164,7 +164,7 @@ static void execute_nav_command(void)
|
||||||
// called with GPS navigation update - not constantly
|
// called with GPS navigation update - not constantly
|
||||||
static void verify_commands(void)
|
static void verify_commands(void)
|
||||||
{
|
{
|
||||||
if(verify_must()){
|
if(verify_must()) {
|
||||||
//Serial.printf("verified must cmd %d\n" , command_nav_index);
|
//Serial.printf("verified must cmd %d\n" , command_nav_index);
|
||||||
command_nav_queue.id = NO_COMMAND;
|
command_nav_queue.id = NO_COMMAND;
|
||||||
|
|
||||||
|
@ -179,7 +179,7 @@ static void verify_commands(void)
|
||||||
//Serial.printf("verified must false %d\n" , command_nav_index);
|
//Serial.printf("verified must false %d\n" , command_nav_index);
|
||||||
}
|
}
|
||||||
|
|
||||||
if(verify_may()){
|
if(verify_may()) {
|
||||||
//Serial.printf("verified may cmd %d\n" , command_cond_index);
|
//Serial.printf("verified may cmd %d\n" , command_cond_index);
|
||||||
command_cond_queue.id = NO_COMMAND;
|
command_cond_queue.id = NO_COMMAND;
|
||||||
}
|
}
|
||||||
|
@ -189,9 +189,9 @@ static void verify_commands(void)
|
||||||
static int16_t find_next_nav_index(int16_t search_index)
|
static int16_t find_next_nav_index(int16_t search_index)
|
||||||
{
|
{
|
||||||
Location tmp;
|
Location tmp;
|
||||||
while(search_index < g.command_total - 1){
|
while(search_index < g.command_total - 1) {
|
||||||
tmp = get_cmd_with_index(search_index);
|
tmp = get_cmd_with_index(search_index);
|
||||||
if(tmp.id <= MAV_CMD_NAV_LAST){
|
if(tmp.id <= MAV_CMD_NAV_LAST) {
|
||||||
return search_index;
|
return search_index;
|
||||||
}else{
|
}else{
|
||||||
search_index++;
|
search_index++;
|
||||||
|
@ -207,11 +207,11 @@ static void exit_mission()
|
||||||
g.command_index = 255;
|
g.command_index = 255;
|
||||||
|
|
||||||
// if we are on the ground, enter stabilize, else Land
|
// if we are on the ground, enter stabilize, else Land
|
||||||
if(land_complete == true){
|
if(land_complete == true) {
|
||||||
// we will disarm the motors after landing.
|
// we will disarm the motors after landing.
|
||||||
}else{
|
}else{
|
||||||
// If the approach altitude is valid (above 1m), do approach, else land
|
// If the approach altitude is valid (above 1m), do approach, else land
|
||||||
if(g.rtl_approach_alt == 0){
|
if(g.rtl_approach_alt == 0) {
|
||||||
set_mode(LAND);
|
set_mode(LAND);
|
||||||
}else{
|
}else{
|
||||||
set_mode(LOITER);
|
set_mode(LOITER);
|
||||||
|
|
Loading…
Reference in New Issue