From fd9c31e39f88d73c680ad0f3f37edab160a7b030 Mon Sep 17 00:00:00 2001 From: uncrustify Date: Thu, 16 Aug 2012 23:20:45 -0700 Subject: [PATCH] uncrustify libraries/AP_Navigation/examples/Navigation/Navigation.pde --- .../examples/Navigation/Navigation.pde | 174 +++++++++--------- 1 file changed, 87 insertions(+), 87 deletions(-) diff --git a/libraries/AP_Navigation/examples/Navigation/Navigation.pde b/libraries/AP_Navigation/examples/Navigation/Navigation.pde index 19b0b54483..08520c3962 100644 --- a/libraries/AP_Navigation/examples/Navigation/Navigation.pde +++ b/libraries/AP_Navigation/examples/Navigation/Navigation.pde @@ -1,8 +1,8 @@ /* - -This test assumes you are at the LOWl demo Airport - -*/ + * + * This test assumes you are at the LOWl demo Airport + * + */ #include "Waypoints.h" #include "Navigation.h" @@ -11,7 +11,7 @@ This test assumes you are at the LOWl demo Airport AP_GPS_IMU gps; -Navigation nav((GPS *) &gps); +Navigation nav((GPS *) &gps); AP_RC rc; #define CH_ROLL 0 @@ -33,73 +33,73 @@ int16_t radio_in[4]; void setup() { - Serial.begin(38400); - Serial.println("Navigation test"); - Waypoints::WP location_B = {0, 0, 74260, 472586364, 113366012}; - nav.set_next_wp(location_B); - rc.init(radio_trim); + Serial.begin(38400); + Serial.println("Navigation test"); + Waypoints::WP location_B = {0, 0, 74260, 472586364, 113366012}; + nav.set_next_wp(location_B); + rc.init(radio_trim); } void loop() { - delay(20); - gps.update(); - rc.read_pwm(); - for(int y=0; y<4; y++) { - //rc.set_ch_pwm(y, rc.input[y]); // send to Servos - radio_in[y] = rc.input[y]; - } - - servo_out[CH_ROLL] = ((radio_in[CH_ROLL] - radio_trim[CH_ROLL]) * 45 * REVERSE_ROLL) / 500; - servo_out[CH_PITCH] = ((radio_in[CH_PITCH] - radio_trim[CH_PITCH]) * 45 * REVERSE_PITCH) / 500; - servo_out[CH_RUDDER] = ((radio_in[CH_RUDDER] - radio_trim[CH_RUDDER]) * 45 * REVERSE_RUDDER) / 500; - servo_out[CH_THROTTLE] = (float)(radio_in[CH_THROTTLE] - CH3_MIN) / (float)(CH3_MAX - CH3_MIN) *100; - servo_out[CH_THROTTLE] = constrain(servo_out[CH_THROTTLE], 0, 100); + delay(20); + gps.update(); + rc.read_pwm(); + for(int y=0; y<4; y++) { + //rc.set_ch_pwm(y, rc.input[y]); // send to Servos + radio_in[y] = rc.input[y]; + } - output_Xplane(); - if(gps.new_data /* && gps.fix */ ){ - if(did_init_home == 0){ - did_init_home = 1; - Waypoints::WP wp = {0, 0, gps.altitude, gps.lattitude, gps.longitude}; - nav.set_home(wp); - Serial.println("MSG init home"); - }else{ - nav.update_gps(); - } - //print_loc(); - } + servo_out[CH_ROLL] = ((radio_in[CH_ROLL] - radio_trim[CH_ROLL]) * 45 * REVERSE_ROLL) / 500; + servo_out[CH_PITCH] = ((radio_in[CH_PITCH] - radio_trim[CH_PITCH]) * 45 * REVERSE_PITCH) / 500; + servo_out[CH_RUDDER] = ((radio_in[CH_RUDDER] - radio_trim[CH_RUDDER]) * 45 * REVERSE_RUDDER) / 500; + servo_out[CH_THROTTLE] = (float)(radio_in[CH_THROTTLE] - CH3_MIN) / (float)(CH3_MAX - CH3_MIN) *100; + servo_out[CH_THROTTLE] = constrain(servo_out[CH_THROTTLE], 0, 100); + + output_Xplane(); + if(gps.new_data /* && gps.fix */ ) { + if(did_init_home == 0) { + did_init_home = 1; + Waypoints::WP wp = {0, 0, gps.altitude, gps.lattitude, gps.longitude}; + nav.set_home(wp); + Serial.println("MSG init home"); + }else{ + nav.update_gps(); + } + //print_loc(); + } } void print_loc() { - Serial.print("MSG GPS: "); - Serial.print(nav.location.lat, DEC); - Serial.print(", "); - Serial.print(nav.location.lng, DEC); - Serial.print(", "); - Serial.print(nav.location.alt, DEC); - Serial.print("\tDistance = "); - Serial.print(nav.distance, DEC); - Serial.print(" Bearing = "); - Serial.print(nav.bearing/100, DEC); - Serial.print(" Bearing err = "); - Serial.print(nav.bearing_error/100, DEC); - Serial.print(" alt err = "); - Serial.print(nav.altitude_error/100, DEC); - Serial.print(" Alt above home = "); - Serial.println(nav.altitude_above_home/100, DEC); + Serial.print("MSG GPS: "); + Serial.print(nav.location.lat, DEC); + Serial.print(", "); + Serial.print(nav.location.lng, DEC); + Serial.print(", "); + Serial.print(nav.location.alt, DEC); + Serial.print("\tDistance = "); + Serial.print(nav.distance, DEC); + Serial.print(" Bearing = "); + Serial.print(nav.bearing/100, DEC); + Serial.print(" Bearing err = "); + Serial.print(nav.bearing_error/100, DEC); + Serial.print(" alt err = "); + Serial.print(nav.altitude_error/100, DEC); + Serial.print(" Alt above home = "); + Serial.println(nav.altitude_above_home/100, DEC); } void print_pwm() { - Serial.print("ch1 "); - Serial.print(rc.input[CH_ROLL],DEC); - Serial.print("\tch2: "); - Serial.print(rc.input[CH_PITCH],DEC); - Serial.print("\tch3 :"); - Serial.print(rc.input[CH_THROTTLE],DEC); - Serial.print("\tch4 :"); - Serial.println(rc.input[CH_RUDDER],DEC); + Serial.print("ch1 "); + Serial.print(rc.input[CH_ROLL],DEC); + Serial.print("\tch2: "); + Serial.print(rc.input[CH_PITCH],DEC); + Serial.print("\tch3 :"); + Serial.print(rc.input[CH_THROTTLE],DEC); + Serial.print("\tch4 :"); + Serial.println(rc.input[CH_RUDDER],DEC); } @@ -110,43 +110,43 @@ byte out_buffer[32]; void output_Xplane(void) { - // output real-time sensor data - Serial.print("AAA"); // Message preamble - output_int((int)(servo_out[CH_ROLL]*100)); // 0 bytes 0,1 - output_int((int)(servo_out[CH_PITCH]*100)); // 1 bytes 2,3 - output_int((int)(servo_out[CH_THROTTLE])); // 2 bytes 4,5 - output_int((int)(servo_out[CH_RUDDER]*100)); // 3 bytes 6,7 - output_int((int)nav.distance); // 4 bytes 8,9 - output_int((int)nav.bearing_error); // 5 bytes 10,11 - output_int((int)nav.altitude_error); // 6 bytes 12,13 - output_int(0); // 7 bytes 14,15 - output_byte(1); // 8 bytes 16 - output_byte(1); // 9 bytes 17 - - // print out the buffer and checksum - // --------------------------------- - print_buffer(); + // output real-time sensor data + Serial.print("AAA"); // Message preamble + output_int((int)(servo_out[CH_ROLL]*100)); // 0 bytes 0,1 + output_int((int)(servo_out[CH_PITCH]*100)); // 1 bytes 2,3 + output_int((int)(servo_out[CH_THROTTLE])); // 2 bytes 4,5 + output_int((int)(servo_out[CH_RUDDER]*100)); // 3 bytes 6,7 + output_int((int)nav.distance); // 4 bytes 8,9 + output_int((int)nav.bearing_error); // 5 bytes 10,11 + output_int((int)nav.altitude_error); // 6 bytes 12,13 + output_int(0); // 7 bytes 14,15 + output_byte(1); // 8 bytes 16 + output_byte(1); // 9 bytes 17 + + // print out the buffer and checksum + // --------------------------------- + print_buffer(); } void output_int(int value) { - //buf_len += 2; - out_buffer[buf_len++] = value & 0xff; - out_buffer[buf_len++] = (value >> 8) & 0xff; + //buf_len += 2; + out_buffer[buf_len++] = value & 0xff; + out_buffer[buf_len++] = (value >> 8) & 0xff; } void output_byte(byte value) { - out_buffer[buf_len++] = value; + out_buffer[buf_len++] = value; } void print_buffer() { - byte ck_a = 0; - byte ck_b = 0; - for (byte i = 0;i < buf_len; i++){ - Serial.print (out_buffer[i]); - } - Serial.print('\r'); - Serial.print('\n'); - buf_len = 0; + byte ck_a = 0; + byte ck_b = 0; + for (byte i = 0; i < buf_len; i++) { + Serial.print (out_buffer[i]); + } + Serial.print('\r'); + Serial.print('\n'); + buf_len = 0; }