[Flightcomputer] HoHoHo 3

// Project Hollands Hoogte Dec 2010
// By Tim Zaman 
// Help from Terry Baume (& ntx2 by jharrison)
 
// Digital Pins
#define GPS_PIN_RX              9       // GPS serial pin RX
#define GPS_PIN_TX              8       // GPS serial pin TX
#define RTTY_PIN_1 		2	// RTTY space pin
#define RTTY_PIN_2 		3	// RTTY mark pin
#define GSM_PIN_RX              5       // GPS serial pin RX
#define GSM_PIN_TX              4       // GPS serial pin TX
#define PHOTOPIN                7       // Photopin to EOS350D
#define RELEASEPIN              6       // Releasepin to Arduino Duemilenova
 
// Baudot
#define ARRAY_LEN 32
#define LETTERS_SHIFT 31
#define FIGURES_SHIFT 27
#define LINEFEED 2
#define CARRRTN  8
 
 
#define is_lowercase(ch)    ((ch) >= 'a' && (ch) <= 'z')
#define is_uppercase(ch)    ((ch) >= 'A' && (ch) <= 'Z')
 
 
// T68i
#define num_to_char(number)   ((number) < 10 ?                           \
                                               ('0' + (number)) :        \
                                               (('A' - 10) + (number)) )
#define first_four(byte)       (0x0F & (byte))
#define last_four(byte)      ((0xF0 & (byte)) >> 4)
#define hexdump_a(byte)  num_to_char( last_four(byte))
#define hexdump_b(byte)  num_to_char(first_four(byte))
 
 
// Misc
#define RTTY_ASCII      7       // ASCII set for RTTY (7/8bit)
#define GPSRATE         9600    // GPS baud rate
#define BUFFERSIZE      90      // How many bytes of input to buffer from the GPS?
#define ENDLN           "\r\n"  // SD
 
#include <stdint.h>
#include <util/crc16.h>
#include <Flash.h>
#include <Streaming.h>
//#include <NewSoftSerial.h>
 
#include <SoftwareSerial.h>
 
//baudot
char letters_arr[33] = "\000E\nA SIU\rDRJNFCKTZLWHYPQOBG\000MXV\000";
char figures_arr[33] = "\0003\n- \a87\r$4',!:(5\")2#6019?&\000./;\000";
 
 
// Initialize flight variables
int numSats = 0;
int fixType = 0;
int time[] = {
  0, 0, 0};
double latitude = 0.0;
double longitude = 0.0;
long altitude = 0;
long maxAlt = 0;
int speed = 0;
int txCount = 0;
int intTemp = 0;
int extTemp = 0;
int intHum = 0;
int bitRate = 50;
int descent = 0;
double lon_release = 5.59;
double lon_test = 4.6996;
 
unsigned long SmsStart = 0;     // SMS-time
unsigned long GpsAltTime = 1800000;    // 1.800.000ms=1.800s=30min
int ExecOnce=0;             // execut0r
int ExecOneRelease=0;          // voor 24km limit ding
char buffer[BUFFERSIZE];
byte nxtln[] = ENDLN;                       //SD ENDLNn
unsigned long NXTlength = sizeof(ENDLN)-1;  //SD ENDLNn length
 
SoftwareSerial GPS = SoftwareSerial(GPS_PIN_RX, GPS_PIN_TX);  //(rx,tx)
SoftwareSerial GSM = SoftwareSerial(GSM_PIN_RX, GSM_PIN_TX);  //(rx,tx)
 
void setup() {
 
  // Start up serial ports
 
  Serial.begin(9600);
  Serial.println("Serial started");
  GPS.begin(4800);
  GSM.begin(9600);
 
 
 
  // LED's
  delay(1000);
  GPS.println("$PSTMNMEACONFIG,0,4800,1,1");
  GPS.println("$PSTMINITGPS,5200.000,N,00421.000,E,0197,22,10,2007,11,40,00");  
  GPS.println("$PSRF103,02,00,00,01*26");
 
  // Canon EOS 350D
  pinMode(PHOTOPIN,OUTPUT);
  pinMode(RELEASEPIN,OUTPUT);
  // Setup for RTTY
  pinMode(2, OUTPUT);
  pinMode(3, OUTPUT);
 
  //GPS
  pinMode(GPS_PIN_RX, INPUT);
  pinMode(GPS_PIN_TX, OUTPUT);
 
  //GSM
  pinMode(GSM_PIN_RX, INPUT);
  pinMode(GSM_PIN_TX, OUTPUT);
 
}
 
 
void loop() {
 
  // Get a GGA string from the GPS,
  // check if it's a valid fix, and extract the data
  getNMEA("$GPGGA");         
  numSats = getSats();
  fixType = getFixType();
 
  digitalWrite(RELEASEPIN, LOW); 
 
  // Make sure we have a valid fix
  if (fixType != 0) {
    getTime(time);
    latitude = getLat();
    longitude = getLong();
    altitude = getAlt();
 
    // Keep track of the maximum altitude
    if (altitude > maxAlt) { 
      maxAlt = altitude; 
    }
 
    // Check to see if we've fallen 600m, if so switch to descent mode
    if (altitude < (maxAlt - 600)) { 
      descent = 1; 
      if (ExecOneRelease == 0){
        ExecOneRelease = 1;
        digitalWrite(RELEASEPIN, HIGH);  //Release
      }
    } 
   }     
 
  // Convert lat & long into strings
  char latString[12]; 
  char longString[12];
  doubleToString(latitude, 4, latString);
  doubleToString(longitude, 4, longString);
 
 
  sprintf(buffer, "$$HHH,%d,%02d:%02d:%02d,%s,%s,%ld,%d", txCount, time[0], time[1], time[2], latString, longString, altitude, numSats);
  txCount++;
 
  if (fixType != 0) { //Als we een fix hebben
    if(txCount >100){ //En we zeker 100 strings hebben gehad
      if (longitude > lon_release){ //En hij is rond flevoland
        ExecOneRelease = 1;
        digitalWrite(RELEASEPIN, HIGH);
        TxString("RELEASE MECHANISM IN OPERATION");
        delay(100);
      }
    }
  }
 
 
  if(txCount == 10){
   send_sms(buffer);
   delay(50);
  }
 
   if (txCount >12){ //Na 12 strings
      if ( (txCount & 0x01) == 0) { //Elke oneven
        digitalWrite(PHOTOPIN, HIGH);  //Maak foto
        Serial.println("Foto");
    }
  }
 
 
  if (altitude > 23000){
    if (ExecOneRelease == 0){
      ExecOneRelease = 1;
      digitalWrite(RELEASEPIN, HIGH);
    }
  }
 
  // Transmit and log to SD card
  TxString(buffer);
 
  digitalWrite(PHOTOPIN, LOW); //Put low anyway
 
 
  // SMS <1000m mode
  if (descent == 1){
    if (altitude < 900){
 
      if (ExecOnce == 0){
        SmsStart = millis();
        ExecOnce = 1;
        send_sms(buffer);
        delay(50);
      }
      if (millis() - SmsStart > 25000){
        ExecOnce =0;
      }
 
    } 
  }
 
  // Delay a moment before restarting loop
  delay(100);
 
} //end
 
// ------- GPS Parsing ----------
 
// Reads a line from the GPS NMEA serial output
// Give up after trying to read 1000 bytes (~2 seconds)
int readLine(void) {
  char c;
  byte bufferIndex = 0;
  boolean startLine = 0;
  byte retries = 0;
  while (retries < 20) {
    //if (GPS.available() > 0) {     
 
      c= GPS.read();
 
      if (c == -1) { 
        delay(2); 
        continue; 
      }
      if (c == '\n') continue;
      if (c == '$') startLine = 1;
      if ((bufferIndex == BUFFERSIZE-1) || (c == '\r')) {
        if (startLine) {
          buffer[bufferIndex] = 0;
          return 1;
        }
      }
      if (startLine) buffer[bufferIndex++] = c;
    //} 
    else {
      retries++;
      delay(50);
    }
  }
  Serial.println(buffer);
  return 0;
}
 
// Returns a specific field from the buffer
void getField(int getId, char *field, int maxLen) {
  byte bufferIndex = 0;
  byte fieldId = 0;
  byte i = 0;
  while (bufferIndex < sizeof(buffer)) {
    if (fieldId == getId) {
      // End of string, or string overflow
      if (buffer[bufferIndex] == ',' || i > (maxLen - 2)) {
        field[i] = 0;	// Null terminate
        return;
      }
      // Buffer chars to field
      field[i++] = buffer[bufferIndex++];
    } 
    else {
      // Advance field on comma
      if (buffer[bufferIndex] == ',') {
        bufferIndex++;						// Advance in buffer
        fieldId++;							// Increase field position counter
      } 
      else {
        bufferIndex++;						// Advance in buffer
      }
    }
  }
  // Null terminate incase we didn't already..
  field[i] = 0;
}
 
// Polls for an NMEA sentence of type requested
// Validates checksum, silently retries on failed checksums
int getNMEA(char *getType) {
  char type[7];
  byte retries = 0;
  while (retries < 2) {
    if (readLine() && validateChecksum()) {
      ;
      getField(0, type, sizeof(type));
      if (strcmp(type, getType) == 0) {
        Serial.println(buffer);
        return 1;
      }
    } 
    else {
      retries++;
    }
  }
  Serial.println("Failed to read GPS");
  return 0;
}
 
// Validates the checksum on an NMEA string
// Returns 1 on valid checksum, 0 otherwise
int validateChecksum(void) {
  char gotSum[2];
  gotSum[0] = buffer[strlen(buffer) - 2];
  gotSum[1] = buffer[strlen(buffer) - 1];
  // Check that the checksums match up
  if ((16 * atoh(gotSum[0])) + atoh(gotSum[1]) == getCheckSum(buffer)) return 1;
  else return 0;
}
 
// Calculates the checksum for a given string
// returns as integer
int getCheckSum(char *string) {
  int i; 
  int XOR;	
  int c;
  // Calculate checksum ignoring any $'s in the string
  for (XOR = 0, i = 0; i < strlen(string); i++) {
    c = (unsigned char)string[i];
    if (c == '*') break;
    if (c != '$') XOR ^= c;
  }
  return XOR;
}
 
// Returns the groundspeed in km/h
int getSpeed(void) {
  char field[10];
  getField(7, field, sizeof(field));
  int speed = atoi(field);
  return speed;
}
 
// Return the fix type from a GGA string
int getFixType(void) {
  char field[5];
  getField(6, field, sizeof(field));
  int fixType = atoi(field);
  return fixType;
}
 
// Return the altitude in meters from a GGA string
long getAlt(void) {
  char field[10];
  getField(9, field, sizeof(field));
  long altitude = atol(field);
  return altitude;
}
 
// Returns the number of satellites being tracked from a GGA string
int getSats(void) {
  char field[3];
  getField(7, field, sizeof(field));
  int numSats = atoi(field);
  return numSats;
}
 
// Read the latitude in decimal format from a GGA string
double getLat(void) {
  char field[12];
  getField(2, field, sizeof(field));			// read the latitude
  double latitude = atof(field);					// convert to a double (precise)
  int deg = (int) latitude / 100;				// extract the number of degrees
  double min = latitude - (100 * deg);			// work out the number of minutes
  latitude = deg + (double) min/60.0;			// convert to decimal format
  getField(3, field, sizeof(field));			// get the hemisphere (N/S)
  if (strcmp(field, "S") == 0) latitude *= -1;	// sign the decimal latitude correctly
  return latitude;
}
 
// Read the longitude in decimal format from a GGA string
double getLong(void) {
  char field[12];
  getField(4, field, sizeof(field));			// read the longitude
  double longitude = atof(field);					// convert to a double
  int deg = (int) longitude / 100;				// extract the number of degrees
  double min = longitude - (100 * deg);			// work out the number of minutes
  longitude = deg + (double) min/60.00;			// convert to decimal format
  getField(5, field, sizeof(field));			// get the E/W status
  if (strcmp(field, "W") == 0) longitude *= -1; // sign decimal latitude correctly
  return longitude;
}
 
// Converts UTC time to the correct timezone
void convertTime(int *time) {
  // How many hours off GMT are we?
  float offset = 1;
  long sectime = ((long)(time[0]) * 3600) + (time[1] * 60) + time[2];
  sectime += (offset * 3600.0);
  // Did we wrap around?
  if (sectime < 0) sectime += 86400;
  if (sectime > 86400) sectime -= 86400;
  // Convert back to time
  time[0] = (int)(sectime / 3600);
  time[1] = (int)((sectime % 3600) / 60);
  time[2] = (int)((sectime % 3600) % 60);
}
 
// Parses a time field from a GGA string
void parseTime(char *field, int *time) {
  char tmp[3]; 
  tmp[2] = 0; // Init tmp and null terminate
  tmp[0] = field[0]; 
  tmp[1] = field[1]; 
  time[0] = atoi(tmp); // Hours
  tmp[0] = field[2]; 
  tmp[1] = field[3]; 
  time[1] = atoi(tmp); // Minutes
  tmp[0] = field[4]; 
  tmp[1] = field[5]; 
  time[2] = atoi(tmp); // Seconds
}
 
// Gets the hours, minutes and seconds from a GGA string
void getTime(int *time) {
  char field[12];
  getField(1, field, sizeof(field));
  parseTime(field, time);
  convertTime(time);
}
 
// ------ RTTY ----------
 
// Transmit a string, log it to SD & produce debug output
void TxString(char *string) {
  // Checksum
  char txSum[4];
  int checkSum = getCheckSum(string);
  sprintf(txSum, "%02X", checkSum);
  Serial << F("RTTY: ") << string << "#" << txSum << ENDLN;
  rtty_txstring(string);
  rtty_txstring("#");
  rtty_txstring(txSum);
  rtty_txstring("\r\n");
}
 
 
uint8_t char_to_baudot(char c, char *array)
{
  int i;
  for (i = 0; i < ARRAY_LEN; i++)
  {
    if (array[i] == c)
      return i;
  }
 
  return 0;
}
 
 
 
void rtty_txbyte(uint8_t b)
{
  int8_t i;
 
  rtty_txbit(0);
 
  /* TODO: I don't know if baudot is MSB first or LSB first */
  /* for (i = 4; i >= 0; i--) */
  for (i = 0; i < 5; i++)
  {
    if (b & (1 << i))
      rtty_txbit(1);
    else
      rtty_txbit(0);
  }
 
  rtty_txbit(1);
}
 
 
 
enum baudot_mode
{
  NONE,
  LETTERS,
  FIGURES
};
 
 
void rtty_txstring(char *str)
{
  enum baudot_mode current_mode = NONE;
  char c;
  uint8_t b;
 
  while (*str != '\0')
  {
    c = *str;
    /* some characters are available in both sets */
    if (c == '\n')
    {
      rtty_txbyte(LINEFEED);
    }
    else if (c == '\r')
    {
      rtty_txbyte(CARRRTN);
    }
    else if (is_lowercase(*str) || is_uppercase(*str))
    {
      if (is_lowercase(*str))
      {
        c -= 32;
      }
 
      if (current_mode != LETTERS)
      {
        rtty_txbyte(LETTERS_SHIFT);
        current_mode = LETTERS;
      }
 
      rtty_txbyte(char_to_baudot(c, letters_arr));
    }
    else
    {
      b = char_to_baudot(c, figures_arr);
 
      if (b != 0 && current_mode != FIGURES)
      {
        rtty_txbyte(FIGURES_SHIFT);
        current_mode = FIGURES;
      }
 
      rtty_txbyte(b);
    }
 
    str++;
  }
}
 
 
// Transmit a bit as a mark or space
void rtty_txbit (int bit) {
  if (bit) {
    // High - mark
    digitalWrite(2, HIGH);
    digitalWrite(3, LOW);
  } 
  else {
    // Low - space
    digitalWrite(3, HIGH);
    digitalWrite(2, LOW);
  }
  // Delay appropriately - tuned to 50 baud.
  delay(20);
  //delayMicroseconds(250);
}
 
 
// ------ T68i GPRS ----------
// © D> Richman
 
void hexdump_byte(unsigned char byte)
{
  GSM.print(hexdump_a(byte), BYTE);
  GSM.print(hexdump_b(byte), BYTE);
}
 
void send_sms(char *data)
{
  size_t data_length, x;
  char c, l;
  long i;
  long n;
 
  data_length = strlen(data);
  i = data_length * 7;
 
  /* Round i up to a multiple of 8 */
  if (i & 0x07) i = (i & ~0x07) + 0x08;
 
  /* Calculate the number of message octets */
  i = i / 8;
 
  GSM.println("AT+CMGF=0");
  delay(1500);
  GSM.print("AT+CMGS=");
  delay(1500);
  GSM.println(i + 14);
  delay(1500);
  GSM.print("0011000B911356537837F80000AA");
  hexdump_byte(data_length & 0xFF);
 
  /* from sms_example_v2.c ALIEN Project Daniel Richman */
  l = 0;
  n = 0;
 
  for (x = 0; x < data_length; x++)
  {
    if (data[x] == '$')  data[x] = 0x02;
 
    n |= (data[x] & 0x7F) << l;
    l += 7;
 
    if (l >= 8)
    {
      hexdump_byte(n & 0xFF);
      l -= 8;
      n >>= 8;
    }
  }
 
  if (l != 0)
  {
    hexdump_byte(n & 0xFF);
  }
 
  GSM.println(0x1A, BYTE);
}
 
// DO NOT COPYRIGHT WITHOUT HIS PERMISSION
// ------ MISC ----------
 
 
// Returns a string with a textual representation of a float
void doubleToString(double val, int precision, char *string){
 
  // Print the int part
  sprintf(string, "%d", (int)(val));
  if(precision > 0) {
    // Print the decimal point
    strcat(string, ".");
    unsigned long frac;
    unsigned long mult = 1;
    int padding = precision -1;
    while (precision--) { 
      mult *=10; 
    }
    if (val >= 0)
      frac = (val - (int)(val)) * mult;
    else
      frac = ((int)(val)- val ) * mult;
    unsigned long frac1 = frac;
    while (frac1 /= 10) { 
      padding--; 
    }
    while (padding--) { 
      strcat(string, "0"); 
    }
 
    // Convert and print the fraction part
    sprintf(string+strlen(string), "%d", (int)(frac));
  }
}
 
 
// Converts a HEX string to an int
int atoh(char c) {
  if (c >= 'A' && c <= 'F')
    return c - 55;
  else if (c >= 'a' && c <= 'f')
    return c - 87;
  else
    return c - 48;
}

 
#include <Servo.h> 
 
Servo myservo;  // create servo object to control a servo 
// a maximum of eight servo objects can be created 
 
 
int inPin = 7;   // pushbutton connected to digital pin 7
int val = 0;     // variable to store the read value
int sum = 0;
 
 
void setup() 
{ 
  myservo.attach(8);  // attaches the servo on pin 9 to the servo object 
  Serial.begin(9600);
 
  pinMode(inPin, INPUT);      // sets the digital pin 7 as input
 
} 
 
 
void loop() 
{ 
  sum=0;
  val = digitalRead(inPin);   // read the input pin
  delay(500);
  while (val == 1){
    val = digitalRead(inPin);   // read the input pin
    delay(500);
    sum=sum+val;
    if (sum > 10){
      release();
    }
  } 
} 
 
void release(){
  myservo.attach(8);
  myservo.write(0);
  delay(2500);
  myservo.write(180);
  delay(2500);
  myservo.detach();
  delay(10000); //wait 10 sec
  releaseinv();
  delay(1000);
  releasevibrate();
  delay(10000);
}
 
void releaseinv(){
  myservo.attach(8);
  myservo.write(180);
  delay(5000);
  myservo.write(0);
  delay(5000);
  myservo.detach();
}
 
void releasevibrate(){
  myservo.attach(8);
  myservo.write(180);
  delay(500);
  myservo.write(0);
  delay(500);
  myservo.write(180);
  delay(500);
  myservo.write(0);
  delay(500);
  myservo.write(180);
  delay(500);
  myservo.write(0);
  delay(500);
  myservo.write(180);
  delay(3000);
  myservo.detach();
}