UAV/UAS Tracking Antenna System
Dependencies: mbed HMC6352 tracker
tracking.c
- Committer:
- danidanko
- Date:
- 2011-05-30
- Revision:
- 0:c63fa6b360a0
File content as of revision 0:c63fa6b360a0:
/* Tracking Antenna for UCSD AUVSI Team DANIEL BEDENKO d.bedenko@gmail.com * This is the final project class ECE118 .. This will work with any autopilot * software as long as the Serial Input communication must be in the following format * 1 Byte 0xFF [Start Byte] * 4 bytes Plane Lat [32 bit float little endian] * 4 bytes Plane Lon * 4 bytes Plane Alt * 4 bytes Ground Lat [32 bit float little endian] * 4 bytes Ground Lon * 4 bytes Gorund Alt * 1 byte 0xFE [End Byte] * Also one would have to recalibrate the servos to their system this particular system * uses one 360 Servo from ServoCity for Azimuth tracking and one 180 Standard Servo for * elevation Tracking. Theoretically this will work for a moble ground station * Use at own risk. */ #include <HMC6352.h> #include <tracking.h> //Magnetometer HMC6352 compass(p28, p27); //serial ports Serial usb_232(USBTX, USBRX); // tx, rx Debugging Port not used in actual Op Serial groundStation(p9, p10); // tx, rx //leds DigitalOut led1(LED1); DigitalOut led2(LED2); DigitalOut led3(LED3); DigitalOut led4(LED4); int main() { // wgs84 g for ground p for plane lat[deg] lon[deg] alt[m] float compas, p_lat, p_lon, p_alt, g_lat, g_lon, g_alt; // everything in meters ecef NED is north east down ecef_xyz is the vector pointing // from the ground to the plane in ecef. NED is the North East Down vector from // ground to plane float p_x, p_y, p_z, g_x, g_y, g_z, ecef_x, ecef_y, ecef_z, N, E, D; // groundStation pos in Radians float g_latR, g_lonR; // Azimuth and Elevation Angles [Radians] float Az, Ev; // Servo commands int Az_uS, Ev_uS; bool sanDiego; //Set up serial settings Bits/S 8N1 is default usb_232.baud(57600); groundStation.baud(57600); //HMC6352 Magnetometor Settings //Continuous mode, periodic set/reset, 20Hz rate. compass.setOpMode(HMC6352_CONTINUOUS, 1, 20); // Set up Leds OFF led1 = led2 = led3 = led4 = OFF; // Set up Servos for Azimuth and Elevation PwmOut Az_servo(p21); Az_servo.period(0.020); PwmOut Ev_servo(p22); Ev_servo.period(0.020); // Set up Initial Servo Position Az = torads(90); Ev = torads(30); Az_uS = (int)(0.9195*Az*Az + 121.18*Az + 1308.43); Ev_uS = (int)(-1.223*Ev*Ev - 589.16*Ev + 2376.78); /* * Get serial string and figure out if you're in Lexington Part, MD or in San Diego, CA * This is competition specific for the san diego team, you have to change these * values if you're not flying in san diego or maryland * Magnetic Declination in San Diego = 12.1167 EAST Turn on LED 3 * Magnetic Declination in Lexington Park, MD = 10.9833 West Turn on LED 4 * if SanDiego = 1 you in sandiego else you're in marryland */ while (1) //set up while { //set up servo for turning on Az_servo.pulsewidth_us(Az_uS); Ev_servo.pulsewidth_us(Ev_uS); wait(0.05); compas = (compass.sample() / 10.0); usb_232.printf("[%f] Heading \n", compas); led2 = !led2; //blink untill heading is set getlla(p_lat, p_lon, p_alt, g_lat, g_lon, g_alt); if (g_lon < -100){sanDiego = 1; led3 = ON;}//-100 long is about center of US else{sanDiego = 0; led4 = ON;} if (sanDiego == 1 && compas > 346.88 && compas < 348.88) {usb_232.printf("[%f] TRUE NORTH = 347.8 Magnetic North -- SD, CA\n", compas); led2 = OFF; //reference found enter main loop Ev = torads(1); Ev_uS = (int)(-1.223*Ev*Ev - 589.16*Ev + 2376.78); Ev_servo.pulsewidth_us(Ev_uS); break; } if (sanDiego == 0 && compas < 11.98 && compas > 9.98) {usb_232.printf("[%f] TRUE NORTH = 10.98 Magnetic North -- Lexington Park, MD\n", compas); led2 = OFF; //reference found enter main loop Ev = torads(1); Ev_uS = (int)(-1.223*Ev*Ev - 589.16*Ev + 2376.78); Ev_servo.pulsewidth_us(Ev_uS); break; } }//end of set up while loop you have now pointed the antenna tracker at TRUE NORTH //Start Main loop while(1){ /* This main loop gets plane coordinates from the serial port * converts it to a NED vector pointing from the ground statio * to the airplane and then sends servos to point at it */ // get ground and plane coordinates getlla(p_lat, p_lon, p_alt, g_lat, g_lon, g_alt); //convert airplane to ecef lla2ecef(p_lat, p_lon, p_alt, p_x, p_y, p_z); //convert ground to ecef lla2ecef(g_lat, g_lon, g_alt, g_x, g_y, g_z); //subtract plane from ground to get the difference vector ecef_x = p_x - g_x; ecef_y = p_y - g_y; ecef_z = p_z - g_z; g_latR= torads(g_lat); g_lonR= torads(g_lon); /* translation ecef xyz to north east down * * TE2L = [ -sin(glat)*cos(glon) -sin(glat)*sin(glon) cos(glat) * -sin(glon) cos(glon) 0 * -cos(glat)*cos(glon) -cos(glat)*sin(glon) -sin(glat) ] * NED = TE2L*ecef */ N = -sin(g_latR)*cos(g_lonR)*ecef_x + -sin(g_latR)*sin(g_lonR)*ecef_y + cos(g_latR)*ecef_z; E = -sin(g_lonR)*ecef_x + cos(g_lonR)*ecef_y; D = -cos(g_latR)*cos(g_lonR)*ecef_x -cos(g_latR)*sin(g_lonR)*ecef_y + -sin(g_latR)*ecef_z; D = (-1)*D; //we are on the ground looking up! :) Az = atan2(N,E); Ev = abs(atan2(D, sqrt(N*N + E*E))); // this will always be positive // if plane is on the ground next to ground station point north if(g_lat == p_lat && g_lon == p_lon) {Ev = 0; Az = PI/2;} //Aling Az is from 0 to 2*pi - no negative numbers if (Az < 0) Az = Az + 2*PI; //from here on out it depends on the how the servo pulse width relates to //angle her my azimuth servo is defined from 270 -> -90 deg 1900 - 1120 uS //and the elevation Servo is defines from 0 - 180 deg 0500 - 2400 uS //blackout region where the azimuth will swing back to Az-PI //And Ev = pi - Ev if (Az > 17*PI/12 && Az < 19*PI/12) {Az = Az - PI; Ev = PI - Ev;} // 270 -> -90 if (Az > 3*PI/2 && Az < 2*PI) Az = Az - 2*PI; //now get the uS .. the values below are from polyfit matlab //from your servo calibrations to the second degree Az_uS = (int)(0.9195*Az*Az + 121.18*Az + 1308.43); Ev_uS = (int)(-1.223*Ev*Ev - 589.16*Ev + 2376.78); // Send Servo Commands Az_servo.pulsewidth_us(Az_uS); Ev_servo.pulsewidth_us(Ev_uS); // usb_232.printf("[%2.2f Az] [%i Az uS] [%2.2f Ev] [%i Az uS] \n", todegs(Az), Az_uS, todegs(Ev), Ev_uS ); //usb_232.printf("%2.2f Azimuth %2.2f North %2.2f East\n", todegs(Az), N, E ); // usb_232.printf("[%f %f %f ] Plane \n", p_lat, p_lon, p_alt ); // usb_232.printf("[%f %f %f ] Plane ecef \n ", p_x, p_y, p_z ); // usb_232.printf("[%f %f %f ] Ground \n", g_lat, g_lon, g_alt); // usb_232.printf("[%f %f %f ] Ground ecef \n ", g_x, g_y, g_z ); } } // end main void lla2ecef(float lat, float lon, float alt, float& x, float& y, float& z) { // WGS84 ellipsoid constants: float a = 6378137; //earth semi-major axis float esqrd = 0.006694379990141; //Eccentricity float N; //calculation lat = torads(lat); lon = torads(lon); //The length of normal (N) to the ellipsoid N = a / sqrt(1 - esqrd * sin( lat ) * sin( lat ) ) ; x = (N+alt) * cos(lat) * cos(lon); y = (N+alt) * cos(lat) * sin(lon); z = ((1-esqrd) * N + alt) * sin(lat); } float torads(float deg) { return (deg*PI) / 180 ; //in rads } float todegs(float rads) { return (rads*180) / PI ; //in degs } void servo_calib(void) { PwmOut servo(p22); servo.period(0.020); char pwm_uS[5]; int pwm_uS_i; while(1) { usb_232.printf(" \n Enter PWM in uS [0600-2400] : "); usb_232.scanf("%s", pwm_uS); pwm_uS_i = atof ( pwm_uS ); servo.pulsewidth_us(pwm_uS_i); usb_232.printf("\n You Entered: %i ", pwm_uS_i); } } void getlla(float& p_lat, float& p_lon, float& p_alt, float& g_lat, float& g_lon, float& g_alt) { uchar msg[26]; do{ while( (msg[0] = groundStation.getc()) != 0xff); // wait for the start of communication string for(int i=1; i<26; i++) { msg[i] = groundStation.getc(); //fill message string } }while(msg[25] != 0xfe); // make sure the last character is 0xFE otherwise re read led1 = !led1; //blink :) //airplane p_lat = tofloat(msg[4], msg[3], msg[2], msg[1]); p_lon = tofloat(msg[8], msg[7], msg[6], msg[5]); p_alt = tofloat(msg[12], msg[11], msg[10], msg[9]); //ground g_lat = tofloat(msg[16], msg[15], msg[14], msg[13]); g_lon = tofloat(msg[20], msg[19], msg[18], msg[17]); g_alt = tofloat(msg[24], msg[23], msg[22], msg[21]); } float tofloat(uchar b0, uchar b1, uchar b2, uchar b3) { uchar b[] = {b3, b2, b1, b0}; float *fp = (float *)b; return *fp; }