Autopilot Code

gistfile1.txt

#include "LedControl.h"
#include "Quadrature.h"
#include "math.h"


Quadrature quad1(38,39);


// ******   LedControl Library is required to drive the MAX7219 7 segment LED displays.  *******
// led_Display_1 is the variable name of my set of displays chained
LedControl led_Display_1 = LedControl(50,48,49,1); // together running off of pins 12,11,10.
// Pin 50 DN 
// Pin 48 is Clock
// Pin 49 is CS
// The 2 is for how many displays you have to chain.
// Allow 8 incase of expansion. It cost no memory.
// You can run any pins you want in the instruction,
// just plug the module in correctly.
// LedControl(Data,Clock,Load,Displays)
// I used 12,10,11 so I could have a straight ribbon connection
// from the module without crossing leads.

int CodeIn;// used on all serial reads
String Digit;    // Variable as a string to take from getChar()
int Count;       // This variable used in loops in the EQUALS() function
int AP_vs_set[4]; // AP_alt_set[5] is an array of the 5 digits that are the Auto Pilot AltitudeSet
char AP_vs_minus;
int AP_alt_set[5];
long Xold1;
String ap, apOld, fd, fdOld, hdg, hdgOld, nav, navOld, app, appOld, yd, ydOld, alt, altOld, bc, bcOld;
String gs, gsOld, mkr, mkrOld, avm, ga, gaOld, narm, narmOld, aarm, aarmOld;
int api, fdi, hdgi, navi, appi, ydi, alti, bci, gsi, mkri, avmi, gai, narmi, aarmi;
int R;// a variable
int Rold;// the old reading
int Rdif;// the difference since last loop
int Switch;
int KpinNo; 
int Koutpin;
int ledPin;


String KoldpinStateSTR, KpinStateSTR, Kstringnewstate,Kstringoldstate;





void setup()
{
    Kstringoldstate = "111111111111111111111111111111111111111111111111111111111111111111111";

  for (int KoutPin = 22; KoutPin <=40; KoutPin++)// Get all the pins ready for simconnect codes and "Keys"(all inputs)  
  {
    pinMode(KoutPin, INPUT);
    digitalWrite(KoutPin, HIGH); 
  }
    pinMode(A1, INPUT_PULLUP);
    pinMode(A2, INPUT_PULLUP);


   for (ledPin = 2; ledPin<=18; ledPin ++)
   {
    pinMode(ledPin, OUTPUT);
   }
    
 
  //The MAX72XX is in power-saving mode on startup, we have to do a wakeup call
  delay (500);
  led_Display_1.shutdown(0, false);     // I have 4 displays, these start them up
  //delay (500);                          // I put the delay in so they all dont start drawing
  //led_Display_1.shutdown(1,false);      // current at the same time.
  // delay (500);                          // I had an issue with running on only USB power and getting a display glitch.
  // led_Display_1.shutdown(2,false);      // The delay seems to have fixed this issue.
  // delay (500);
  // led_Display_1.shutdown(3,false);

  // Set the brightness to a lower than medium values
  led_Display_1.setIntensity(0, 8);
  //led_Display_1.setIntensity(1,8);
  // led_Display_1.setIntensity(2,8);
  //led_Display_1.setIntensity(3,8);

  // and clear the display
  led_Display_1.clearDisplay(0);
  //led_Display_1.clearDisplay(1);
  //led_Display_1.clearDisplay(2);
  //led_Display_1.clearDisplay(3);

  Serial.begin(115200);
  }


void loop() {
   {INPUTS();} //Check the Simconnect and "keys" section for any input pins
   {ENCODER();} //Check the Rotary Encoders
   {SWITCH();} // Check the Switch
   {ARM_or_COUPLED();} // Check this void
                    

        if (Serial.available()) 
    {
        CodeIn = getChar();
        if (CodeIn == '='){EQUALS();} // The first identifier is "="
        if (CodeIn == '#'){HASH();} // The first identifier is "#"
        if (CodeIn == '?'){QUESTION();} // The first identifier is "?"
    }
  }

char getChar()// Get a character from the serial buffer
{
  while (Serial.available() == 0); // wait for data
  return ((char)Serial.read()); // Thanks Doug
}// end of getchar void.

void SWITCH()
{
  if (digitalRead(A1) == LOW && avmi != 0)

  {
            led_Display_1.setDigit(0,4,AP_alt_set[0],false);   // First digit of Alt Set is displayed  
            led_Display_1.setDigit(0,3,AP_alt_set[1],false);   // Second digit of Alt Set is displayed 
            led_Display_1.setDigit(0,2,AP_alt_set[2],false);   // Third digit of Alt Set is displayed 
            led_Display_1.setDigit(0,1,AP_alt_set[3],false);   // Fourth digit of Alt Set is displayed 
            led_Display_1.setDigit(0,0,AP_alt_set[4],false);   // Fith digit of Alt Set is displayed
}
else if (digitalRead(A2) == LOW && avmi != 0)
{
            led_Display_1.setChar(0,4, AP_vs_minus,false);
            led_Display_1.setDigit(0,3, AP_vs_set[0],false);
            led_Display_1.setDigit(0,2, AP_vs_set[1],false);
            led_Display_1.setDigit(0,1, AP_vs_set[2],false);
            led_Display_1.setDigit(0,0, AP_vs_set[3],false);

  } 
}

void EQUALS() 
{   // The first identifier was "="
  CodeIn = getChar(); // Get the second identifier
  switch (CodeIn) 
  { // Now lets find what to do with it

    case 'x':{
      gs = "";
      gs += getChar();
      gs += getChar();
      gs += getChar();
      gs += getChar();
      gsi = gs.toInt();
      if((gsi != (-119)) && (appi > 0)){digitalWrite(4, HIGH);}else{digitalWrite(4, LOW);}
    }
    break;

    case 'a':{
      ap = "";
      ap += getChar();
      api = ap.toInt();
      if(api > 0){digitalWrite(12, HIGH);}else{digitalWrite(12, LOW);}
      ARM_or_COUPLED();
    }
    break;

    case 'q':{
      fd = "";
      fd += getChar();
      fdi = fd.toInt();
      if(fdi > 0){digitalWrite(11, HIGH);}else{digitalWrite(11, LOW);}
      if(fdi == 0){digitalWrite(17, LOW);}
    }
    break;

    case 'j':
      hdg = "";
      hdg += getChar();
      hdgi = hdg.toInt();
      if(hdgi > 0){digitalWrite(9, HIGH);}else{digitalWrite(9, LOW);}
    break;

    case 'o':
      nav = "";
      nav += getChar();
      navi = nav.toInt();
      if(navi > 0){digitalWrite(7, HIGH);}else{digitalWrite(7, LOW);}
    ARM_or_COUPLED();
      break;

    case 'm':
      app = "";
      app += getChar();
      appi = app.toInt();
      if(appi > 0){digitalWrite(6, HIGH);}else{digitalWrite(6, LOW);}
    ARM_or_COUPLED();
      break;

    case 'k':
     alt = "";
     alt += getChar();
     alti = alt.toInt();
     if(alti > 0){digitalWrite(8, HIGH);}else{digitalWrite(8, LOW);}
    break;

    case 'n':
      bc = "";
      bc += getChar();
      bci = bc.toInt();
      if(bci > 0){digitalWrite(5, HIGH);}else{digitalWrite(5, LOW);}
    break;

    case 'V':
      mkr = "";
      mkr += getChar();
      mkri = mkr.toInt();
      if (mkri == 1) {digitalWrite(16, HIGH);} else {digitalWrite(16, LOW);}
      if (mkri == 2) {digitalWrite(15, HIGH);} else {digitalWrite(15, LOW);}
      if (mkri == 3) {digitalWrite(14, HIGH);} else {digitalWrite(14, LOW);}
    break;

    case 'w':
      narm = "";
      narm += getChar();
      narm += getChar();
      narm += getChar();
      narm += getChar();
      narmi = narm.toInt();
      ARM_or_COUPLED();
    break; 



        //Com2 Active -- the download is fixed 6 character format eg. 123.45
  case 'b': {
    int tempInt; int count = 0;
    while (count < 5) {
      tempInt = getChar() - '0';
      if (tempInt != (-2)) {AP_alt_set[count] = tempInt; count++;} }
  }// close C
  break;

 // case 'c': {
  //  int tempInt; int count = 0;
   // while (count < 5) {
   //   tempInt = getChar() - '0';
    //  if (tempInt != (-2)) {AP_vs_set[count] = tempInt; count++;} }
 // }// close C   
   
    case 'c':    //  AP vertical speed set
      Count = 0;
      while (Count < 4)
      {
        Digit = "";
        Digit += getChar();
        if (Count == 0)
        {
          if (Digit == "-")
          {
            AP_vs_minus = '-';
          }
          else
          {
            AP_vs_minus = ' ';
          }
          Digit = "";
          Digit += getChar();
        }
        AP_vs_set[Count] = Digit.toInt();
        Count++;
      }
        led_Display_1.setChar(0,4, AP_vs_minus,false);
        led_Display_1.setDigit(0,3, AP_vs_set[0],false);
        led_Display_1.setDigit(0,2, AP_vs_set[1],false);
        led_Display_1.setDigit(0,1, AP_vs_set[2],false);
        led_Display_1.setDigit(0,0, AP_vs_set[3],false);
      
    }
  }// end of switch


void QUESTION()               // The first identifier was "?"
{  
    CodeIn = getChar();     // Get another character
    switch(CodeIn)          // Now lets find what to do with it
    {
      case 'U':
        avm = "";
        avm += getChar();
        avmi = avm.toInt();
        if (avmi != 0)
         {

            led_Display_1.setDigit(0,4,AP_alt_set[0],false);   // First digit of Alt Set is displayed  
            led_Display_1.setDigit(0,3,AP_alt_set[1],false);   // Second digit of Alt Set is displayed 
            led_Display_1.setDigit(0,2,AP_alt_set[2],false);   // Third digit of Alt Set is displayed 
            led_Display_1.setDigit(0,1,AP_alt_set[3],false);   // Fourth digit of Alt Set is displayed 
            led_Display_1.setDigit(0,0,AP_alt_set[4],false);
            led_Display_1.setChar(0,4, AP_vs_minus,false);
            led_Display_1.setDigit(0,3, AP_vs_set[0],false);
            led_Display_1.setDigit(0,2, AP_vs_set[1],false);
            led_Display_1.setDigit(0,1, AP_vs_set[2],false);
            led_Display_1.setDigit(0,0, AP_vs_set[3],false);
         }
         else
         {
        led_Display_1.setChar(0,4, ' ',false);
        led_Display_1.setChar(0,3, ' ',false);
        led_Display_1.setChar(0,2, ' ',false);
        led_Display_1.setChar(0,1, ' ',false);
        led_Display_1.setChar(0,0, ' ',false); 
      }
  }// end of switch
}// end of QUESTION void


void HASH()
{
 CodeIn = getChar(); // Get the second identifier
  switch (CodeIn) 
  { // Now lets find what to do with it

    case 'A':
      yd = "";
      yd += getChar();
      ydi = yd.toInt();
      if(ydi == 1){digitalWrite(10, HIGH);}else{digitalWrite(10, LOW);} 
    
  }
}

void ARM_or_COUPLED()
{
   if ((api == 1) && ((appi == 1) || (navi == 1)) && ((narmi > 63) || (narmi < (-63)))) {digitalWrite(2, LOW); digitalWrite(3, HIGH);} // armed
  else if ((api == 1) && ((appi == 1) || (navi == 1)) && ((narmi >= (-63) && (narmi <= 63)))) {digitalWrite(3, LOW); digitalWrite(2, HIGH);} // coupled
  else {digitalWrite(2, LOW); digitalWrite(3, LOW);}
}// end armed or coupled



 void INPUTS(){ // Simconnect codes and "Keys" section
  Kstringnewstate = "";
  for (int KpinNo = 22; KpinNo <=40; KpinNo++){ //set this to the input pins. (pins 10 to 19)
    KpinStateSTR = String(digitalRead(KpinNo)); //read the state of the pin
    KoldpinStateSTR = String(Kstringoldstate.charAt(KpinNo - 22));// gets the 'old' state of the pin from string
    if (KpinStateSTR != KoldpinStateSTR){// checks if it's different to the last reading of that pinNo
      if (KpinNo != 13){ // avoid using pin 13 as an input unless you know the tricks.
        
        if (KpinNo == 31 && KpinStateSTR == "0"){Serial.println ("B13");} //Increments VS
        if (KpinNo == 32 && KpinStateSTR == "0"){Serial.println ("B14");} //Decrements VS
        if (KpinNo == 27 && KpinStateSTR == "0"){Serial.println ("Z091");} //Alt Arm
        if (KpinNo == 26 && KpinStateSTR == "0"){Serial.println ("B05");} //Alt Hold
        if (KpinNo == 25 && KpinStateSTR == "0"){Serial.println ("Z081");} //Hdg Hold
        if (KpinNo == 22 && KpinStateSTR == "0"){Serial.println ("B01");} //Toggles AP Master
        if (KpinNo == 24 && KpinStateSTR == "0"){Serial.println ("Z071");} //Toggles Yaw Damper
        if (KpinNo == 28 && KpinStateSTR == "0"){Serial.println ("B10");} //Toggles Nav Hold
        if (KpinNo == 23 && KpinStateSTR == "0"){Serial.println ("B30");} //Toggles Flight Director
        if (KpinNo == 29 && KpinStateSTR == "0"){Serial.println ("B08");} //Toggles App Hold
        if (KpinNo == 30 && KpinStateSTR == "0"){Serial.println ("B09");} //Toggles Back Course
        if (KpinNo == 35 && KpinStateSTR == "0"){Serial.println ("A54");} //Toggles NAV/GPS
        if (KpinNo == 36 && KpinStateSTR == "0"){Serial.println ("A54");} //Toggles NAV/GPS

        // The following lines are for the go around function. I do not know how to include multiple 
        //Serial.println on on entry so did multiple....It works!
        if (KpinNo == 33 && KpinStateSTR == "0"){
          Serial.println ("Z081"); // Hdg hold on
          Serial.println ("B03"); // AP Master On
          Serial.println ("Z091"); // Alt Arm
          Serial.println ("B301"); // Flight Director
          Serial.println ("B211000"); // Sets vs to 1000fpm
          digitalWrite(17, HIGH);}
   
    
        
      }//end of 'its not pin 13'
    }//end of 'its different'       
    Kstringnewstate += KpinStateSTR;
  }//end of 'for' loop (read the pins)
  Kstringoldstate = Kstringnewstate;
}//end of INPUTS void

void ENCODER(){
R =(quad1.position()/2); //The /2 is to suit the encoder(See my website)
  if (R != Rold) { // checks to see if it different
    (Rdif = (R-Rold));// finds out the difference
   if (Rdif == 1) Serial.println ("B12");//Com1 Mhz down
   if (Rdif == -1) Serial.println ("B11"); //Com1 Mhz up
   Rold = R; // overwrites the old reading with the new one.
  } 
}