da bada / Mbed 2 deprecated TrackerFinal

UAV/UAS Tracking Antenna System

Dependencies:   mbed HMC6352

tracker.c@0:a1f6917f071e, 2011-05-30 (annotated)

Committer:
danidanko
Date:
Mon May 30 10:42:16 2011 +0000
Revision:
0:a1f6917f071e
v1.0

Who changed what in which revision?

UserRevisionLine numberNew contents of line
danidanko 0:a1f6917f071e 1 /* Tracking Antenna for UCSD AUVSI Team DANIEL BEDENKO d.bedenko@gmail.com
danidanko 0:a1f6917f071e 2 * This is the final project class ECE118 .. This will work with any autopilot
danidanko 0:a1f6917f071e 3 * software as long as the Serial Input communication must be in the following format
danidanko 0:a1f6917f071e 4 * 1 Byte 0xFF [Start Byte]
danidanko 0:a1f6917f071e 5 * 4 bytes Plane Lat [32 bit float little endian]
danidanko 0:a1f6917f071e 6 * 4 bytes Plane Lon
danidanko 0:a1f6917f071e 7 * 4 bytes Plane Alt
danidanko 0:a1f6917f071e 8 * 4 bytes Ground Lat [32 bit float little endian]
danidanko 0:a1f6917f071e 9 * 4 bytes Ground Lon
danidanko 0:a1f6917f071e 10 * 4 bytes Gorund Alt
danidanko 0:a1f6917f071e 11 * 1 byte 0xFE [End Byte]
danidanko 0:a1f6917f071e 12 * Also one would have to recalibrate the servos to their system this particular system
danidanko 0:a1f6917f071e 13 * uses one 360 Servo from ServoCity for Azimuth tracking and one 180 Standard Servo for
danidanko 0:a1f6917f071e 14 * elevation Tracking. Theoretically this will work for a moble ground station
danidanko 0:a1f6917f071e 15 * Use at own risk.
danidanko 0:a1f6917f071e 16 */
danidanko 0:a1f6917f071e 17
danidanko 0:a1f6917f071e 18 #include <HMC6352.h>
danidanko 0:a1f6917f071e 19 #include <tracker.h>
danidanko 0:a1f6917f071e 20
danidanko 0:a1f6917f071e 21
danidanko 0:a1f6917f071e 22 //Magnetometer
danidanko 0:a1f6917f071e 23 HMC6352 compass(p28, p27);
danidanko 0:a1f6917f071e 24 //serial ports
danidanko 0:a1f6917f071e 25 Serial usb_232(USBTX, USBRX); // tx, rx Debugging Port not used in actual Op
danidanko 0:a1f6917f071e 26 Serial groundStation(p9, p10); // tx, rx
danidanko 0:a1f6917f071e 27 //leds
danidanko 0:a1f6917f071e 28 DigitalOut led1(LED1); DigitalOut led2(LED2); DigitalOut led3(LED3); DigitalOut led4(LED4);
danidanko 0:a1f6917f071e 29
danidanko 0:a1f6917f071e 30
danidanko 0:a1f6917f071e 31 int main() {
danidanko 0:a1f6917f071e 32 // wgs84 g for ground p for plane lat[deg] lon[deg] alt[m]
danidanko 0:a1f6917f071e 33 float compas, p_lat, p_lon, p_alt, g_lat, g_lon, g_alt;
danidanko 0:a1f6917f071e 34 // everything in meters ecef NED is north east down ecef_xyz is the vector pointing
danidanko 0:a1f6917f071e 35 // from the ground to the plane in ecef. NED is the North East Down vector from
danidanko 0:a1f6917f071e 36 // ground to plane
danidanko 0:a1f6917f071e 37 float p_x, p_y, p_z, g_x, g_y, g_z, ecef_x, ecef_y, ecef_z, N, E, D;
danidanko 0:a1f6917f071e 38 // groundStation pos in Radians
danidanko 0:a1f6917f071e 39 float g_latR, g_lonR;
danidanko 0:a1f6917f071e 40 // Azimuth and Elevation Angles [Radians]
danidanko 0:a1f6917f071e 41 float Az, Ev;
danidanko 0:a1f6917f071e 42 // Servo commands
danidanko 0:a1f6917f071e 43 int Az_uS, Ev_uS;
danidanko 0:a1f6917f071e 44 bool sanDiego;
danidanko 0:a1f6917f071e 45
danidanko 0:a1f6917f071e 46 //Set up serial settings Bits/S 8N1 is default
danidanko 0:a1f6917f071e 47 usb_232.baud(57600);
danidanko 0:a1f6917f071e 48 groundStation.baud(57600);
danidanko 0:a1f6917f071e 49 //HMC6352 Magnetometor Settings //Continuous mode, periodic set/reset, 20Hz rate.
danidanko 0:a1f6917f071e 50 compass.setOpMode(HMC6352_CONTINUOUS, 1, 20);
danidanko 0:a1f6917f071e 51 // Set up Leds OFF
danidanko 0:a1f6917f071e 52 led1 = led2 = led3 = led4 = OFF;
danidanko 0:a1f6917f071e 53
danidanko 0:a1f6917f071e 54 // Set up Servos for Azimuth and Elevation
danidanko 0:a1f6917f071e 55 PwmOut Az_servo(p21);
danidanko 0:a1f6917f071e 56 Az_servo.period(0.020);
danidanko 0:a1f6917f071e 57 PwmOut Ev_servo(p22);
danidanko 0:a1f6917f071e 58 Ev_servo.period(0.020);
danidanko 0:a1f6917f071e 59
danidanko 0:a1f6917f071e 60 // Set up Initial Servo Position
danidanko 0:a1f6917f071e 61 Az = torads(90);
danidanko 0:a1f6917f071e 62 Ev = torads(30);
danidanko 0:a1f6917f071e 63 Az_uS = (int)(0.9195*Az*Az + 121.18*Az + 1308.43);
danidanko 0:a1f6917f071e 64 Ev_uS = (int)(-1.223*Ev*Ev - 589.16*Ev + 2376.78);
danidanko 0:a1f6917f071e 65
danidanko 0:a1f6917f071e 66
danidanko 0:a1f6917f071e 67 /*
danidanko 0:a1f6917f071e 68 * Get serial string and figure out if you're in Lexington Part, MD or in San Diego, CA
danidanko 0:a1f6917f071e 69 * This is competition specific for the san diego team, you have to change these
danidanko 0:a1f6917f071e 70 * values if you're not flying in san diego or maryland
danidanko 0:a1f6917f071e 71 * Magnetic Declination in San Diego = 12.1167 EAST Turn on LED 3
danidanko 0:a1f6917f071e 72 * Magnetic Declination in Lexington Park, MD = 10.9833 West Turn on LED 4
danidanko 0:a1f6917f071e 73 * if SanDiego = 1 you in sandiego else you're in marryland
danidanko 0:a1f6917f071e 74 */
danidanko 0:a1f6917f071e 75 while (1) //set up while
danidanko 0:a1f6917f071e 76 {
danidanko 0:a1f6917f071e 77
danidanko 0:a1f6917f071e 78
danidanko 0:a1f6917f071e 79 //set up servo for turning on
danidanko 0:a1f6917f071e 80
danidanko 0:a1f6917f071e 81 Az_servo.pulsewidth_us(Az_uS);
danidanko 0:a1f6917f071e 82 Ev_servo.pulsewidth_us(Ev_uS);
danidanko 0:a1f6917f071e 83
danidanko 0:a1f6917f071e 84
danidanko 0:a1f6917f071e 85 wait(0.05);
danidanko 0:a1f6917f071e 86 compas = (compass.sample() / 10.0);
danidanko 0:a1f6917f071e 87 usb_232.printf("[%f] Heading \n", compas);
danidanko 0:a1f6917f071e 88 led2 = !led2; //blink untill heading is set
danidanko 0:a1f6917f071e 89
danidanko 0:a1f6917f071e 90 getlla(p_lat, p_lon, p_alt, g_lat, g_lon, g_alt);
danidanko 0:a1f6917f071e 91
danidanko 0:a1f6917f071e 92 if (g_lon < -100){sanDiego = 1; led3 = ON;}//-100 long is about center of US
danidanko 0:a1f6917f071e 93 else{sanDiego = 0; led4 = ON;}
danidanko 0:a1f6917f071e 94
danidanko 0:a1f6917f071e 95 if (sanDiego == 1 && compas > 346.88 && compas < 348.88)
danidanko 0:a1f6917f071e 96 {usb_232.printf("[%f] TRUE NORTH = 347.8 Magnetic North -- SD, CA\n", compas);
danidanko 0:a1f6917f071e 97 led2 = OFF; //reference found enter main loop
danidanko 0:a1f6917f071e 98 Ev = torads(1);
danidanko 0:a1f6917f071e 99 Ev_uS = (int)(-1.223*Ev*Ev - 589.16*Ev + 2376.78);
danidanko 0:a1f6917f071e 100 Ev_servo.pulsewidth_us(Ev_uS);
danidanko 0:a1f6917f071e 101 break;
danidanko 0:a1f6917f071e 102 }
danidanko 0:a1f6917f071e 103 if (sanDiego == 0 && compas < 11.98 && compas > 9.98)
danidanko 0:a1f6917f071e 104 {usb_232.printf("[%f] TRUE NORTH = 10.98 Magnetic North -- Lexington Park, MD\n", compas);
danidanko 0:a1f6917f071e 105 led2 = OFF; //reference found enter main loop
danidanko 0:a1f6917f071e 106 Ev = torads(1);
danidanko 0:a1f6917f071e 107 Ev_uS = (int)(-1.223*Ev*Ev - 589.16*Ev + 2376.78);
danidanko 0:a1f6917f071e 108 Ev_servo.pulsewidth_us(Ev_uS);
danidanko 0:a1f6917f071e 109 break;
danidanko 0:a1f6917f071e 110 }
danidanko 0:a1f6917f071e 111 }//end of set up while loop you have now pointed the antenna tracker at TRUE NORTH
danidanko 0:a1f6917f071e 112
danidanko 0:a1f6917f071e 113
danidanko 0:a1f6917f071e 114 //Start Main loop
danidanko 0:a1f6917f071e 115 while(1){
danidanko 0:a1f6917f071e 116
danidanko 0:a1f6917f071e 117 /* This main loop gets plane coordinates from the serial port
danidanko 0:a1f6917f071e 118 * converts it to a NED vector pointing from the ground statio
danidanko 0:a1f6917f071e 119 * to the airplane and then sends servos to point at it
danidanko 0:a1f6917f071e 120 */
danidanko 0:a1f6917f071e 121
danidanko 0:a1f6917f071e 122 // get ground and plane coordinates
danidanko 0:a1f6917f071e 123 getlla(p_lat, p_lon, p_alt, g_lat, g_lon, g_alt);
danidanko 0:a1f6917f071e 124
danidanko 0:a1f6917f071e 125 //convert airplane to ecef
danidanko 0:a1f6917f071e 126 lla2ecef(p_lat, p_lon, p_alt, p_x, p_y, p_z);
danidanko 0:a1f6917f071e 127
danidanko 0:a1f6917f071e 128 //convert ground to ecef
danidanko 0:a1f6917f071e 129 lla2ecef(g_lat, g_lon, g_alt, g_x, g_y, g_z);
danidanko 0:a1f6917f071e 130
danidanko 0:a1f6917f071e 131 //subtract plane from ground to get the difference vector
danidanko 0:a1f6917f071e 132 ecef_x = p_x - g_x;
danidanko 0:a1f6917f071e 133 ecef_y = p_y - g_y;
danidanko 0:a1f6917f071e 134 ecef_z = p_z - g_z;
danidanko 0:a1f6917f071e 135
danidanko 0:a1f6917f071e 136 g_latR= torads(g_lat);
danidanko 0:a1f6917f071e 137 g_lonR= torads(g_lon);
danidanko 0:a1f6917f071e 138
danidanko 0:a1f6917f071e 139 /* translation ecef xyz to north east down
danidanko 0:a1f6917f071e 140 *
danidanko 0:a1f6917f071e 141 * TE2L = [ -sin(glat)*cos(glon) -sin(glat)*sin(glon) cos(glat)
danidanko 0:a1f6917f071e 142 * -sin(glon) cos(glon) 0
danidanko 0:a1f6917f071e 143 * -cos(glat)*cos(glon) -cos(glat)*sin(glon) -sin(glat) ]
danidanko 0:a1f6917f071e 144 * NED = TE2L*ecef
danidanko 0:a1f6917f071e 145 */
danidanko 0:a1f6917f071e 146
danidanko 0:a1f6917f071e 147 N = -sin(g_latR)*cos(g_lonR)*ecef_x + -sin(g_latR)*sin(g_lonR)*ecef_y + cos(g_latR)*ecef_z;
danidanko 0:a1f6917f071e 148 E = -sin(g_lonR)*ecef_x + cos(g_lonR)*ecef_y;
danidanko 0:a1f6917f071e 149 D = -cos(g_latR)*cos(g_lonR)*ecef_x -cos(g_latR)*sin(g_lonR)*ecef_y + -sin(g_latR)*ecef_z;
danidanko 0:a1f6917f071e 150 D = (-1)*D; //we are on the ground looking up! :)
danidanko 0:a1f6917f071e 151
danidanko 0:a1f6917f071e 152 Az = atan2(N,E);
danidanko 0:a1f6917f071e 153 Ev = abs(atan2(D, sqrt(N*N + E*E))); // this will always be positive
danidanko 0:a1f6917f071e 154
danidanko 0:a1f6917f071e 155 // if plane is on the ground next to ground station point north
danidanko 0:a1f6917f071e 156 if(g_lat == p_lat && g_lon == p_lon)
danidanko 0:a1f6917f071e 157 {Ev = 0; Az = PI/2;}
danidanko 0:a1f6917f071e 158
danidanko 0:a1f6917f071e 159 //Aling Az is from 0 to 2*pi - no negative numbers
danidanko 0:a1f6917f071e 160 if (Az < 0)
danidanko 0:a1f6917f071e 161 Az = Az + 2*PI;
danidanko 0:a1f6917f071e 162
danidanko 0:a1f6917f071e 163 //from here on out it depends on the how the servo pulse width relates to
danidanko 0:a1f6917f071e 164 //angle her my azimuth servo is defined from 270 -> -90 deg 1900 - 1120 uS
danidanko 0:a1f6917f071e 165 //and the elevation Servo is defines from 0 - 180 deg 0500 - 2400 uS
danidanko 0:a1f6917f071e 166
danidanko 0:a1f6917f071e 167
danidanko 0:a1f6917f071e 168 //blackout region where the azimuth will swing back to Az-PI
danidanko 0:a1f6917f071e 169 //And Ev = pi - Ev
danidanko 0:a1f6917f071e 170 if (Az > 17*PI/12 && Az < 19*PI/12)
danidanko 0:a1f6917f071e 171 {Az = Az - PI;
danidanko 0:a1f6917f071e 172 Ev = PI - Ev;}
danidanko 0:a1f6917f071e 173
danidanko 0:a1f6917f071e 174 // 270 -> -90
danidanko 0:a1f6917f071e 175 if (Az > 3*PI/2 && Az < 2*PI)
danidanko 0:a1f6917f071e 176 Az = Az - 2*PI;
danidanko 0:a1f6917f071e 177
danidanko 0:a1f6917f071e 178 //now get the uS .. the values below are from polyfit matlab
danidanko 0:a1f6917f071e 179 //from your servo calibrations to the second degree
danidanko 0:a1f6917f071e 180
danidanko 0:a1f6917f071e 181 Az_uS = (int)(0.9195*Az*Az + 121.18*Az + 1308.43);
danidanko 0:a1f6917f071e 182 Ev_uS = (int)(-1.223*Ev*Ev - 589.16*Ev + 2376.78);
danidanko 0:a1f6917f071e 183
danidanko 0:a1f6917f071e 184 // Send Servo Commands
danidanko 0:a1f6917f071e 185 Az_servo.pulsewidth_us(Az_uS);
danidanko 0:a1f6917f071e 186 Ev_servo.pulsewidth_us(Ev_uS);
danidanko 0:a1f6917f071e 187
danidanko 0:a1f6917f071e 188
danidanko 0:a1f6917f071e 189 // usb_232.printf("[%2.2f Az] [%i Az uS] [%2.2f Ev] [%i Az uS] \n", todegs(Az), Az_uS, todegs(Ev), Ev_uS );
danidanko 0:a1f6917f071e 190
danidanko 0:a1f6917f071e 191 //usb_232.printf("%2.2f Azimuth %2.2f North %2.2f East\n", todegs(Az), N, E );
danidanko 0:a1f6917f071e 192 // usb_232.printf("[%f %f %f ] Plane \n", p_lat, p_lon, p_alt );
danidanko 0:a1f6917f071e 193 // usb_232.printf("[%f %f %f ] Plane ecef \n ", p_x, p_y, p_z );
danidanko 0:a1f6917f071e 194 // usb_232.printf("[%f %f %f ] Ground \n", g_lat, g_lon, g_alt);
danidanko 0:a1f6917f071e 195 // usb_232.printf("[%f %f %f ] Ground ecef \n ", g_x, g_y, g_z );
danidanko 0:a1f6917f071e 196 }
danidanko 0:a1f6917f071e 197
danidanko 0:a1f6917f071e 198
danidanko 0:a1f6917f071e 199 } // end main
danidanko 0:a1f6917f071e 200 void lla2ecef(float lat, float lon, float alt, float& x, float& y, float& z)
danidanko 0:a1f6917f071e 201 {
danidanko 0:a1f6917f071e 202 // WGS84 ellipsoid constants:
danidanko 0:a1f6917f071e 203 float a = 6378137; //earth semi-major axis
danidanko 0:a1f6917f071e 204 float esqrd = 0.006694379990141; //Eccentricity
danidanko 0:a1f6917f071e 205 float N;
danidanko 0:a1f6917f071e 206 //calculation
danidanko 0:a1f6917f071e 207 lat = torads(lat);
danidanko 0:a1f6917f071e 208 lon = torads(lon);
danidanko 0:a1f6917f071e 209 //The length of normal (N) to the ellipsoid
danidanko 0:a1f6917f071e 210 N = a / sqrt(1 - esqrd * sin( lat ) * sin( lat ) ) ;
danidanko 0:a1f6917f071e 211 x = (N+alt) * cos(lat) * cos(lon);
danidanko 0:a1f6917f071e 212 y = (N+alt) * cos(lat) * sin(lon);
danidanko 0:a1f6917f071e 213 z = ((1-esqrd) * N + alt) * sin(lat);
danidanko 0:a1f6917f071e 214
danidanko 0:a1f6917f071e 215 }
danidanko 0:a1f6917f071e 216 float torads(float deg)
danidanko 0:a1f6917f071e 217 {
danidanko 0:a1f6917f071e 218 return (deg*PI) / 180 ; //in rads
danidanko 0:a1f6917f071e 219 }
danidanko 0:a1f6917f071e 220 float todegs(float rads)
danidanko 0:a1f6917f071e 221 {
danidanko 0:a1f6917f071e 222 return (rads*180) / PI ; //in degs
danidanko 0:a1f6917f071e 223 }
danidanko 0:a1f6917f071e 224 void servo_calib(void)
danidanko 0:a1f6917f071e 225 {
danidanko 0:a1f6917f071e 226 PwmOut servo(p22);
danidanko 0:a1f6917f071e 227 servo.period(0.020);
danidanko 0:a1f6917f071e 228 char pwm_uS[5];
danidanko 0:a1f6917f071e 229 int pwm_uS_i;
danidanko 0:a1f6917f071e 230 while(1)
danidanko 0:a1f6917f071e 231 {
danidanko 0:a1f6917f071e 232 usb_232.printf(" \n Enter PWM in uS [0600-2400] : ");
danidanko 0:a1f6917f071e 233 usb_232.scanf("%s", pwm_uS);
danidanko 0:a1f6917f071e 234 pwm_uS_i = atof ( pwm_uS );
danidanko 0:a1f6917f071e 235 servo.pulsewidth_us(pwm_uS_i);
danidanko 0:a1f6917f071e 236 usb_232.printf("\n You Entered: %i ", pwm_uS_i);
danidanko 0:a1f6917f071e 237 }
danidanko 0:a1f6917f071e 238 }
danidanko 0:a1f6917f071e 239 void getlla(float& p_lat, float& p_lon, float& p_alt, float& g_lat, float& g_lon, float& g_alt)
danidanko 0:a1f6917f071e 240 {
danidanko 0:a1f6917f071e 241 uchar msg[26];
danidanko 0:a1f6917f071e 242 do{
danidanko 0:a1f6917f071e 243 while( (msg[0] = groundStation.getc()) != 0xff); // wait for the start of communication string
danidanko 0:a1f6917f071e 244 for(int i=1; i<26; i++) {
danidanko 0:a1f6917f071e 245 msg[i] = groundStation.getc(); //fill message string
danidanko 0:a1f6917f071e 246 }
danidanko 0:a1f6917f071e 247 }while(msg[25] != 0xfe); // make sure the last character is 0xFE otherwise re read
danidanko 0:a1f6917f071e 248 led1 = !led1; //blink :)
danidanko 0:a1f6917f071e 249 //airplane
danidanko 0:a1f6917f071e 250 p_lat = tofloat(msg[4], msg[3], msg[2], msg[1]);
danidanko 0:a1f6917f071e 251 p_lon = tofloat(msg[8], msg[7], msg[6], msg[5]);
danidanko 0:a1f6917f071e 252 p_alt = tofloat(msg[12], msg[11], msg[10], msg[9]);
danidanko 0:a1f6917f071e 253 //ground
danidanko 0:a1f6917f071e 254 g_lat = tofloat(msg[16], msg[15], msg[14], msg[13]);
danidanko 0:a1f6917f071e 255 g_lon = tofloat(msg[20], msg[19], msg[18], msg[17]);
danidanko 0:a1f6917f071e 256 g_alt = tofloat(msg[24], msg[23], msg[22], msg[21]);
danidanko 0:a1f6917f071e 257
danidanko 0:a1f6917f071e 258 }
danidanko 0:a1f6917f071e 259 float tofloat(uchar b0, uchar b1, uchar b2, uchar b3)
danidanko 0:a1f6917f071e 260 {
danidanko 0:a1f6917f071e 261 uchar b[] = {b3, b2, b1, b0};
danidanko 0:a1f6917f071e 262 float *fp = (float *)b;
danidanko 0:a1f6917f071e 263 return *fp;
danidanko 0:a1f6917f071e 264 }