//level 6 heading based turn

#include <PWMServo.h> //level 1

#include <i2c_t3.h> //level 2
#include <Adafruit_10DOF.h> //level2
#include <Adafruit_Sensor.h> //level2
#include <Adafruit_LSM303_U.h> //level2
#include <Adafruit_BMP085_U.h> //level2
#include <Adafruit_Simple_AHRS.h> //level2


#include <TinyGPS.h> //level4

PWMServo myservo1;  // level 1 create servo object servo1
PWMServo myservo2;  // level 1 create servo object servo2

TinyGPS gps; //level 4

// Create sensor instances. //level2
Adafruit_LSM303_Accel_Unified accel(30301); 
Adafruit_LSM303_Mag_Unified   mag(30302);
Adafruit_BMP085_Unified       bmp(18001);

// Create simple AHRS algorithm using the above sensors. //level2
Adafruit_Simple_AHRS          ahrs(&accel, &mag);

//  LEVEL 2 Update this with the correct SLP for accurate altitude measurements
float seaLevelPressure = SENSORS_PRESSURE_SEALEVELHPA;

HardwareSerial Uart = HardwareSerial(); //level4
//--------------------
int pos1 = 140;
int pos2 = 50;
float poscom1 = pos1;
float poscom2 = pos2;
int n = 0;
int xint=0;
int loopcnt=100;
int turnlevel=50;

float heador = 0;
float headorOLD = 0;

float latgoal=37.362081;
float longgoal=-120.993190;

float brng = 0;
float brng1 = 0;
float turngain = .25;
float dist_calc = 0;
float ERRORhead = 1; // difference between heading and bearing
float ERabs = 1; 
float ERsplit=1;

//Use for Testing Altitude based logic! yea it mostly works
float testalt= 600; //meters

unsigned long course = 0;
//-------------------
void gpsdump(TinyGPS &gps);
void printFloat(double f, int digits = 2);

void setup() {
  Serial.begin(9600);
   Uart.begin(9600);
    
    myservo1.attach(3); //set servo 1 to pin 3 pwm level 1
    myservo2.attach(4); //set servo 2 to pin 4 pwm level 1
    
     
 delay(100);
  // Initialize the sensors. level2
  accel.begin();
  delay(10);
  mag.begin();
  delay(10);
  bmp.begin();
  delay(10);
  Serial.println("started");
  Serial.print("Servo1 Position, ");Serial.println(pos1);//level1
  Serial.print("Servo2 Position, ");Serial.println(pos2);//level1
     
  Serial.println("10 DOF Board AHRS Starting"); Serial.println("");//level2
}

void loop() {
  sensors_vec_t   orientation;
  delay(10);

  sensors_event_t event; //level3
  bmp.getEvent(&event); //level3
  
  myservo1.write(pos1);
  myservo2.write(pos2);

   bool newdata = false; //level4
  unsigned long start = millis(); //level4


   // Every 5 seconds we print an update //level4
  while (millis() - start < 5000) {
    if (Uart.available()) {
      char c = Uart.read();
       //Serial.print(c);  // uncomment to see raw GPS data
      if (gps.encode(c)) {
        newdata = true;
        //break;  // uncomment to print new data immediately!
      }
    }
  }

//turn circles when near waypoint
while(gps.f_altitude() < 500 && dist_calc < 1){

//while(testalt<15 && dist_calc < 0.8){
  delay(50);
myservo1.write(100);
myservo2.write(pos2);
Serial.print("right turn"); Serial.println();
Serial.print("Goal");Serial.println();
Serial.print(testalt);Serial.println();
delay(45000);
Serial.print("Hands Up");Serial.println();
myservo1.write(pos1);
myservo2.write(pos2);
delay(20000);
Serial.print("left turn");Serial.println();
myservo1.write(pos1);
myservo2.write(80);
delay(45000);
Serial.print("Hands Up");Serial.println();
myservo1.write(pos1);
myservo2.write(pos2);
delay(20000);
Serial.print("Distance to Goal: ");
    Serial.println(dist_calc);Serial.println();
}

//head for waypoint between release and waypoint

while((gps.f_altitude()) > 500 && gps.f_altitude() < 16150 ){
//while(testalt >15 && testalt < 1000){
  myservo1.write(pos1);
  myservo2.write(pos2);
  Serial.println();

brng = brng1;
 
//hands up for +/- 10 degrees on heading
  if(ERsplit < 180 && ERsplit > 160){
    turngain=1; 
   poscom1=pos1;
   poscom2 = pos2; 
   Serial.print("hands up");Serial.println();
}
 if (ERsplit < 20 && ERsplit > 0){
   turngain=1; 
   poscom1=pos1;
   poscom2 = pos2;
    Serial.print("hands up");Serial.println();
   }

   //

//small turn from +/- 10 to +/-45
  if(ERsplit < 160 && ERsplit > 135)
    turngain=0.4;
  if (ERsplit <45 && ERsplit >20)
    turngain=0.4;
    
//medium turn from +/-45 to +/-67.5
  if(ERsplit < 135 && ERsplit > 112.5)
    turngain=.7;
  if(ERsplit < 67.5 && ERsplit > 45)
    turngain=.7;

//fullturn from +/- 90 to +/- 67.5
  if(ERsplit < 112.5 && ERsplit > 67.5)
  turngain = .2;
 
Serial.print(" pos2   ");Serial.println(poscom2);
Serial.print(" pos1   ");Serial.println(poscom1);

if(ERRORhead > 0 && ERabs > 180){
  myservo2.write(poscom2);
  delay(500);
  Serial.println("Left (+)(>180)"); Serial.println();
}
if(ERRORhead > 0 && ERabs < 180){
  myservo1.write(poscom1);
  delay(500);
  Serial.println("Right (+)(<180)"); Serial.println();
}
if(ERRORhead < 0 && ERabs > 180){
  myservo1.write(poscom1);
  delay(500);
  Serial.println("Right (-)(>180)"); Serial.println();
}
if(ERRORhead < 0 && ERabs < 180){
  myservo2.write(poscom2);
  delay(500);
  Serial.println("Left (-)(<180)"); Serial.println();
}
//headorOLD = orientation.heading;
if (ahrs.getOrientation(&orientation))
  Serial.println("Orientation:  ");
//Serial.println(orientation.heading);
heador = course;//orientation.heading;// //200;// comment out orientation and put value to test
//heador = ((heador+headorOLD)/2);

//-----------------print heading .. in this case course. 
Serial.println(heador);

ERRORhead = brng - (heador);
ERabs = sqrt(sq(ERRORhead));
ERsplit = ERabs/2;

poscom1= floor(pos1-(turnlevel*turngain));
poscom2= floor(pos2+(turnlevel*turngain));

Serial.println("Error in Heading:   ");
Serial.print(ERRORhead); Serial.println();

//Serial.println("Error in split:   ");
//Serial.print(ERsplit); Serial.println();

//Serial.println("Turning Gain:  ");
//Serial.print(turngain); Serial.println();

xint=xint+1;
//Serial.println("Cnt: ");
//Serial.println(xint);
//Serial.println();

//level 3 pressure and alt
   // Serial.print("Pressure:    ");
   // Serial.print(event.pressure);
    //Serial.println(" hPa");
      /* Then convert the atmospheric pressure, and SLP to altitude         */
    /* Update this next line with the current SLP for better results      */
    float seaLevelPressure = SENSORS_PRESSURE_SEALEVELHPA;
    Serial.print("Altitude:    "); 
    Serial.print(bmp.pressureToAltitude(seaLevelPressure,
                                        event.pressure)); 
                                        Serial.println();
                                       Serial.print(testalt);
    Serial.println(" m");
    Serial.println("");
    if (newdata) {
    Serial.println("Acquired Data");
    Serial.println("-------------");
    gpsdump(gps);
    Serial.println("-------------");
    Serial.println();
    delay(2000);
    
  }
}

//Full Stall 
while(gps.f_altitude()> 16150 && gps.f_altitude()<24400){
//while(testalt>1000 && testalt <10000) {
  //Stall
myservo1.write(70);
myservo2.write(120);
Serial.println("FullStall");
Serial.println();
  
   
   if (newdata) {
    Serial.println("Acquired Data");
    Serial.println("-------------");
    gpsdump(gps);
    Serial.println("-------------");
    Serial.println();

    delay(2000);
}

delay(1000);
}  
}
//GPS function Level 4 Calculate GPS alt, Current Position, Bearing to Goal

void gpsdump(TinyGPS &gps)
{
  
  long lat, lon;
  float flat, flon;
  unsigned long age, date, time, course;//, chars;
  int year;
  byte month, day, hour, minute, second, hundredths;
 
   
   float dist_calc=0;
  float dist_calc2=0;   
   
   gps.get_position(&lat, &lon, &age);
   gps.f_get_position(&flat, &flon, &age);
course = gps.course();
   float teta1 = radians(flat);
    float teta2 = radians(latgoal);
    float delta1 = radians(latgoal- flat);
    float delta2 = radians(longgoal- flon);
  
  
  Serial.print("Lat/Long(float): "); printFloat(flat, 5); Serial.print(", "); printFloat(flon, 5);
  Serial.print(" Fix age: "); Serial.print(age); Serial.println("ms.");
  Serial.print("Alt(meters): "); printFloat(gps.f_altitude()); Serial.println();
  Serial.print("Speed(knots): "); printFloat(gps.f_speed_knots()); Serial.println();

  gps.crack_datetime(&year, &month, &day, &hour, &minute, &second, &hundredths, &age);
  Serial.print("Date: "); Serial.print(static_cast<int>(month)); Serial.print("/"); 
    Serial.print(static_cast<int>(day)); Serial.print("/"); Serial.print(year);
  Serial.print("  Time: "); Serial.print(static_cast<int>(hour)); Serial.print(":"); 
    Serial.print(static_cast<int>(minute)); Serial.print(":"); Serial.print(static_cast<int>(second));
    Serial.print("."); Serial.print(static_cast<int>(hundredths));
     Serial.println(" ");

//calcuate bearing
  float y = sin(delta2) * cos(teta2);
    float x = cos(teta1)*sin(teta2) - sin(teta1)*cos(teta2)*cos(delta2);
    brng1 = atan2(y,x);
    brng1 = degrees(brng1);// radians to degrees
    brng1 = ( ((int)brng1 + 360) % 360 ); 
     
     Serial.print("Bearing to Goal: ");
    Serial.println(brng1);

      //calculate range
dist_calc = (sin(delta1/2.0)*sin(delta1/2.0));
dist_calc2= cos(teta1);
dist_calc2*=cos(teta2);
dist_calc2*=sin(delta2/2.0);
dist_calc2*=sin(delta2/2.0);
dist_calc +=dist_calc2;

dist_calc=(2*atan2(sqrt(dist_calc),sqrt(1.0-dist_calc)));

dist_calc*=6371000.0 / 1000; //Converting to meters -> km
//Serial.println(dist_calc);

Serial.print("Distance to Goal: ");
    Serial.println(dist_calc);


}

  void printFloat(double number, int digits)
{
  // Handle negative numbers
  if (number < 0.0) {
     Serial.print('-');
     number = -number;
  }

  // Round correctly so that print(1.999, 2) prints as "2.00"
  double rounding = 0.5;
  for (uint8_t i=0; i<digits; ++i)
    rounding /= 10.0;
  
  number += rounding;

  // Extract the integer part of the number and print it
  unsigned long int_part = (unsigned long)number;
  double remainder = number - (double)int_part;
  Serial.print(int_part);

  // Print the decimal point, but only if there are digits beyond
  if (digits > 0)
    Serial.print("."); 

  // Extract digits from the remainder one at a time
  while (digits-- > 0) {
    remainder *= 10.0;
    int toPrint = int(remainder);
    Serial.print(toPrint);
    remainder -= toPrint;
  }
}

There are a couple things that I learned. There are also a few things that make paragliders an advantage when trying to automate. They are pendulum stable. This is a unique advantage.

There is a funny joke about how not many things when they break become almost as useful. The punch line.. is when the escalator breaks.. you have. stairs.. thank you. ill take the stairs. 

Why do I make this joke. When thinking about distributed sensors, those who take to the ocean, such as Saildrones or Sofar are at a bit of an advantage. If they loose the ability to steer or be mobile, they still can be a valuable data source floating along.. 

I make this comparison because a paraglider, unlike an airplane, quadcopter, or helicopter, is sometimes best left alone... hahah. 

When instructing paragliding I mention there are three things to manage. The equipment, the weather, and mindset. All of these are focused on the pilot managing their future self. 

If the air is calm. The wing is open.  Everything will work great. With no input. This is an interesting insight. 

It leads into the code, I promise. It means. do less. Do as little as you can to maintain heading. 

What about collapses, managing wing pressure you ask? Id say. For now. We leave all that out. There is more than enough to take care of before managing all that. I have a plan... Ill write about it.. but not yet. 

So that leaves us with.. 

Where do you want to go.. Where does the robot think it is, where does it think its going, How do I get from Robot State (where I am) to Robot State (where I want to go) 

To our advantage. We only have two outputs. Left and Right. 

We only have one set of inputs. GPS based bearing, GPS location. 

To make this all as simple as possible I pre-prescribed a final location waypoint. Why? so that I didnt have to use a compass. Each loop I took the error between bearing to the waypoint GPS location and the current GPS bearing. That would become the angle required to steer back to course.. 

With the error angle we have some choice to make!!! 

Where would you imagine the largest input would be... 

Lets walk through it.. 

Imagine a large angle like 180 degrees. Would you slam on a control to get back?  

What about a very small angle like 5 degrees. Should you even do anything? 

I drew a compass for all angles. 

I stared at it for a long time. Imagining what I do flying... I  have the ability to correct.. but my robot does not.. What would I tell a student on the radio.. 

I will post what I choose on this log when I dig up the picture.. or will just redraw it. Less is more.. Depending on altitude.. who cares how long it takes.. It only matters if you are not making it where you want to go.. Many times flying people put in big ears.. or spiral to get down.. when they could calmly go find sink or fly somewhere else.. This is important to layer onto how the robot is programed. 

What about modes?? Drop Mode. Recover Mode. Waypoint Mode. Thermal Mode. Team... etc etc. 

Ok well.. to make it easy again.. with limited sensing. Lets use modes triggered by altitude. 

We designed the balloon to go to 80,000ft.. with a planned drop at 60,000ft. 

At max altitude the air density is  = 0.0003097 kg/m^3

At Sealevel the air density is = 1.225 kg/m^3

If we can someday show this all works above 114,000ft then well. We also have a Martian explorer. I digress.

Ok.. Here is the challenge. Our speeds will be very different. Our control inputs will also have to vary.. by how much??!?!?! we actually dont know.. Less is more.. 

From later testing I learned about the torque issue with the servos.. Only from later testing.. I think if I had the right servos on the high altitude flight I would have fallen out of the sky.. BUT the servos were not capable of over controlling to the point of spin or stall.. WHAT I got soooo freaking lucky.. Happy accident for sure. 

I've uploaded what I used. as well as the new code on github.