UAV/UAS Tracking Antenna System

Dependencies:   mbed HMC6352

Files at this revision

API Documentation at this revision

Comitter:
danidanko
Date:
Mon May 30 10:42:16 2011 +0000
Commit message:
v1.0

Changed in this revision

HMC6352.lib Show annotated file Show diff for this revision Revisions of this file
mbed.bld Show annotated file Show diff for this revision Revisions of this file
tracker.c Show annotated file Show diff for this revision Revisions of this file
tracker/tracker.h Show annotated file Show diff for this revision Revisions of this file
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/HMC6352.lib	Mon May 30 10:42:16 2011 +0000
@@ -0,0 +1,1 @@
+http://mbed.org/users/aberk/code/HMC6352/#83c0cb554099
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/mbed.bld	Mon May 30 10:42:16 2011 +0000
@@ -0,0 +1,1 @@
+http://mbed.org/users/mbed_official/code/mbed/builds/63bcd7ba4912
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/tracker.c	Mon May 30 10:42:16 2011 +0000
@@ -0,0 +1,264 @@
+/* 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 <tracker.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
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/tracker/tracker.h	Mon May 30 10:42:16 2011 +0000
@@ -0,0 +1,21 @@
+#define PI 3.14159274
+#define ON  1
+#define OFF 0
+typedef unsigned char uchar;
+
+
+float torads(float deg);
+float todegs(float rad);
+
+//Gets string from groundStation and converts to Lat Lon Alt
+void  getlla(float& p_lat, float& p_lon, float& p_alt, float& g_lat, float& g_lon, float& g_alot);   
+
+//converts lat lon alt to ECEF earth centered earth fixed coordinates
+void  lla2ecef(float lat, float lon, float alt, float& x, float& y, float& z);
+
+//converts 4 bytes to float
+float tofloat(uchar b0, uchar b1, uchar b2, uchar b3); 
+
+//this is a testing function it allows me to input a puse width in uS and output it to the servo(s)
+// Allows me to get y = Mx + B.. it's not used in the main program but is usefull
+void  servo_calib();