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