ardupilot/libraries/AP_Navigation/examples/Navigation/Navigation.pde

153 lines
3.6 KiB
Plaintext
Raw Normal View History

/*
This test assumes you are at the LOWl demo Airport
*/
#include "Waypoints.h"
#include "Navigation.h"
#include "AP_GPS_IMU.h"
#include "AP_RC.h"
AP_GPS_IMU gps;
Navigation nav((GPS *) &gps);
AP_RC rc;
#define CH_ROLL 0
#define CH_PITCH 1
#define CH_THROTTLE 2
#define CH_RUDDER 3
#define CH3_MIN 1100
#define CH3_MAX 1900
#define REVERSE_RUDDER 1
#define REVERSE_ROLL 1
#define REVERSE_PITCH 1
int8_t did_init_home;
int16_t servo_out[4];
int16_t radio_trim[4] = {1500,1500,1100,1500};
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);
}
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);
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);
}
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);
}
byte buf_len = 0;
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();
}
void output_int(int value)
{
//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;
}
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;
}