Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-24 01:28:29 -04:00
Code Issues Packages Projects Releases Wiki Activity

uncrustify libraries/AP_Navigation/examples/Navigation/Navigation.pde

Browse Source
This commit is contained in:
uncrustify 2012-08-16 23:20:45 -07:00 committed by Pat Hickey
parent 763d36ecf0
commit fd9c31e39f
1 changed files with 87 additions and 87 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

174
libraries/AP_Navigation/examples/Navigation/Navigation.pde
View File

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