UKHAS Wiki

UK High Altitude Society

User Tools

Site Tools


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 (external edit)