projects:parafoil:liftingbody:code:navsoft.c
/* navsoft.c * James Coxon - jacoxon@googlemail.com * reviewed by Steve Randall 11-Aug-07 * Serial code adapted from: * Copyleft (c) 2006, Tod E. Kurt, tod@todbot.com * http://todbot.com/blog/ * http://todbot.com/arduino/host/arduino-serial/arduino-serial.c * * Compile on the gumstix: * gcc -o navsoft navsoft.c -static * * Probably doesn' work :-D For functions coords_to_distance and coords_to_bearing: /*************************************************************************** * Copyright (C) 2006 the `High Altitude Slug Project' * * * * Permission is hereby granted, free of charge, to any person obtaining * * a copy of this software and associated documentation files (the * * "Software"), to deal in the Software without restriction, including * * without limitation the rights to use, copy, modify, merge, publish, * * distribute, sublicense, and/or sell copies of the Software, and to * * permit persons to whom the Software is furnished to do so, subject to * * the following conditions: * * * * The above copyright notice and this permission notice shall be * * included in all copies or substantial portions of the Software. * * * * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, * * EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF * * MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.* * IN NO EVENT SHALL THE AUTHORS BE LIABLE FOR ANY CLAIM, DAMAGES OR * * OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, * * ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR * * OTHER DEALINGS IN THE SOFTWARE. * *************************************************************************** */ #include <stdio.h> /* Standard input/output definitions */ #include <string.h> /* String function definitions */ #include <unistd.h> /* UNIX standard function definitions */ #include <fcntl.h> /* File control definitions */ #include <errno.h> /* Error number definitions */ #include <termios.h> /* POSIX terminal control definitions */ #include <stdlib.h> #include <math.h> #include "coordinates.h" /* * Constants */ /* a, b = major & minor semiaxes of the ellipsoid */ #define VIN_A (long long) 6378137 #define VIN_B (long long) 6356752.3142 // SR: humm long with decimal pint - long long? /* f = flattening (a - b) / a */ #define VIN_F (1 / 298.257223563) /* earth's mean radius in km */ #define EARTH_RADIUS 6371 /* * Function-like macros */ #define deg_to_rad(a) (a * M_PI / 180) #define rad_to_deg(a) (a * 180 / M_PI) int fdgps, fdservo; // global file descriptor of the gps and servo serial ports int servo_pos; char buf[255]; int gpsTime; // SR: not used anywhere??? float gpsLong, gpsLat, RMCgroundSpeed, gpsTangle, magVariation; // SR: Not used anywhere float trueTrack, magTrack, VTGgroundSpeedKn, VTGgroundSpeedKm; int GGAsats, GGAtime, GGAfix; float GGAlat, GGAlong, GGAaltM, GGAhori, GGAgeoid; static unsigned char servo_buf[8]; // holds the current RC servo data block - 8 channels worth of values float targetLat = 52.213583; // default target location is chirchill float targetLon = 0.096477; double bearing; /* variables for check_against_parameters() */ int apogee; // SR: not initiallized float prevAlt, altDifference; //Set parameters float parachuteReleaseheight = 1000.0; FILE * logFile; // Log file pointer // ******************************* Serial Port functions ********************************* int open_gps_serial_port(const char* ttySx) { // Open port for both reading ands writing (writes will not block) fdgps = open(ttySx, O_RDWR | O_NOCTTY | O_NDELAY); if (fdgps == -1) { printf("open_port: Unable to open %s - \n", ttySx); } else fcntl(fdgps, F_SETFL, 0); return (fdgps); } int close_gps_serial_port(void) { close (fdgps); } int open_servo_serial_port(const char* ttySx) { // Open port for both reading ands writing (writes will not block) fdservo = open(ttySx, O_RDWR | O_NOCTTY | O_NDELAY); if (fdservo == -1) { printf("open_port: Unable to open %s - \n", ttySx); } else fcntl(fdservo, F_SETFL, 0); return (fdservo); } int close_servo_serial_port(void) { close (fdservo); } // ********************************* GPS functions ************************************** // read and parse NMEA sentances from GPS int read_gps_serial_port(void) { int res; // set to length of line read char *token, *str1; res = read(fdgps,buf,255); // try to read a line of input from the gps, buf[res] = 0; // termiante string, so we can printf etc. if (strncmp(buf, "$GPVTG", 6) == 0) { // NMEA GPVTG sentance read from GPS (contains ground course and speed) printf("GPVTG found\n"); str1 = strdup(buf); // duplicate buffer so strtok can carve it up token = strtok(str1, ","); token = strtok(NULL, ","); trueTrack = atof(token); token = strtok(NULL, ","); token = strtok(NULL, ","); magTrack = atof(token); token = strtok(NULL, ","); token = strtok(NULL, ","); VTGgroundSpeedKn = atof(token); token = strtok(NULL, ","); token = strtok(NULL, ","); VTGgroundSpeedKm = atof(token); printf("%f,%f,%f,%f\n", trueTrack, magTrack, VTGgroundSpeedKn, VTGgroundSpeedKm); } else if (strncmp(buf, "$GPGGA", 6) ==0) { // NMEA GPGGA sentance read from GPS (contains position and height information) // printf ("GPGGA found\n"); /* Splits the GPGGA sequence into tokens and then converts them into respective ints or floats */ str1 = strdup(buf); // duplicate buffer so strtok can carve it up token = strtok(str1, ","); token = strtok(NULL, ","); GGAtime = atoi(token); token = strtok(NULL, ","); GGAlat = atof(token); /* Convert into decimal */ GGAlat = GGAlat / 100.0; int deglat = (int)GGAlat; float minlat = GGAlat - (float)deglat; minlat = minlat / 0.6; GGAlat = (float)deglat + minlat; token = strtok(NULL, ","); token = strtok(NULL, ","); GGAlong = atof(token); /* Convert into decimal */ GGAlong = GGAlong / 100.0; int deglong = (int)GGAlong; float minlong = GGAlong - (float)deglong; minlong = minlong / 0.6; GGAlong = (float)deglong + minlong; token = strtok(NULL, ","); token = strtok(NULL, ","); GGAfix = atoi(token); token = strtok(NULL, ","); GGAsats = atoi(token); token = strtok(NULL, ","); GGAhori = atof(token); token = strtok(NULL, ","); GGAaltM = atof(token); token = strtok(NULL, ","); GGAgeoid = atof(token); printf("%d,lat:%f,long:%f,fix:%d,sats:%d,hori:%f,alt:%f,geo:%f\n", GGAtime, GGAlat, GGAlong, GGAfix, GGAsats, GGAhori, GGAaltM, GGAgeoid); } else { // neither a GPVTG or GPGGA sentance read // printf("No Lock\n"); } } // ********************************* Logging functions ************************************** int open_log_file() { logFile = fopen("gps.log", "a+"); } int write_log() { fprintf( logFile, "%d,%f,%f,%d,%d,%f,%f,%f\n", GGAtime, GGAlat, GGAlong, GGAfix, GGAsats, GGAhori, GGAaltM, GGAgeoid); } int close_log_file() { fclose(logFile); } // ********************************* Servo functions ************************************** void WriteServo(unsigned char uc) { // WriteUART2(uc); // writes to the USART serving the servo controller int n = write(fdservo, (void *)&uc, 1); // Write one raw byte to USART printf("%d\n", n); // should be 1 } // ouput the servo data block to the servo controller void SendServo(void) { int i; unsigned char Servo_CRC; WriteServo(0x5A); // Write Header Servo_CRC = 0x5A; for(i = 0;i < sizeof(servo_buf); i++) { WriteServo(servo_buf[i]); Servo_CRC ^= servo_buf[i]; } WriteServo(Servo_CRC); // Write checksum printf("Written\n"); } // servo_pos is the current absolute servo position // change is the required difference from that value // SR: probably need to add limits on servo positions. int adjust_servo_position(int change) { int i, pos; /* Take data from calculate_differences and adjust servo position accordingly */ pos = servo_pos + change; open_servo_serial_port("/dev/ttyS1"); printf("Serial port open\n"); for (i = 0; i < sizeof(servo_buf); i++) { servo_buf[i] = pos; // servo_buf[i] = 0x5A; // set all servos to 90 (results in a string of 10 Zs) } SendServo(); // writes servo block to servo driver close_servo_serial_port(); servo_pos = pos; } void centre_servo(void) { servo_pos = 128; adjust_servo_position(0); } // ************************ Position and Direction Calculation functions ************************** // Distance and Bearings Calculations double coords_to_distance(struct coordinates coords1, struct coordinates coords2) { /*Calculate the distance between present position and landing site (on the ground) using high altitude slug */ /* code (on wiki) */ /** * Finds the distance between two sets of coordinates * * @param coords1 first coordinates * @param coords2 second coordinates * @return distance in metres */ /* *Vincenty Formula * http://www.movable-type.co.uk/scripts/LatLongVincenty.html * far too complicated, I love it ;) * seriously though - maybe we should use the simpler haversine formula * for speed */ if (coords1.lat == coords2.lat && coords1.lon == coords2.lon) return 0; coords1.lat = deg_to_rad(coords1.lat); coords1.lon = deg_to_rad(coords1.lon); coords2.lat = deg_to_rad(coords2.lat); coords2.lon = deg_to_rad(coords2.lon); double L = coords2.lon - coords1.lon; double U1 = atan((1 - VIN_F) * tan(coords1.lat)); double U2 = atan((1 - VIN_F) * tan(coords2.lat)); double sinU1 = sin(U1); double cosU1 = cos(U1); double sinU2 = sin(U2); double cosU2 = cos(U2); double lambda = L; double lambdaP = 2 * M_PI; double iterLimit = 20; double sinLambda, cosLambda; double sinSigma, cosSigma; double sigma, alpha; double cosSqAlpha, cos2SigmaM, C; while (abs(lambda - lambdaP) > 1e-12 && --iterLimit > 0) { sinLambda = sin(lambda); cosLambda = cos(lambda); sinSigma = sqrt((cosU2 * sinLambda) * (cosU2 * sinLambda) + (cosU1 * sinU2 - sinU1 * cosU2 * cosLambda) * (cosU1 * sinU2-sinU1 * cosU2 * cosLambda)); cosSigma = sinU1 * sinU2 + cosU1 * cosU2 * cosLambda; sigma = atan2(sinSigma, cosSigma); alpha = asin(cosU1 * cosU2 * sinLambda / sinSigma); cosSqAlpha = cos(alpha) * cos(alpha); cos2SigmaM = cosSigma - 2 * sinU1 * sinU2 / cosSqAlpha; C = VIN_F / 16 * cosSqAlpha * (4 + VIN_F * (4 - 3 * cosSqAlpha)); lambdaP = lambda; lambda = L + (1 - C) * VIN_F * sin(alpha) * (sigma + C * sinSigma * (cos2SigmaM + C * cosSigma * (-1 + 2 * cos2SigmaM * cos2SigmaM))); } if (iterLimit == 0) return 0; /* formula failed to converge */ double uSq = cosSqAlpha * (VIN_A * VIN_A - VIN_B * VIN_B) / (VIN_B * VIN_B); double A = 1 + uSq / 16384 * (4096 + uSq * (-768 + uSq * (320 - 175 *uSq))); double B = uSq / 1024 * (256 + uSq * (-128 + uSq * (74 - 47 * uSq))); double deltaSigma = B * sinSigma * (cos2SigmaM + B / 4 * (cosSigma * (-1 + 2 * cos2SigmaM * cos2SigmaM) - B / 6 * cos2SigmaM*(- 3 + 4 * sinSigma * sinSigma) * (-3 + 4 * cos2SigmaM * cos2SigmaM))); double s = VIN_B * A * (sigma - deltaSigma); printf("s = %fm\n", s); double skm = s / 1000.0; printf("s = %fkm\n", skm); return s; } /* Finds the bearing (in degrees) between two sets of coordinates */ double coords_to_bearing(struct coordinates coords1, struct coordinates coords2) { // Convert degree co-ordinates to Radians coords1.lat = deg_to_rad(coords1.lat); coords1.lon = deg_to_rad(coords1.lon); coords2.lat = deg_to_rad(coords2.lat); coords2.lon = deg_to_rad(coords2.lon); double dlat = coords2.lat - coords1.lat; /* delta */ double dlon = coords2.lon - coords1.lon; /* delta */ bearing = atan2((sin(dlon) * cos(coords2.lat)) , (cos(coords1.lat) * sin(coords2.lat) - sin(coords1.lat) * cos(coords2.lat) * cos(dlon))); bearing = rad_to_deg(bearing); // set global bearing if (bearing < 0) bearing += 360; return bearing; } /* Compare required bearing to present bearing and calculate difference, also trigger alarms if getting near */ /* to the landing site and finally look at altitude and how much height we have left before landing */ /* Rough example - will update with edwardmoores' version */ int calculate_differences() { printf("Target Bearing = %f\n", bearing); printf("True Track = %f\n", trueTrack); // Calculate the difference between the baering to target and the current ground track double bearingDifference = bearing - trueTrack; // since bearing and trueTrack can take any value from 0 - 360 then // bearingDifference can vary from +360 to -360 // values -360 to -180 mapped to 0 to +180 // values +360 to +180 mapped to 0 to -180 if (bearingDifference <= -180.0) bearingDifference += 360.0; if(bearingDifference >= 180.0) bearingDifference -= 360.0; // Bearing difference now adjusted to range -180 to +180 printf("Bearing Difference = %f\n", bearingDifference); if (bearingDifference < 0.0) { /*Turn Left*/ adjust_servo_position(-5); // nudge servo ccw } if (bearingDifference > 0.0) { /* Turn Right */ adjust_servo_position(5); // nudge servo cw } } int check_against_paramters() { /* This compares data against various set paramaters eg. altitude and decides whether to cut down secondary parachute */ // Calulates if the payload is descending altDifference = GGAaltM - prevAlt; prevAlt = GGAaltM; if ( altDifference < 0) { apogee++; } // Decide whether to release parachute. if (GGAaltM < parachuteReleaseheight && apogee > 2) { system("/root/scripts/Parachutecutdown"); } } // ********************************* The Main loop ************************************** // main expects two commend line parameters: Latitude and Longitude of the target loaction // the code echos the target loaction, opens the gps serial port and logging files and then // enters the main loop. The main loop then // reads the position and direction from the GPS - read_gps_serial_port() // calculates bearing and distance to the target loaction // adjusts the servo int main (int argc, char **argv) { targetLat = atof(argv[1]); targetLon = atof(argv[2]); printf("HARVe Lifting Body Navigation Software\n"); printf("By James Coxon, Ed Moore and Steve Randall"); printf("Target Coords = %f, %f\n", targetLat, targetLon); open_gps_serial_port("/dev/ttyS2"); open_log_file(); centre_servo(); // Start Servo at midway position while (1) // Loop forever { read_gps_serial_port(); // Do we have a GPS lock? if (GGAfix == 1) { // Valid GPS fix write_log(); struct coordinates presentCoords; presentCoords.lat = GGAlat; presentCoords.lon = GGAlong; struct coordinates targetCoords; targetCoords.lat = targetLat; targetCoords.lon = targetLon; coords_to_distance(presentCoords, targetCoords); coords_to_bearing(presentCoords, targetCoords); check_against_paramters(); calculate_differences(); // adjust_servo_position(); GGAfix = 0; // SR: Added this - otherwise will be perminatly set following single vaild GGA } else { // No GPS lock. centre_servo(); // Centre servo if GPS lock lost usleep(500); // SR: 500 micro seconds - too short?? } usleep(500); // SR: 2 lots of usleep(500) if no lock ?? } close_log_file(); close_serial_port(); }
projects/parafoil/liftingbody/code/navsoft.c.txt · Last modified: 2008/07/19 23:33 by 127.0.0.1