aepko/Ardupilot2
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

uncrustify ArduCopter/commands_logic.pde

Browse Source
This commit is contained in:
uncrustify 2012-08-21 19:19:50 -07:00 committed by Pat Hickey
parent f783ace0de
commit 6791da4ee7
1 changed files with 574 additions and 574 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

90
ArduCopter/commands_logic.pde
View File

@ -5,7 +5,7 @@
/********************************************************************************/ /********************************************************************************/
static void process_nav_command() static void process_nav_command()
{ {
switch(command_nav_queue.id){ switch(command_nav_queue.id) {
case MAV_CMD_NAV_TAKEOFF: // 22 case MAV_CMD_NAV_TAKEOFF: // 22
do_takeoff(); do_takeoff();
@ -49,7 +49,7 @@ static void process_nav_command()
static void process_cond_command() static void process_cond_command()
{ {
switch(command_cond_queue.id){ switch(command_cond_queue.id) {
case MAV_CMD_CONDITION_DELAY: // 112 case MAV_CMD_CONDITION_DELAY: // 112
do_wait_delay(); do_wait_delay();
@ -74,7 +74,7 @@ static void process_cond_command()
static void process_now_command() static void process_now_command()
{ {
switch(command_cond_queue.id){ switch(command_cond_queue.id) {
case MAV_CMD_DO_JUMP: // 177 case MAV_CMD_DO_JUMP: // 177
do_jump(); do_jump();
@ -148,7 +148,7 @@ static bool verify_must()
break; break;
case MAV_CMD_NAV_LAND: case MAV_CMD_NAV_LAND:
if(g.sonar_enabled == true){ if(g.sonar_enabled == true) {
return verify_land_sonar(); return verify_land_sonar();
}else{ }else{
return verify_land_baro(); return verify_land_baro();
@ -273,7 +273,7 @@ static void do_nav_wp()
// this is the delay, stored in seconds and expanded to millis // this is the delay, stored in seconds and expanded to millis
loiter_time_max = command_nav_queue.p1 * 1000; loiter_time_max = command_nav_queue.p1 * 1000;
if((next_WP.options & WP_OPTION_ALT_REQUIRED) == false){ if((next_WP.options & WP_OPTION_ALT_REQUIRED) == false) {
wp_verify_byte |= NAV_ALTITUDE; wp_verify_byte |= NAV_ALTITUDE;
} }
} }
@ -304,7 +304,7 @@ static void do_loiter_unlimited()
wp_control = LOITER_MODE; wp_control = LOITER_MODE;
//Serial.println("dloi "); //Serial.println("dloi ");
if(command_nav_queue.lat == 0){ if(command_nav_queue.lat == 0) {
set_next_WP(&current_loc); set_next_WP(&current_loc);
wp_control = LOITER_MODE; wp_control = LOITER_MODE;
}else{ }else{
@ -317,9 +317,9 @@ static void do_loiter_turns()
{ {
wp_control = CIRCLE_MODE; wp_control = CIRCLE_MODE;
if(command_nav_queue.lat == 0){ if(command_nav_queue.lat == 0) {
// allow user to specify just the altitude // allow user to specify just the altitude
if(command_nav_queue.alt > 0){ if(command_nav_queue.alt > 0) {
current_loc.alt = command_nav_queue.alt; current_loc.alt = command_nav_queue.alt;
} }
set_next_WP(&current_loc); set_next_WP(&current_loc);
@ -340,7 +340,7 @@ static void do_loiter_turns()
static void do_loiter_time() static void do_loiter_time()
{ {
if(command_nav_queue.lat == 0){ if(command_nav_queue.lat == 0) {
wp_control = LOITER_MODE; wp_control = LOITER_MODE;
loiter_time = millis(); loiter_time = millis();
set_next_WP(&current_loc); set_next_WP(&current_loc);
@ -359,7 +359,7 @@ static void do_loiter_time()
static bool verify_takeoff() static bool verify_takeoff()
{ {
// wait until we are ready! // wait until we are ready!
if(g.rc_3.control_in == 0){ if(g.rc_3.control_in == 0) {
return false; return false;
} }
// are we above our target altitude? // are we above our target altitude?
@ -369,7 +369,7 @@ static bool verify_takeoff()
// called at 10hz // called at 10hz
static bool verify_land_sonar() static bool verify_land_sonar()
{ {
if(current_loc.alt > 300){ if(current_loc.alt > 300) {
wp_control = LOITER_MODE; wp_control = LOITER_MODE;
ground_detector = 0; ground_detector = 0;
}else{ }else{
@ -378,20 +378,20 @@ static bool verify_land_sonar()
landing_boost = min(landing_boost, 40); landing_boost = min(landing_boost, 40);
} }
if(current_loc.alt < 200 ){ if(current_loc.alt < 200 ) {
wp_control = NO_NAV_MODE; wp_control = NO_NAV_MODE;
} }
if(current_loc.alt < 150 ){ if(current_loc.alt < 150 ) {
// if we are low or don't seem to be decending much, increment ground detector // if we are low or don't seem to be decending much, increment ground detector
if(current_loc.alt < 80 || abs(climb_rate) < 20) { if(current_loc.alt < 80 || abs(climb_rate) < 20) {
landing_boost++; // reduce the throttle at twice the normal rate landing_boost++; // reduce the throttle at twice the normal rate
if(ground_detector < 30) { if(ground_detector < 30) {
ground_detector++; ground_detector++;
}else if (ground_detector == 30){ }else if (ground_detector == 30) {
land_complete = true; land_complete = true;
if(g.rc_3.control_in == 0){ if(g.rc_3.control_in == 0) {
ground_detector++; ground_detector++;
init_disarm_motors(); init_disarm_motors();
} }
@ -404,7 +404,7 @@ static bool verify_land_sonar()
static bool verify_land_baro() static bool verify_land_baro()
{ {
if(current_loc.alt > 300){ if(current_loc.alt > 300) {
wp_control = LOITER_MODE; wp_control = LOITER_MODE;
ground_detector = 0; ground_detector = 0;
}else{ }else{
@ -413,19 +413,19 @@ static bool verify_land_baro()
landing_boost = min(landing_boost, 40); landing_boost = min(landing_boost, 40);
} }
if(current_loc.alt < 100 ){ if(current_loc.alt < 100 ) {
wp_control = NO_NAV_MODE; wp_control = NO_NAV_MODE;
} }
if(current_loc.alt < 200 ){ if(current_loc.alt < 200 ) {
if(abs(climb_rate) < 40) { if(abs(climb_rate) < 40) {
landing_boost++; landing_boost++;
if(ground_detector < 30) { if(ground_detector < 30) {
ground_detector++; ground_detector++;
}else if (ground_detector == 30){ }else if (ground_detector == 30) {
land_complete = true; land_complete = true;
if(g.rc_3.control_in == 0){ if(g.rc_3.control_in == 0) {
ground_detector++; ground_detector++;
init_disarm_motors(); init_disarm_motors();
} }
@ -439,9 +439,9 @@ static bool verify_land_baro()
static bool verify_nav_wp() static bool verify_nav_wp()
{ {
// Altitude checking // Altitude checking
if(next_WP.options & WP_OPTION_ALT_REQUIRED){ if(next_WP.options & WP_OPTION_ALT_REQUIRED) {
// we desire a certain minimum altitude // we desire a certain minimum altitude
if(alt_change_flag == REACHED_ALT){ if(alt_change_flag == REACHED_ALT) {
// we have reached that altitude // we have reached that altitude
wp_verify_byte |= NAV_ALTITUDE; wp_verify_byte |= NAV_ALTITUDE;
@ -449,20 +449,20 @@ static bool verify_nav_wp()
} }
// Did we pass the WP? // Distance checking // Did we pass the WP? // Distance checking
if((wp_distance <= (waypoint_radius * 100)) || check_missed_wp()){ if((wp_distance <= (waypoint_radius * 100)) || check_missed_wp()) {
// if we have a distance calc error, wp_distance may be less than 0 // if we have a distance calc error, wp_distance may be less than 0
if(wp_distance > 0){ if(wp_distance > 0) {
wp_verify_byte |= NAV_LOCATION; wp_verify_byte |= NAV_LOCATION;
if(loiter_time == 0){ if(loiter_time == 0) {
loiter_time = millis(); loiter_time = millis();
} }
} }
} }
// Hold at Waypoint checking, we cant move on until this is OK // Hold at Waypoint checking, we cant move on until this is OK
if(wp_verify_byte & NAV_LOCATION){ if(wp_verify_byte & NAV_LOCATION) {
// we have reached our goal // we have reached our goal
// loiter at the WP // loiter at the WP
@ -475,7 +475,7 @@ static bool verify_nav_wp()
} }
} }
if(wp_verify_byte >= 7){ if(wp_verify_byte >= 7) {
//if(wp_verify_byte & NAV_LOCATION){ //if(wp_verify_byte & NAV_LOCATION){
char message[30]; char message[30];
sprintf(message,"Reached Command #%i",command_nav_index); sprintf(message,"Reached Command #%i",command_nav_index);
@ -490,7 +490,7 @@ static bool verify_nav_wp()
static bool verify_loiter_unlimited() static bool verify_loiter_unlimited()
{ {
if(wp_control == WP_MODE && wp_distance <= (g.waypoint_radius * 100)){ if(wp_control == WP_MODE && wp_distance <= (g.waypoint_radius * 100)) {
// switch to position hold // switch to position hold
wp_control = LOITER_MODE; wp_control = LOITER_MODE;
} }
@ -499,12 +499,12 @@ static bool verify_loiter_unlimited()
static bool verify_loiter_time() static bool verify_loiter_time()
{ {
if(wp_control == LOITER_MODE){ if(wp_control == LOITER_MODE) {
if ((millis() - loiter_time) > loiter_time_max) { if ((millis() - loiter_time) > loiter_time_max) {
return true; return true;
} }
} }
if(wp_control == WP_MODE && wp_distance <= (g.waypoint_radius * 100)){ if(wp_control == WP_MODE && wp_distance <= (g.waypoint_radius * 100)) {
// reset our loiter time // reset our loiter time
loiter_time = millis(); loiter_time = millis();
// switch to position hold // switch to position hold
@ -533,7 +533,7 @@ static bool verify_RTL()
wp_control = WP_MODE; wp_control = WP_MODE;
// Did we pass the WP? // Distance checking // Did we pass the WP? // Distance checking
if((wp_distance <= (g.waypoint_radius * 100)) || check_missed_wp()){ if((wp_distance <= (g.waypoint_radius * 100)) || check_missed_wp()) {
wp_control = LOITER_MODE; wp_control = LOITER_MODE;
//gcs_send_text_P(SEVERITY_LOW,PSTR("Reached home")); //gcs_send_text_P(SEVERITY_LOW,PSTR("Reached home"));
@ -590,10 +590,10 @@ static void do_yaw()
if(command_yaw_dir > 1) if(command_yaw_dir > 1)
command_yaw_dir = 0; // 0 = counter clockwise, 1 = clockwise command_yaw_dir = 0; // 0 = counter clockwise, 1 = clockwise
if(command_yaw_relative == 1){ if(command_yaw_relative == 1) {
// relative // relative
command_yaw_delta = command_cond_queue.alt * 100; command_yaw_delta = command_cond_queue.alt * 100;
if(command_yaw_dir == 0){ // 0 = counter clockwise, 1 = clockwise if(command_yaw_dir == 0) { // 0 = counter clockwise, 1 = clockwise
command_yaw_end = command_yaw_start - command_yaw_delta; command_yaw_end = command_yaw_start - command_yaw_delta;
}else{ }else{
command_yaw_end = command_yaw_start + command_yaw_delta; command_yaw_end = command_yaw_start + command_yaw_delta;
@ -604,14 +604,14 @@ static void do_yaw()
command_yaw_end = command_cond_queue.alt * 100; command_yaw_end = command_cond_queue.alt * 100;
// calculate the delta travel in deg * 100 // calculate the delta travel in deg * 100
if(command_yaw_dir == 0){ // 0 = counter clockwise, 1 = clockwise if(command_yaw_dir == 0) { // 0 = counter clockwise, 1 = clockwise
if(command_yaw_start > command_yaw_end){ if(command_yaw_start > command_yaw_end) {
command_yaw_delta = command_yaw_start - command_yaw_end; command_yaw_delta = command_yaw_start - command_yaw_end;
}else{ }else{
command_yaw_delta = 36000 + (command_yaw_start - command_yaw_end); command_yaw_delta = 36000 + (command_yaw_start - command_yaw_end);
} }
}else{ }else{
if(command_yaw_start >= command_yaw_end){ if(command_yaw_start >= command_yaw_end) {
command_yaw_delta = 36000 - (command_yaw_start - command_yaw_end); command_yaw_delta = 36000 - (command_yaw_start - command_yaw_end);
}else{ }else{
command_yaw_delta = command_yaw_end - command_yaw_start; command_yaw_delta = command_yaw_end - command_yaw_start;
@ -632,7 +632,7 @@ static void do_yaw()
static bool verify_wait_delay() static bool verify_wait_delay()
{ {
//Serial.print("vwd"); //Serial.print("vwd");
if ((unsigned)(millis() - condition_start) > (unsigned)condition_value){ if ((unsigned)(millis() - condition_start) > (unsigned)condition_value) {
//Serial.println("y"); //Serial.println("y");
condition_value = 0; condition_value = 0;
return true; return true;
@ -644,14 +644,14 @@ static bool verify_wait_delay()
static bool verify_change_alt() static bool verify_change_alt()
{ {
//Serial.printf("change_alt, ca:%d, na:%d\n", (int)current_loc.alt, (int)next_WP.alt); //Serial.printf("change_alt, ca:%d, na:%d\n", (int)current_loc.alt, (int)next_WP.alt);
if ((int32_t)condition_start < next_WP.alt){ if ((int32_t)condition_start < next_WP.alt) {
// we are going higer // we are going higer
if(current_loc.alt > next_WP.alt){ if(current_loc.alt > next_WP.alt) {
return true; return true;
} }
}else{ }else{
// we are going lower // we are going lower
if(current_loc.alt < next_WP.alt){ if(current_loc.alt < next_WP.alt) {
return true; return true;
} }
} }
@ -661,7 +661,7 @@ static bool verify_change_alt()
static bool verify_within_distance() static bool verify_within_distance()
{ {
//Serial.printf("cond dist :%d\n", (int)condition_value); //Serial.printf("cond dist :%d\n", (int)condition_value);
if (wp_distance < condition_value){ if (wp_distance < condition_value) {
condition_value = 0; condition_value = 0;
return true; return true;
} }
@ -672,7 +672,7 @@ static bool verify_yaw()
{ {
//Serial.printf("vyaw %d\n", (int)(nav_yaw/100)); //Serial.printf("vyaw %d\n", (int)(nav_yaw/100));
if((millis() - command_yaw_start_time) > command_yaw_time){ if((millis() - command_yaw_start_time) > command_yaw_time) {
// time out // time out
// make sure we hold at the final desired yaw angle // make sure we hold at the final desired yaw angle
nav_yaw = command_yaw_end; nav_yaw = command_yaw_end;
@ -747,7 +747,7 @@ static void do_target_yaw()
{ {
yaw_tracking = command_cond_queue.p1; yaw_tracking = command_cond_queue.p1;
if(yaw_tracking == MAV_ROI_LOCATION){ if(yaw_tracking == MAV_ROI_LOCATION) {
target_WP = command_cond_queue; target_WP = command_cond_queue;
} }
} }
@ -766,7 +766,7 @@ static void do_jump()
//Serial.printf("do Jump: %d\n", jump); //Serial.printf("do Jump: %d\n", jump);
if(jump == -10){ if(jump == -10) {
//Serial.printf("Fresh Jump\n"); //Serial.printf("Fresh Jump\n");
// we use a locally stored index for jump // we use a locally stored index for jump
jump = command_cond_queue.lat; jump = command_cond_queue.lat;
@ -778,7 +778,7 @@ static void do_jump()
jump--; jump--;
change_command(command_cond_queue.p1); change_command(command_cond_queue.p1);
} else if (jump == 0){ } else if (jump == 0) {
//Serial.printf("Did last jump\n"); //Serial.printf("Did last jump\n");
// we're done, move along // we're done, move along
jump = -11; jump = -11;