tracking.c

00001 /* Tracking Antenna for UCSD AUVSI Team DANIEL BEDENKO d.bedenko@gmail.com
00002  * This is the final project class ECE118 .. This will work with any autopilot 
00003  * software as long as the Serial Input communication must be in the following format
00004  * 1 Byte  0xFF        [Start  Byte] 
00005  * 4 bytes Plane Lat   [32 bit float little endian]
00006  * 4 bytes Plane Lon
00007  * 4 bytes Plane Alt
00008  * 4 bytes Ground Lat  [32 bit float little endian]
00009  * 4 bytes Ground Lon
00010  * 4 bytes Gorund Alt
00011  * 1 byte 0xFE         [End Byte]
00012  * Also one would have to recalibrate the servos to their system this particular system
00013  * uses one 360 Servo from ServoCity for Azimuth tracking and one 180 Standard Servo for 
00014  * elevation Tracking. Theoretically this will work for a moble ground station
00015  * Use at own risk.
00016  */
00017 
00018 #include <HMC6352.h>
00019 #include <tracking.h>
00020 
00021 
00022 //Magnetometer
00023 HMC6352 compass(p28, p27);
00024 //serial ports
00025 Serial usb_232(USBTX, USBRX);      // tx, rx Debugging Port not used in actual Op
00026 Serial groundStation(p9, p10);     // tx, rx
00027 //leds
00028 DigitalOut led1(LED1); DigitalOut led2(LED2); DigitalOut led3(LED3); DigitalOut led4(LED4);
00029 
00030 
00031 int main() {
00032 // wgs84 g for ground p for plane lat[deg] lon[deg] alt[m]
00033 float compas, p_lat, p_lon, p_alt, g_lat, g_lon, g_alt;
00034 // everything in meters ecef  NED is north east down ecef_xyz is the vector pointing
00035 // from the ground to the plane in ecef. NED is the North East Down vector from 
00036 // ground to plane
00037 float p_x, p_y, p_z, g_x, g_y, g_z, ecef_x, ecef_y, ecef_z, N, E, D;
00038 // groundStation pos in Radians
00039 float g_latR, g_lonR; 
00040 // Azimuth and Elevation Angles [Radians]
00041 float Az, Ev; 
00042 // Servo commands
00043 int Az_uS, Ev_uS;
00044 bool  sanDiego;                       
00045 
00046 //Set up serial settings Bits/S 8N1 is default
00047    usb_232.baud(57600);
00048    groundStation.baud(57600);
00049 //HMC6352 Magnetometor Settings //Continuous mode, periodic set/reset, 20Hz rate. 
00050    compass.setOpMode(HMC6352_CONTINUOUS, 1, 20);
00051 // Set up Leds OFF
00052    led1 = led2 = led3 = led4 = OFF;
00053    
00054 // Set up Servos for Azimuth and Elevation
00055  PwmOut Az_servo(p21);
00056  Az_servo.period(0.020); 
00057  PwmOut Ev_servo(p22);
00058  Ev_servo.period(0.020); 
00059  
00060  // Set up Initial Servo Position
00061   Az = torads(90);
00062   Ev = torads(30);
00063   Az_uS = (int)(0.9195*Az*Az + 121.18*Az + 1308.43);   
00064   Ev_uS = (int)(-1.223*Ev*Ev - 589.16*Ev + 2376.78);
00065  
00066  
00067 /*
00068  *  Get serial string and figure out if you're in Lexington Part, MD or in San Diego, CA
00069  *  This is competition specific for the san diego team, you have to change these 
00070  *  values if you're not flying in san diego or maryland
00071  *  Magnetic Declination in San Diego = 12.1167  EAST    Turn on LED 3
00072  *  Magnetic Declination in Lexington Park, MD = 10.9833 West Turn on LED 4
00073  *  if SanDiego = 1 you in sandiego else you're in marryland
00074  */
00075   while (1) //set up while
00076     {
00077       
00078       
00079       //set up servo for turning on
00080         
00081       Az_servo.pulsewidth_us(Az_uS);   
00082       Ev_servo.pulsewidth_us(Ev_uS);
00083       
00084        
00085       wait(0.05);
00086       compas = (compass.sample() / 10.0); 
00087       usb_232.printf("[%f] Heading \n", compas);
00088       led2 = !led2;                              //blink untill heading is set 
00089       
00090       getlla(p_lat, p_lon, p_alt, g_lat, g_lon, g_alt); 
00091       
00092       if (g_lon < -100){sanDiego = 1; led3 = ON;}//-100 long is about center of US
00093       else{sanDiego = 0; led4 = ON;}
00094         
00095       if (sanDiego == 1 && compas > 346.88 && compas < 348.88)    
00096         {usb_232.printf("[%f] TRUE NORTH = 347.8 Magnetic North -- SD, CA\n", compas);
00097          led2 = OFF;  //reference found enter main loop
00098          Ev = torads(1);
00099          Ev_uS = (int)(-1.223*Ev*Ev - 589.16*Ev + 2376.78);
00100          Ev_servo.pulsewidth_us(Ev_uS);
00101          break;
00102         }         
00103       if (sanDiego == 0 && compas < 11.98 && compas > 9.98)
00104         {usb_232.printf("[%f] TRUE NORTH = 10.98 Magnetic North -- Lexington Park, MD\n", compas);
00105          led2 = OFF;  //reference found enter main loop
00106          Ev = torads(1);
00107          Ev_uS = (int)(-1.223*Ev*Ev - 589.16*Ev + 2376.78);
00108          Ev_servo.pulsewidth_us(Ev_uS);
00109          break;
00110         }
00111      }//end of set up while loop you have now pointed the antenna tracker at TRUE NORTH
00112 
00113 
00114 //Start Main loop
00115  while(1){
00116     
00117   /* This main loop gets plane coordinates from the serial port 
00118    * converts it to a NED vector pointing from the ground statio
00119    * to the airplane and then sends servos to point at it
00120    */
00121    
00122   // get ground and plane coordinates
00123   getlla(p_lat, p_lon, p_alt, g_lat, g_lon, g_alt);
00124   
00125   //convert airplane to ecef
00126   lla2ecef(p_lat, p_lon, p_alt, p_x, p_y, p_z);
00127   
00128   //convert ground to ecef
00129   lla2ecef(g_lat, g_lon, g_alt, g_x, g_y, g_z);
00130   
00131   //subtract plane from ground to get the difference vector
00132   ecef_x = p_x - g_x;
00133   ecef_y = p_y - g_y;
00134   ecef_z = p_z - g_z;
00135   
00136   g_latR= torads(g_lat); 
00137   g_lonR= torads(g_lon);  
00138     
00139   /* translation ecef xyz to north east down
00140    *
00141    *  TE2L = [ -sin(glat)*cos(glon)  -sin(glat)*sin(glon)  cos(glat)  
00142    *           -sin(glon)             cos(glon)               0       
00143    *           -cos(glat)*cos(glon)  -cos(glat)*sin(glon) -sin(glat)  ]
00144    *  NED = TE2L*ecef
00145    */
00146   
00147   N = -sin(g_latR)*cos(g_lonR)*ecef_x + -sin(g_latR)*sin(g_lonR)*ecef_y + cos(g_latR)*ecef_z;
00148   E = -sin(g_lonR)*ecef_x + cos(g_lonR)*ecef_y;
00149   D = -cos(g_latR)*cos(g_lonR)*ecef_x  -cos(g_latR)*sin(g_lonR)*ecef_y + -sin(g_latR)*ecef_z;
00150   D = (-1)*D; //we are on the ground looking up! :) 
00151   
00152   Az = atan2(N,E);
00153   Ev = abs(atan2(D, sqrt(N*N + E*E))); // this will always be positive
00154   
00155  // if plane is on the ground next to ground station point north
00156  if(g_lat == p_lat && g_lon == p_lon)
00157   {Ev = 0; Az = PI/2;}
00158   
00159   //Aling Az is from 0 to 2*pi - no negative numbers 
00160   if (Az < 0)
00161      Az = Az + 2*PI;
00162   
00163   //from here on out it depends on the how the servo pulse width relates to
00164   //angle her my azimuth servo is defined from 270 -> -90 deg 1900 - 1120 uS
00165   //and the elevation Servo is defines from 0 - 180 deg  0500 - 2400 uS
00166   
00167   
00168   //blackout region where the azimuth will swing back to Az-PI
00169   //And Ev = pi - Ev
00170     if (Az > 17*PI/12 && Az < 19*PI/12)
00171         {Az = Az - PI;
00172          Ev = PI - Ev;}
00173   
00174   // 270 -> -90
00175      if (Az > 3*PI/2 && Az < 2*PI)
00176      Az = Az - 2*PI;
00177      
00178   //now get the uS .. the values below are from polyfit matlab
00179   //from your servo calibrations to the second degree
00180   
00181   Az_uS = (int)(0.9195*Az*Az + 121.18*Az + 1308.43);   
00182   Ev_uS = (int)(-1.223*Ev*Ev - 589.16*Ev + 2376.78);
00183   
00184   // Send Servo Commands
00185    Az_servo.pulsewidth_us(Az_uS);   
00186    Ev_servo.pulsewidth_us(Ev_uS);
00187   
00188     
00189  // usb_232.printf("[%2.2f Az]  [%i Az uS]  [%2.2f Ev] [%i Az uS] \n", todegs(Az), Az_uS, todegs(Ev), Ev_uS );
00190  
00191  //usb_232.printf("%2.2f Azimuth %2.2f  North %2.2f East\n", todegs(Az), N, E ); 
00192  // usb_232.printf("[%f   %f    %f ] Plane \n", p_lat, p_lon, p_alt );
00193  // usb_232.printf("[%f   %f    %f ] Plane ecef \n ", p_x, p_y, p_z );
00194  // usb_232.printf("[%f   %f    %f ] Ground \n", g_lat, g_lon, g_alt);
00195  // usb_232.printf("[%f   %f    %f ] Ground ecef \n ", g_x, g_y, g_z );
00196 }
00197     
00198 } // end main
00199 void  lla2ecef(float lat, float lon, float alt, float& x, float& y, float& z)
00200 {
00201   // WGS84 ellipsoid constants:
00202    float a = 6378137; //earth semi-major axis
00203    float esqrd = 0.006694379990141; //Eccentricity
00204    float N; 
00205    //calculation
00206     lat = torads(lat);
00207     lon = torads(lon);
00208      //The length of normal (N) to the ellipsoid
00209     N = a / sqrt(1 - esqrd * sin( lat ) * sin( lat ) ) ;
00210     x = (N+alt) * cos(lat) * cos(lon);
00211     y = (N+alt) * cos(lat) * sin(lon);
00212     z = ((1-esqrd) * N + alt) * sin(lat);
00213 
00214 }
00215 float torads(float deg)
00216 {
00217     return (deg*PI) / 180 ; //in rads
00218 }
00219 float todegs(float rads)
00220 {
00221     return (rads*180) / PI ; //in degs
00222 }
00223 void servo_calib(void)
00224 {
00225  PwmOut servo(p22);
00226  servo.period(0.020);       
00227  char pwm_uS[5];
00228  int pwm_uS_i;
00229     while(1)
00230             { 
00231             usb_232.printf(" \n Enter PWM in uS [0600-2400] : ");
00232             usb_232.scanf("%s", pwm_uS);
00233             pwm_uS_i = atof ( pwm_uS );
00234             servo.pulsewidth_us(pwm_uS_i);
00235             usb_232.printf("\n You Entered: %i ", pwm_uS_i);
00236             }
00237 }
00238 void  getlla(float& p_lat, float& p_lon, float& p_alt, float& g_lat, float& g_lon, float& g_alt)
00239 {
00240      uchar msg[26];
00241      do{
00242         while( (msg[0] = groundStation.getc()) != 0xff); // wait for the start of communication string
00243             for(int i=1; i<26; i++)  {
00244             msg[i] = groundStation.getc(); //fill message string
00245             }
00246        }while(msg[25] != 0xfe); // make sure the last character is 0xFE otherwise re read
00247              led1 = !led1; //blink :) 
00248     //airplane
00249     p_lat = tofloat(msg[4], msg[3], msg[2], msg[1]);
00250     p_lon = tofloat(msg[8], msg[7], msg[6], msg[5]);
00251     p_alt = tofloat(msg[12], msg[11], msg[10], msg[9]);
00252     //ground
00253     g_lat = tofloat(msg[16], msg[15], msg[14], msg[13]);
00254     g_lon = tofloat(msg[20], msg[19], msg[18], msg[17]);
00255     g_alt = tofloat(msg[24], msg[23], msg[22], msg[21]);
00256 
00257 }
00258 float tofloat(uchar b0, uchar b1, uchar b2, uchar b3)
00259 {
00260     uchar b[] = {b3, b2, b1, b0};
00261     float *fp = (float *)b;
00262     return *fp;
00263 }