Diff: tracking.c

Revision:

0:c63fa6b360a0

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/tracking.c	Mon May 30 21:31:17 2011 +0000
@@ -0,0 +1,263 @@
+/* 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;
+}
\ No newline at end of file