initial commit master
initial commit

--- /dev/null
+++ b/ParamotorFC/Compass.ino
@@ -1,1 +1,28 @@
 
+void Compass_Heading()
+{
+  float MAG_X;
+  float MAG_Y;
+  float cos_roll;
+  float sin_roll;
+  float cos_pitch;
+  float sin_pitch;
+  
+  cos_roll = cos(roll);
+  sin_roll = sin(roll);
+  cos_pitch = cos(pitch);
+  sin_pitch = sin(pitch);
+  
+  // adjust for LSM303 compass axis offsets/sensitivity differences by scaling to +/-0.5 range
+  c_magnetom_x = (float)(magnetom_x - SENSOR_SIGN[6]*M_X_MIN) / (M_X_MAX - M_X_MIN) - SENSOR_SIGN[6]*0.5;
+  c_magnetom_y = (float)(magnetom_y - SENSOR_SIGN[7]*M_Y_MIN) / (M_Y_MAX - M_Y_MIN) - SENSOR_SIGN[7]*0.5;
+  c_magnetom_z = (float)(magnetom_z - SENSOR_SIGN[8]*M_Z_MIN) / (M_Z_MAX - M_Z_MIN) - SENSOR_SIGN[8]*0.5;
+  
+  // Tilt compensated Magnetic filed X:
+  MAG_X = c_magnetom_x*cos_pitch+c_magnetom_y*sin_roll*sin_pitch+c_magnetom_z*cos_roll*sin_pitch;
+  // Tilt compensated Magnetic filed Y:
+  MAG_Y = c_magnetom_y*cos_roll-c_magnetom_z*sin_roll;
+  // Magnetic Heading
+  MAG_Heading = atan2(-MAG_Y,MAG_X);
+}

+

--- /dev/null
+++ b/ParamotorFC/DCM.ino
@@ -1,1 +1,142 @@
 
+/**************************************************/
+void Normalize(void)
+{
+  float error=0;
+  float temporary[3][3];
+  float renorm=0;
+  
+  error= -Vector_Dot_Product(&DCM_Matrix[0][0],&DCM_Matrix[1][0])*.5; //eq.19
+
+  Vector_Scale(&temporary[0][0], &DCM_Matrix[1][0], error); //eq.19
+  Vector_Scale(&temporary[1][0], &DCM_Matrix[0][0], error); //eq.19
+  
+  Vector_Add(&temporary[0][0], &temporary[0][0], &DCM_Matrix[0][0]);//eq.19
+  Vector_Add(&temporary[1][0], &temporary[1][0], &DCM_Matrix[1][0]);//eq.19
+  
+  Vector_Cross_Product(&temporary[2][0],&temporary[0][0],&temporary[1][0]); // c= a x b //eq.20
+  
+  renorm= .5 *(3 - Vector_Dot_Product(&temporary[0][0],&temporary[0][0])); //eq.21
+  Vector_Scale(&DCM_Matrix[0][0], &temporary[0][0], renorm);
+  
+  renorm= .5 *(3 - Vector_Dot_Product(&temporary[1][0],&temporary[1][0])); //eq.21
+  Vector_Scale(&DCM_Matrix[1][0], &temporary[1][0], renorm);
+  
+  renorm= .5 *(3 - Vector_Dot_Product(&temporary[2][0],&temporary[2][0])); //eq.21
+  Vector_Scale(&DCM_Matrix[2][0], &temporary[2][0], renorm);
+}
+
+/**************************************************/
+void Drift_correction(void)
+{
+  float mag_heading_x;
+  float mag_heading_y;
+  float errorCourse;
+  //Compensation the Roll, Pitch and Yaw drift. 
+  static float Scaled_Omega_P[3];
+  static float Scaled_Omega_I[3];
+  float Accel_magnitude;
+  float Accel_weight;
+  
+  
+  //*****Roll and Pitch***************
+
+  // Calculate the magnitude of the accelerometer vector
+  Accel_magnitude = sqrt(Accel_Vector[0]*Accel_Vector[0] + Accel_Vector[1]*Accel_Vector[1] + Accel_Vector[2]*Accel_Vector[2]);
+  Accel_magnitude = Accel_magnitude / GRAVITY; // Scale to gravity.
+  // Dynamic weighting of accelerometer info (reliability filter)
+  // Weight for accelerometer info (<0.5G = 0.0, 1G = 1.0 , >1.5G = 0.0)
+  Accel_weight = constrain(1 - 2*abs(1 - Accel_magnitude),0,1);  //  
+
+  Vector_Cross_Product(&errorRollPitch[0],&Accel_Vector[0],&DCM_Matrix[2][0]); //adjust the ground of reference
+  Vector_Scale(&Omega_P[0],&errorRollPitch[0],Kp_ROLLPITCH*Accel_weight);
+  
+  Vector_Scale(&Scaled_Omega_I[0],&errorRollPitch[0],Ki_ROLLPITCH*Accel_weight);
+  Vector_Add(Omega_I,Omega_I,Scaled_Omega_I);     
+  
+  //*****YAW***************
+  // We make the gyro YAW drift correction based on compass magnetic heading
+ 
+  mag_heading_x = cos(MAG_Heading);
+  mag_heading_y = sin(MAG_Heading);
+  errorCourse=(DCM_Matrix[0][0]*mag_heading_y) - (DCM_Matrix[1][0]*mag_heading_x);  //Calculating YAW error
+  Vector_Scale(errorYaw,&DCM_Matrix[2][0],errorCourse); //Applys the yaw correction to the XYZ rotation of the aircraft, depeding the position.
+  
+  Vector_Scale(&Scaled_Omega_P[0],&errorYaw[0],Kp_YAW);//.01proportional of YAW.
+  Vector_Add(Omega_P,Omega_P,Scaled_Omega_P);//Adding  Proportional.
+  
+  Vector_Scale(&Scaled_Omega_I[0],&errorYaw[0],Ki_YAW);//.00001Integrator
+  Vector_Add(Omega_I,Omega_I,Scaled_Omega_I);//adding integrator to the Omega_I
+}
+/**************************************************/
+/*
+void Accel_adjust(void)
+{
+ Accel_Vector[1] += Accel_Scale(speed_3d*Omega[2]);  // Centrifugal force on Acc_y = GPS_speed*GyroZ
+ Accel_Vector[2] -= Accel_Scale(speed_3d*Omega[1]);  // Centrifugal force on Acc_z = GPS_speed*GyroY 
+}
+*/
+/**************************************************/
+
+void Matrix_update(void)
+{
+  Gyro_Vector[0]=Gyro_Scaled_X(gyro_x); //gyro x roll
+  Gyro_Vector[1]=Gyro_Scaled_Y(gyro_y); //gyro y pitch
+  Gyro_Vector[2]=Gyro_Scaled_Z(gyro_z); //gyro Z yaw
+  
+  Accel_Vector[0]=accel_x;
+  Accel_Vector[1]=accel_y;
+  Accel_Vector[2]=accel_z;
+    
+  Vector_Add(&Omega[0], &Gyro_Vector[0], &Omega_I[0]);  //adding proportional term
+  Vector_Add(&Omega_Vector[0], &Omega[0], &Omega_P[0]); //adding Integrator term
+
+  //Accel_adjust();    //Remove centrifugal acceleration.   We are not using this function in this version - we have no speed measurement
+  
+ #if OUTPUTMODE==1         
+  Update_Matrix[0][0]=0;
+  Update_Matrix[0][1]=-G_Dt*Omega_Vector[2];//-z
+  Update_Matrix[0][2]=G_Dt*Omega_Vector[1];//y
+  Update_Matrix[1][0]=G_Dt*Omega_Vector[2];//z
+  Update_Matrix[1][1]=0;
+  Update_Matrix[1][2]=-G_Dt*Omega_Vector[0];//-x
+  Update_Matrix[2][0]=-G_Dt*Omega_Vector[1];//-y
+  Update_Matrix[2][1]=G_Dt*Omega_Vector[0];//x
+  Update_Matrix[2][2]=0;
+ #else                    // Uncorrected data (no drift correction)
+  Update_Matrix[0][0]=0;
+  Update_Matrix[0][1]=-G_Dt*Gyro_Vector[2];//-z
+  Update_Matrix[0][2]=G_Dt*Gyro_Vector[1];//y
+  Update_Matrix[1][0]=G_Dt*Gyro_Vector[2];//z
+  Update_Matrix[1][1]=0;
+  Update_Matrix[1][2]=-G_Dt*Gyro_Vector[0];
+  Update_Matrix[2][0]=-G_Dt*Gyro_Vector[1];
+  Update_Matrix[2][1]=G_Dt*Gyro_Vector[0];
+  Update_Matrix[2][2]=0;
+ #endif
+
+  Matrix_Multiply(DCM_Matrix,Update_Matrix,Temporary_Matrix); //a*b=c
+
+  for(int x=0; x<3; x++) //Matrix Addition (update)
+  {
+    for(int y=0; y<3; y++)
+    {
+      DCM_Matrix[x][y]+=Temporary_Matrix[x][y];
+    } 
+  }
+}
+
+void Euler_angles(void)
+{
+  pitch = -asin(DCM_Matrix[2][0]);
+  roll = atan2(DCM_Matrix[2][1],DCM_Matrix[2][2]);
+  yaw = atan2(DCM_Matrix[1][0],DCM_Matrix[0][0]);
+  pitchDeg = ToDeg(pitch);
+  rollDeg = ToDeg(roll);
+  yawDeg = ToDeg(yaw);
+  if (yawDeg<0) {
+    yawDeg+=360;
+  }
+}
+

+

--- /dev/null
+++ b/ParamotorFC/I2C.ino
@@ -1,1 +1,99 @@
 
+#include <L3G.h>
+#include <LSM303.h>
+#include <LPS.h>
+
+L3G gyro;
+LSM303 compass;
+LPS ps;
+
+void I2C_Init()
+{
+  Wire.begin();
+}
+
+void Gyro_Init()
+{
+  gyro.init();
+  gyro.writeReg(L3G_CTRL_REG4, 0x20); // 2000 dps full scale
+  gyro.writeReg(L3G_CTRL_REG1, 0x0F); // normal power mode, all axes enabled, 100 Hz
+}
+
+void Ps_Init()
+{
+  if (!ps.init())
+  {
+    Serial.println("Failed to autodetect pressure sensor!");
+    while (1);
+  }
+  ps.enableDefault();
+}
+
+void Read_Ps()
+{
+  pressure = ps.readPressureMillibars();
+  altitude = ps.pressureToAltitudeMeters(pressure);
+  temperature = ps.readTemperatureC();
+  if (altitudeBase==0) {
+    altitudeBase=altitude;
+  }
+  altitude-=altitudeBase;
+}
+
+void Read_Gyro()
+{
+  gyro.read();
+  
+  AN[0] = gyro.g.x;
+  AN[1] = gyro.g.y;
+  AN[2] = gyro.g.z;
+  gyro_x = SENSOR_SIGN[0] * (AN[0] - AN_OFFSET[0]);
+  gyro_y = SENSOR_SIGN[1] * (AN[1] - AN_OFFSET[1]);
+  gyro_z = SENSOR_SIGN[2] * (AN[2] - AN_OFFSET[2]);
+}
+
+void Accel_Init()
+{
+  compass.init();
+  compass.enableDefault();
+  switch (compass.getDeviceType())
+  {
+    case LSM303::device_D:
+      compass.writeReg(LSM303::CTRL2, 0x18); // 8 g full scale: AFS = 011
+      break;
+    case LSM303::device_DLHC:
+      compass.writeReg(LSM303::CTRL_REG4_A, 0x28); // 8 g full scale: FS = 10; high resolution output mode
+      break;
+    default: // DLM, DLH
+      compass.writeReg(LSM303::CTRL_REG4_A, 0x30); // 8 g full scale: FS = 11
+  }
+}
+
+// Reads x,y and z accelerometer registers
+void Read_Accel()
+{
+  compass.readAcc();
+  
+  AN[3] = compass.a.x >> 4; // shift left 4 bits to use 12-bit representation (1 g = 256)
+  AN[4] = compass.a.y >> 4;
+  AN[5] = compass.a.z >> 4;
+  accel_x = SENSOR_SIGN[3] * (AN[3] - AN_OFFSET[3]);
+  accel_y = SENSOR_SIGN[4] * (AN[4] - AN_OFFSET[4]);
+  accel_z = SENSOR_SIGN[5] * (AN[5] - AN_OFFSET[5]);
+}
+
+void Compass_Init()
+{
+  // doesn't need to do anything because Accel_Init() should have already called compass.enableDefault()
+}
+
+void Read_Compass()
+{
+  compass.readMag();
+  
+  magnetom_x = SENSOR_SIGN[6] * compass.m.x;
+  magnetom_y = SENSOR_SIGN[7] * compass.m.y;
+  magnetom_z = SENSOR_SIGN[8] * compass.m.z;
+}
+

+

--- /dev/null
+++ b/ParamotorFC/Output.ino
@@ -1,1 +1,76 @@
 
+void printPs()
+{
+    dtostrf(pressure, 8, 2, dtostrfbuffer);
+    logger ("pressure %s", dtostrfbuffer);
+    dtostrf(altitude, 8, 2, dtostrfbuffer);
+    logger ("altitude %s", dtostrfbuffer);
+    dtostrf(temperature, 8, 2, dtostrfbuffer);
+    logger ("temperature %s", dtostrfbuffer);
+}
+
+void printdata(void)
+{    
+      //Serial.print("!");
+      #if PRINT_EULER == 1
+      Serial.println("---------------------------");      
+      Serial.print("ROLL:");
+      Serial.print(rollDeg);
+      Serial.print(" / PITCH:");
+      Serial.print(pitchDeg);
+      Serial.print(" / YAW:");
+      Serial.println(yawDeg);
+      //Serial.print("MAG_Heading:");
+      //Serial.println(MAG_Heading);
+      Serial.println("---------------------------");
+      #endif      
+      #if PRINT_ANALOGS==1
+      Serial.print(",AN:");
+      Serial.print(AN[0]);  //(int)read_adc(0)
+      Serial.print(",");
+      Serial.print(AN[1]);
+      Serial.print(",");
+      Serial.print(AN[2]);  
+      Serial.print(",");
+      Serial.print(AN[3]);
+      Serial.print (",");
+      Serial.print(AN[4]);
+      Serial.print (",");
+      Serial.print(AN[5]);
+      Serial.print(",");
+      Serial.print(c_magnetom_x);
+      Serial.print (",");
+      Serial.print(c_magnetom_y);
+      Serial.print (",");
+      Serial.print(c_magnetom_z);
+      #endif
+      /*#if PRINT_DCM == 1
+      Serial.print (",DCM:");
+      Serial.print(convert_to_dec(DCM_Matrix[0][0]));
+      Serial.print (",");
+      Serial.print(convert_to_dec(DCM_Matrix[0][1]));
+      Serial.print (",");
+      Serial.print(convert_to_dec(DCM_Matrix[0][2]));
+      Serial.print (",");
+      Serial.print(convert_to_dec(DCM_Matrix[1][0]));
+      Serial.print (",");
+      Serial.print(convert_to_dec(DCM_Matrix[1][1]));
+      Serial.print (",");
+      Serial.print(convert_to_dec(DCM_Matrix[1][2]));
+      Serial.print (",");
+      Serial.print(convert_to_dec(DCM_Matrix[2][0]));
+      Serial.print (",");
+      Serial.print(convert_to_dec(DCM_Matrix[2][1]));
+      Serial.print (",");
+      Serial.print(convert_to_dec(DCM_Matrix[2][2]));
+      #endif*/
+      Serial.println();    
+      
+}
+
+long convert_to_dec(float x)
+{
+  return x*10000000;
+}
+

+

--- /dev/null
+++ b/ParamotorFC/ParamotorFC.ino
@@ -1,1 +1,335 @@
-
+/************ Servos ***************/
+
+#include <Servo.h>
+
+Servo servoLeft;
+Servo servoRight;
+char _moveServo; float _moveServoVal = 0.0;
+int _servoBaseVal = 45;
+int _servoMaxVal = 70;
+/**********************************/
+
+float altitudeBase = 0;
+
+/* Heading */
+#define HEADINGPIN  10
+#define GPSPIN  12
+bool _headingSet = false;
+float _headingVal;
+unsigned long _pinValue;
+
+static char dtostrfbuffer[15];
+
+// Uncomment the below line to use this axis definition: 
+   // X axis pointing forward
+   // Y axis pointing to the right 
+   // and Z axis pointing down.
+// Positive pitch : nose up
+// Positive roll : right wing down
+// Positive yaw : clockwise
+//int SENSOR_SIGN[9] = {1,1,1,-1,-1,-1,1,1,1}; //Correct directions x,y,z - gyro, accelerometer, magnetometer
+// Uncomment the below line to use this axis definition: 
+   // X axis pointing forward
+   // Y axis pointing to the left 
+   // and Z axis pointing up.
+// Positive pitch : nose down
+// Positive roll : right wing down
+// Positive yaw : counterclockwise
+int SENSOR_SIGN[9] = {1,-1,-1,-1,1,1,1,-1,-1}; //Correct directions x,y,z - gyro, accelerometer, magnetometer
+
+// tested with Arduino Uno with ATmega328 and Arduino Duemilanove with ATMega168
+
+#include <Wire.h>
+
+// LSM303 accelerometer: 8 g sensitivity
+// 3.9 mg/digit; 1 g = 256
+#define GRAVITY 256  //this equivalent to 1G in the raw data coming from the accelerometer 
+
+#define ToRad(x) ((x)*0.01745329252)  // *pi/180
+#define ToDeg(x) ((x)*57.2957795131)  // *180/pi
+
+// L3G4200D gyro: 2000 dps full scale
+// 70 mdps/digit; 1 dps = 0.07
+#define Gyro_Gain_X 0.07 //X axis Gyro gain
+#define Gyro_Gain_Y 0.07 //Y axis Gyro gain
+#define Gyro_Gain_Z 0.07 //Z axis Gyro gain
+#define Gyro_Scaled_X(x) ((x)*ToRad(Gyro_Gain_X)) //Return the scaled ADC raw data of the gyro in radians for second
+#define Gyro_Scaled_Y(x) ((x)*ToRad(Gyro_Gain_Y)) //Return the scaled ADC raw data of the gyro in radians for second
+#define Gyro_Scaled_Z(x) ((x)*ToRad(Gyro_Gain_Z)) //Return the scaled ADC raw data of the gyro in radians for second
+
+// LSM303 magnetometer calibration constants; use the Calibrate example from
+// the Pololu LSM303 library to find the right values for your board
+#define M_X_MIN -421
+#define M_Y_MIN -639
+#define M_Z_MIN -238
+#define M_X_MAX 424
+#define M_Y_MAX 295
+#define M_Z_MAX 472
+
+#define Kp_ROLLPITCH 0.02
+#define Ki_ROLLPITCH 0.00002
+#define Kp_YAW 1.2
+#define Ki_YAW 0.00002
+
+/*For debugging purposes*/
+//OUTPUTMODE=1 will print the corrected data, 
+//OUTPUTMODE=0 will print uncorrected data of the gyros (with drift)
+#define OUTPUTMODE 1
+
+//#define PRINT_DCM 0     //Will print the whole direction cosine matrix
+#define PRINT_ANALOGS 0 //Will print the analog raw data
+#define PRINT_EULER 0   //Will print the Euler angles Roll, Pitch and Yaw
+
+#define STATUS_LED 13.
+
+float G_Dt=0.02;    // Integration time (DCM algorithm)  We will run the integration loop at 50Hz if possible
+
+long timer=0;   //general purpuse timer
+long timer_old;
+long timer24=0; //Second timer used to print values 
+int AN[6]; //array that stores the gyro and accelerometer data
+int AN_OFFSET[6]={0,0,0,0,0,0}; //Array that stores the Offset of the sensors
+
+int gyro_x;
+int gyro_y;
+int gyro_z;
+int accel_x;
+int accel_y;
+int accel_z;
+int magnetom_x;
+int magnetom_y;
+int magnetom_z;
+float c_magnetom_x;
+float c_magnetom_y;
+float c_magnetom_z;
+float MAG_Heading;
+
+float Accel_Vector[3]= {0,0,0}; //Store the acceleration in a vector
+float Gyro_Vector[3]= {0,0,0};//Store the gyros turn rate in a vector
+float Omega_Vector[3]= {0,0,0}; //Corrected Gyro_Vector data
+float Omega_P[3]= {0,0,0};//Omega Proportional correction
+float Omega_I[3]= {0,0,0};//Omega Integrator
+float Omega[3]= {0,0,0};
+
+// Euler angles
+float roll;
+float pitch;
+float yaw;
+float rollDeg;
+float pitchDeg;
+float yawDeg;
+
+float errorRollPitch[3]= {0,0,0}; 
+float errorYaw[3]= {0,0,0};
+
+unsigned int counter=0;
+byte gyro_sat=0;
+
+float pressure;
+float altitude;
+float temperature;
+
+float DCM_Matrix[3][3]= {
+  {
+    1,0,0  }
+  ,{
+    0,1,0  }
+  ,{
+    0,0,1  }
+}; 
+float Update_Matrix[3][3]={{0,1,2},{3,4,5},{6,7,8}}; //Gyros here
+
+
+float Temporary_Matrix[3][3]={
+  {
+    0,0,0  }
+  ,{
+    0,0,0  }
+  ,{
+    0,0,0  }
+};
+ 
+void setup()
+{ 
+  Serial.begin(115200);
+  //pinMode (STATUS_LED,OUTPUT);  // Status LED
+  
+  I2C_Init();
+
+  delay (2000);
+  Serial.println("Pololu MinIMU-10 v4 + Arduino AHRS");
+
+  //digitalWrite(STATUS_LED,LOW);
+  delay(1500);
+ 
+  Accel_Init();
+  Compass_Init();
+  Gyro_Init();
+  Ps_Init();
+  
+  delay(20);
+  
+  for(int i=0;i<32;i++)    // We take some readings...
+    {
+    Read_Gyro();
+    Read_Accel();
+    for(int y=0; y<6; y++)   // Cumulate values
+      AN_OFFSET[y] += AN[y];
+    delay(20);
+    }
+    
+  for(int y=0; y<6; y++)
+    AN_OFFSET[y] = AN_OFFSET[y]/32;
+    
+  AN_OFFSET[5]-=GRAVITY*SENSOR_SIGN[5];
+  
+  //Serial.println("Offset:");
+  for(int y=0; y<6; y++)
+    //Serial.println(AN_OFFSET[y]);
+  
+  //delay(2000);
+  //digitalWrite(STATUS_LED,HIGH);
+    
+  timer=millis();
+  delay(20);
+  counter=0;
+  
+  servoLeft.attach(6);
+  servoRight.attach(5);
+  
+  pinMode (HEADINGPIN, INPUT);
+  pinMode (GPSPIN, INPUT);
+}
+
+void loop() //Main Loop
+{
+  if((millis()-timer)>=20)  // Main loop runs at 50Hz
+  {
+    counter++;
+    timer_old = timer;
+    timer=millis();
+    if (timer>timer_old)
+      G_Dt = (timer-timer_old)/1000.0;    // Real time of loop run. We use this on the DCM algorithm (gyro integration time)
+    else
+      G_Dt = 0;
+    
+    // *** DCM algorithm
+    // Data adquisition
+    Read_Gyro();   // This read gyro data
+    Read_Accel();     // Read I2C accelerometer
+    Read_Ps();
+    
+    if (counter > 5)  // Read compass data at 10Hz... (5 loop runs)
+      {
+      counter=0;
+      Read_Compass();    // Read I2C magnetometer
+      Compass_Heading(); // Calculate magnetic heading  
+      }
+    
+    // Calculations...
+    Matrix_update();
+    Normalize();
+    Drift_correction();
+    Euler_angles();
+    // ***
+  
+    watchPins();
+    tempHeadingToFollow();
+    yawBasedHeading();  
+  
+    //printPs();
+   
+    //printdata();
+  }
+   
+}
+
+void logger(char *format,...)
+{
+  char buff[128];
+  va_list args;
+  va_start (args,format);
+  vsnprintf(buff,sizeof(buff),format,args);
+  va_end (args);
+  buff[sizeof(buff)/sizeof(buff[0])-1]='\0';
+  Serial.println(buff);
+}
+
+void tempHeadingToFollow()
+{
+  if ( pitchDeg > 30 ) {
+    _headingSet = true;
+    _headingVal = yawDeg;
+  }  
+}
+
+void watchPins()
+{
+  _pinValue = pulseIn(HEADINGPIN, HIGH);
+  if ( _headingSet == false && _pinValue == 90 ) {
+    _headingSet = true;
+    _headingVal = yawDeg;
+  } else if ( _pinValue == 0 ) {
+    _headingSet = false;
+    _headingVal = 0;
+  }
+  //dtostrf(_pinValue, 8, 2, dtostrfbuffer);
+  //logger ("HEADINGPIN : %s", dtostrfbuffer);
+  
+  _pinValue = pulseIn(GPSPIN, HIGH);
+  //dtostrf(_pinValue, 8, 2, dtostrfbuffer);
+  //logger ("GPSPIN : %s", dtostrfbuffer);
+}
+
+void yawBasedHeading ()
+{
+    if ( _headingSet ) {
+      servoMovement ();
+    }
+}
+
+void servoMovement ()
+{
+    if ( _headingVal >= 0 && _headingVal <= 180 ) { // NES (0-180)
+      if ( yawDeg < _headingVal ) {
+        _moveServo = 'r';
+        _moveServoVal = _headingVal - yawDeg;
+      } else if ( yawDeg > (180+_headingVal) ) {
+        _moveServo = 'r';
+        _moveServoVal = 360 - yawDeg + _headingVal;
+      } else if ( yawDeg > _headingVal && yawDeg < (180+_headingVal) ) {
+        _moveServo = 'l';
+        _moveServoVal = yawDeg - _headingVal;
+      }
+    } else if ( _headingVal > 180 && _headingVal <= 360 ) { // NVS (181-360)
+      if ( yawDeg > _headingVal ) {
+        _moveServo = 'l';
+        _moveServoVal = yawDeg - _headingVal;        
+      } else if ( yawDeg < (_headingVal-180) ) {
+         _moveServo = 'l';        
+        _moveServoVal = 360 - _headingVal + yawDeg;
+      } else if ( yawDeg < _headingVal && yawDeg > (_headingVal-180) ) {
+        _moveServo = 'r';        
+        _moveServoVal = _headingVal - yawDeg;
+      }
+    }
+    _moveServoVal = (_servoBaseVal+_moveServoVal) < _servoMaxVal ? (_servoBaseVal+_moveServoVal) : _servoMaxVal;
+    if ( _moveServoVal > _servoBaseVal ) {
+      if ( _moveServo == 'l' ) {
+        servoLeft.write(180-_moveServoVal);
+      }
+      if ( _moveServo == 'r' ) {
+        servoRight.write(_moveServoVal);
+      }
+      dtostrf(_moveServoVal, 8, 2, dtostrfbuffer);
+      logger ("move servo %c : %s", _moveServo, dtostrfbuffer);
+    } else {
+        servoLeft.write(180-_servoBaseVal);
+        servoRight.write(_servoBaseVal);
+    }
+}
+
+float Smoothing(float raw, float smooth){
+  return (raw * (0.15)) + (smooth * 0.85);
+}

+

--- /dev/null
+++ b/ParamotorFC/Vector.ino
@@ -1,1 +1,42 @@
 
+//Computes the dot product of two vectors
+float Vector_Dot_Product(float vector1[3],float vector2[3])
+{
+  float op=0;
+  
+  for(int c=0; c<3; c++)
+  {
+  op+=vector1[c]*vector2[c];
+  }
+  
+  return op; 
+}
+
+//Computes the cross product of two vectors
+void Vector_Cross_Product(float vectorOut[3], float v1[3],float v2[3])
+{
+  vectorOut[0]= (v1[1]*v2[2]) - (v1[2]*v2[1]);
+  vectorOut[1]= (v1[2]*v2[0]) - (v1[0]*v2[2]);
+  vectorOut[2]= (v1[0]*v2[1]) - (v1[1]*v2[0]);
+}
+
+//Multiply the vector by a scalar. 
+void Vector_Scale(float vectorOut[3],float vectorIn[3], float scale2)
+{
+  for(int c=0; c<3; c++)
+  {
+   vectorOut[c]=vectorIn[c]*scale2; 
+  }
+}
+
+void Vector_Add(float vectorOut[3],float vectorIn1[3], float vectorIn2[3])
+{
+  for(int c=0; c<3; c++)
+  {
+     vectorOut[c]=vectorIn1[c]+vectorIn2[c];
+  }
+}
+
+
+

+

--- /dev/null
+++ b/ParamotorFC/libraries/L3G/L3G.cpp
@@ -1,1 +1,149 @@
+#include <L3G.h>
+#include <Wire.h>
+#include <math.h>
 
+// Defines ////////////////////////////////////////////////////////////////
+
+// The Arduino two-wire interface uses a 7-bit number for the address,
+// and sets the last bit correctly based on reads and writes
+#define L3G4200D_ADDRESS_SA0_LOW  (0xD0 >> 1)
+#define L3G4200D_ADDRESS_SA0_HIGH (0xD2 >> 1)
+#define L3GD20_ADDRESS_SA0_LOW    (0xD4 >> 1)
+#define L3GD20_ADDRESS_SA0_HIGH   (0xD6 >> 1)
+
+// Public Methods //////////////////////////////////////////////////////////////
+
+bool L3G::init(byte device, byte sa0)
+{
+  _device = device;
+  switch (_device)
+  {
+    case L3G4200D_DEVICE:
+      if (sa0 == L3G_SA0_LOW)
+      {
+        address = L3G4200D_ADDRESS_SA0_LOW;
+        return true;
+      }
+      else if (sa0 == L3G_SA0_HIGH)
+      {
+        address = L3G4200D_ADDRESS_SA0_HIGH;
+        return true;
+      }
+      else
+        return autoDetectAddress();
+      break;
+
+    case L3GD20_DEVICE:
+      if (sa0 == L3G_SA0_LOW)
+      {
+        address = L3GD20_ADDRESS_SA0_LOW;
+        return true;
+      }
+      else if (sa0 == L3G_SA0_HIGH)
+      {
+        address = L3GD20_ADDRESS_SA0_HIGH;
+        return true;
+      }
+      else
+        return autoDetectAddress();
+      break;
+
+    default:
+      return autoDetectAddress();
+  }
+}
+
+// Turns on the L3G's gyro and places it in normal mode.
+void L3G::enableDefault(void)
+{
+  // 0x0F = 0b00001111
+  // Normal power mode, all axes enabled
+  writeReg(L3G_CTRL_REG1, 0x0F);
+}
+
+// Writes a gyro register
+void L3G::writeReg(byte reg, byte value)
+{
+  Wire.beginTransmission(address);
+  Wire.write(reg);
+  Wire.write(value);
+  Wire.endTransmission();
+}
+
+// Reads a gyro register
+byte L3G::readReg(byte reg)
+{
+  byte value;
+
+  Wire.beginTransmission(address);
+  Wire.write(reg);
+  Wire.endTransmission();
+  Wire.requestFrom(address, (byte)1);
+  value = Wire.read();
+  Wire.endTransmission();
+
+  return value;
+}
+
+// Reads the 3 gyro channels and stores them in vector g
+void L3G::read()
+{
+  Wire.beginTransmission(address);
+  // assert the MSB of the address to get the gyro
+  // to do slave-transmit subaddress updating.
+  Wire.write(L3G_OUT_X_L | (1 << 7));
+  Wire.endTransmission();
+  Wire.requestFrom(address, (byte)6);
+
+  while (Wire.available() < 6);
+
+  uint8_t xlg = Wire.read();
+  uint8_t xhg = Wire.read();
+  uint8_t ylg = Wire.read();
+  uint8_t yhg = Wire.read();
+  uint8_t zlg = Wire.read();
+  uint8_t zhg = Wire.read();
+
+  // combine high and low bytes
+  g.x = (int16_t)(xhg << 8 | xlg);
+  g.y = (int16_t)(yhg << 8 | ylg);
+  g.z = (int16_t)(zhg << 8 | zlg);
+}
+
+void L3G::vector_cross(const vector *a,const vector *b, vector *out)
+{
+  out->x = a->y*b->z - a->z*b->y;
+  out->y = a->z*b->x - a->x*b->z;
+  out->z = a->x*b->y - a->y*b->x;
+}
+
+float L3G::vector_dot(const vector *a,const vector *b)
+{
+  return a->x*b->x+a->y*b->y+a->z*b->z;
+}
+
+void L3G::vector_normalize(vector *a)
+{
+  float mag = sqrt(vector_dot(a,a));
+  a->x /= mag;
+  a->y /= mag;
+  a->z /= mag;
+}
+
+// Private Methods //////////////////////////////////////////////////////////////
+
+bool L3G::autoDetectAddress(void)
+{
+  // try each possible address and stop if reading WHO_AM_I returns the expected response
+  address = L3G4200D_ADDRESS_SA0_LOW;
+  if (readReg(L3G_WHO_AM_I) == 0xD3) return true;
+  address = L3G4200D_ADDRESS_SA0_HIGH;
+  if (readReg(L3G_WHO_AM_I) == 0xD3) return true;
+  address = L3GD20_ADDRESS_SA0_LOW;
+  if (readReg(L3G_WHO_AM_I) == 0xD4 || readReg(L3G_WHO_AM_I) == 0xD7) return true;
+  address = L3GD20_ADDRESS_SA0_HIGH;
+  if (readReg(L3G_WHO_AM_I) == 0xD4 || readReg(L3G_WHO_AM_I) == 0xD7) return true;
+
+  return false;
+}
+

--- /dev/null
+++ b/ParamotorFC/libraries/L3G/L3G.h
@@ -1,1 +1,89 @@
+#ifndef L3G_h
+#define L3G_h
 
+#include <Arduino.h> // for byte data type
+
+// device types
+
+#define L3G_DEVICE_AUTO 0
+#define L3G4200D_DEVICE 1
+#define L3GD20_DEVICE   2
+
+
+// SA0 states
+
+#define L3G_SA0_LOW  0
+#define L3G_SA0_HIGH 1
+#define L3G_SA0_AUTO 2
+
+// register addresses
+
+#define L3G_WHO_AM_I      0x0F
+
+#define L3G_CTRL_REG1     0x20
+#define L3G_CTRL_REG2     0x21
+#define L3G_CTRL_REG3     0x22
+#define L3G_CTRL_REG4     0x23
+#define L3G_CTRL_REG5     0x24
+#define L3G_REFERENCE     0x25
+#define L3G_OUT_TEMP      0x26
+#define L3G_STATUS_REG    0x27
+
+#define L3G_OUT_X_L       0x28
+#define L3G_OUT_X_H       0x29
+#define L3G_OUT_Y_L       0x2A
+#define L3G_OUT_Y_H       0x2B
+#define L3G_OUT_Z_L       0x2C
+#define L3G_OUT_Z_H       0x2D
+
+#define L3G_FIFO_CTRL_REG 0x2E
+#define L3G_FIFO_SRC_REG  0x2F
+
+#define L3G_INT1_CFG      0x30
+#define L3G_INT1_SRC      0x31
+#define L3G_INT1_THS_XH   0x32
+#define L3G_INT1_THS_XL   0x33
+#define L3G_INT1_THS_YH   0x34
+#define L3G_INT1_THS_YL   0x35
+#define L3G_INT1_THS_ZH   0x36
+#define L3G_INT1_THS_ZL   0x37
+#define L3G_INT1_DURATION 0x38
+#define L3G_LOW_ODR       0x39
+
+
+class L3G
+{
+  public:
+    typedef struct vector
+    {
+      float x, y, z;
+    } vector;
+
+    vector g; // gyro angular velocity readings
+
+    bool init(byte device = L3G_DEVICE_AUTO, byte sa0 = L3G_SA0_AUTO);
+
+    void enableDefault(void);
+
+    void writeReg(byte reg, byte value);
+    byte readReg(byte reg);
+
+    void read(void);
+
+    // vector functions
+    static void vector_cross(const vector *a, const vector *b, vector *out);
+    static float vector_dot(const vector *a,const vector *b);
+    static void vector_normalize(vector *a);
+
+  private:
+      byte _device; // chip type (4200D or D20)
+      byte address;
+
+      bool autoDetectAddress(void);
+};
+
+#endif
+
+
+
+

--- /dev/null
+++ b/ParamotorFC/libraries/L3G/examples/Serial/Serial.ino
@@ -1,1 +1,32 @@
+#include <Wire.h>
+#include <L3G.h>
 
+L3G gyro;
+
+void setup() {
+  Serial.begin(9600);
+  Wire.begin();
+
+  if (!gyro.init())
+  {
+    Serial.println("Failed to autodetect gyro type!");
+    while (1);
+  }
+
+  gyro.enableDefault();
+}
+
+void loop() {
+  gyro.read();
+
+  Serial.print("G ");
+  Serial.print("X: ");
+  Serial.print((int)gyro.g.x);
+  Serial.print(" Y: ");
+  Serial.print((int)gyro.g.y);
+  Serial.print(" Z: ");
+  Serial.println((int)gyro.g.z);
+
+  delay(100);
+}
+

--- /dev/null
+++ b/ParamotorFC/libraries/L3G/keywords.txt
@@ -1,1 +1,43 @@
+L3G	KEYWORD1
 
+init	KEYWORD2
+enableDefault	KEYWORD2
+writeReg	KEYWORD2
+readReg	KEYWORD2
+read	KEYWORD2
+vector_cross	KEYWORD2
+vector_dot	KEYWORD2
+vector_normalize	KEYWORD2
+
+L3G_DEVICE_AUTO	LITERAL1
+L3G4200D_DEVICE	LITERAL1
+L3GD20_DEVICE	LITERAL1
+L3G_SA0_LOW	LITERAL1
+L3G_SA0_HIGH	LITERAL1
+L3G_SA0_AUTO	LITERAL1
+L3G_WHO_AM_I	LITERAL1
+L3G_CTRL_REG1	LITERAL1
+L3G_CTRL_REG2	LITERAL1
+L3G_CTRL_REG3	LITERAL1
+L3G_CTRL_REG4	LITERAL1
+L3G_CTRL_REG5	LITERAL1
+L3G_REFERENCE	LITERAL1
+L3G_OUT_TEMP	LITERAL1
+L3G_STATUS_REG	LITERAL1
+L3G_OUT_X_L	LITERAL1
+L3G_OUT_X_H	LITERAL1
+L3G_OUT_Y_L	LITERAL1
+L3G_OUT_Y_H	LITERAL1
+L3G_OUT_Z_L	LITERAL1
+L3G_OUT_Z_H	LITERAL1
+L3G_FIFO_CTRL_REG	LITERAL1
+L3G_FIFO_SRC_REG	LITERAL1
+L3G_INT1_CFG	LITERAL1
+L3G_INT1_SRC	LITERAL1
+L3G_INT1_THS_XH	LITERAL1
+L3G_INT1_THS_XL	LITERAL1
+L3G_INT1_THS_YH	LITERAL1
+L3G_INT1_THS_YL	LITERAL1
+L3G_INT1_THS_ZH	LITERAL1
+L3G_INT1_THS_ZL	LITERAL1
+L3G_INT1_DURATION	LITERAL1

--- /dev/null
+++ b/ParamotorFC/libraries/LPS/LPS.cpp
@@ -1,1 +1,235 @@
-
+#include <LPS.h>
+#include <Wire.h>
+
+// Defines ///////////////////////////////////////////////////////////
+
+// The Arduino two-wire interface uses a 7-bit number for the address,
+// and sets the last bit correctly based on reads and writes
+#define SA0_LOW_ADDRESS  0b1011100
+#define SA0_HIGH_ADDRESS 0b1011101
+
+#define TEST_REG_NACK -1
+
+#define LPS331AP_WHO_ID 0xBB
+#define LPS25H_WHO_ID   0xBD
+
+// Constructors //////////////////////////////////////////////////////
+
+LPS::LPS(void)
+{
+  _device = device_auto;
+  
+  // Pololu board pulls SA0 high, so default assumption is that it is
+  // high
+  address = SA0_HIGH_ADDRESS;
+}
+
+// Public Methods ////////////////////////////////////////////////////
+
+// sets or detects device type and slave address; returns bool indicating success
+bool LPS::init(deviceType device, byte sa0)
+{
+  if (!detectDeviceAndAddress(device, (sa0State)sa0))
+    return false;
+    
+  switch (_device)
+  {
+    case device_25H:
+      translated_regs[-INTERRUPT_CFG] = LPS25H_INTERRUPT_CFG;
+      translated_regs[-INT_SOURCE]    = LPS25H_INT_SOURCE;
+      translated_regs[-THS_P_L]       = LPS25H_THS_P_L;
+      translated_regs[-THS_P_H]       = LPS25H_THS_P_H;
+      return true;
+      break;
+      
+    case device_331AP:
+      translated_regs[-INTERRUPT_CFG] = LPS331AP_INTERRUPT_CFG;
+      translated_regs[-INT_SOURCE]    = LPS331AP_INT_SOURCE;
+      translated_regs[-THS_P_L]       = LPS331AP_THS_P_L;
+      translated_regs[-THS_P_H]       = LPS331AP_THS_P_H;
+      return true;
+      break;
+  }
+}
+
+// turns on sensor and enables continuous output
+void LPS::enableDefault(void)
+{
+  if (_device == device_25H)
+  {
+    // 0xB0 = 0b10110000
+    // PD = 1 (active mode);  ODR = 011 (12.5 Hz pressure & temperature output data rate)
+    writeReg(CTRL_REG1, 0xB0);
+  }
+  else if (_device == device_331AP)
+  {
+    // 0xE0 = 0b11100000
+    // PD = 1 (active mode);  ODR = 110 (12.5 Hz pressure & temperature output data rate)
+    writeReg(CTRL_REG1, 0xE0);
+  }
+}
+
+// writes register
+void LPS::writeReg(int reg, byte value)
+{
+  // if dummy register address, look up actual translated address (based on device type)
+  if (reg < 0)
+  {
+    reg = translated_regs[-reg];
+  }
+
+  Wire.beginTransmission(address);
+  Wire.write(reg);
+  Wire.write(value);
+  Wire.endTransmission();
+}
+
+// reads register
+byte LPS::readReg(int reg)
+{
+  byte value;
+  
+  // if dummy register address, look up actual translated address (based on device type)
+  if (reg < 0)
+  {
+    reg = translated_regs[-reg];
+  }
+
+  Wire.beginTransmission(address);
+  Wire.write(reg);
+  Wire.endTransmission(false); // restart
+  Wire.requestFrom(address, (byte)1);
+  value = Wire.read();
+  Wire.endTransmission();
+
+  return value;
+}
+
+// reads pressure in millibars (mbar)/hectopascals (hPa)
+float LPS::readPressureMillibars(void)
+{
+  return (float)readPressureRaw() / 4096;
+}
+
+// reads pressure in inches of mercury (inHg)
+float LPS::readPressureInchesHg(void)
+{
+  return (float)readPressureRaw() / 138706.5;
+}
+
+// reads pressure and returns raw 24-bit sensor output
+int32_t LPS::readPressureRaw(void)
+{
+  Wire.beginTransmission(address);
+  // assert MSB to enable register address auto-increment
+  Wire.write(PRESS_OUT_XL | (1 << 7));
+  Wire.endTransmission();
+  Wire.requestFrom(address, (byte)3);
+
+  while (Wire.available() < 3);
+
+  uint8_t pxl = Wire.read();
+  uint8_t pl = Wire.read();
+  uint8_t ph = Wire.read();
+
+  // combine bytes
+  return (int32_t)(int8_t)ph << 16 | (uint16_t)pl << 8 | pxl;
+}
+
+// reads temperature in degrees C
+float LPS::readTemperatureC(void)
+{
+  return 42.5 + (float)readTemperatureRaw() / 480;
+}
+
+// reads temperature in degrees F
+float LPS::readTemperatureF(void)
+{
+  return 108.5 + (float)readTemperatureRaw() / 480 * 1.8;
+}
+
+// reads temperature and returns raw 16-bit sensor output
+int16_t LPS::readTemperatureRaw(void)
+{
+  Wire.beginTransmission(address);
+  // assert MSB to enable register address auto-increment
+  Wire.write(TEMP_OUT_L | (1 << 7));
+  Wire.endTransmission();
+  Wire.requestFrom(address, (byte)2);
+
+  while (Wire.available() < 2);
+
+  uint8_t tl = Wire.read();
+  uint8_t th = Wire.read();
+
+  // combine bytes
+  return (int16_t)(th << 8 | tl);
+}
+
+// converts pressure in mbar to altitude in meters, using 1976 US
+// Standard Atmosphere model (note that this formula only applies to a
+// height of 11 km, or about 36000 ft)
+//  If altimeter setting (QNH, barometric pressure adjusted to sea
+//  level) is given, this function returns an indicated altitude
+//  compensated for actual regional pressure; otherwise, it returns
+//  the pressure altitude above the standard pressure level of 1013.25
+//  mbar or 29.9213 inHg
+float LPS::pressureToAltitudeMeters(float pressure_mbar, float altimeter_setting_mbar)
+{
+  return (1 - pow(pressure_mbar / altimeter_setting_mbar, 0.190263)) * 44330.8;
+}
+
+// converts pressure in inHg to altitude in feet; see notes above
+float LPS::pressureToAltitudeFeet(float pressure_inHg, float altimeter_setting_inHg)
+{
+  return (1 - pow(pressure_inHg / altimeter_setting_inHg, 0.190263)) * 145442;
+}
+
+// Private Methods ///////////////////////////////////////////////////
+
+bool LPS::detectDeviceAndAddress(deviceType device, sa0State sa0)
+{
+  if (sa0 == sa0_auto || sa0 == sa0_high)
+  {
+    address = SA0_HIGH_ADDRESS;
+    if (detectDevice(device)) return true;
+  }
+  if (sa0 == sa0_auto || sa0 == sa0_low)
+  {
+    address = SA0_LOW_ADDRESS;
+    if (detectDevice(device)) return true;
+  }
+
+  return false;
+}
+
+bool LPS::detectDevice(deviceType device)
+{
+  int id = testWhoAmI(address);
+  
+  if ((device == device_auto || device == device_25H) && id == LPS25H_WHO_ID)
+  {
+    _device = device_25H;
+    return true;
+  }
+  if ((device == device_auto || device == device_331AP) && id == LPS331AP_WHO_ID)
+  {
+    _device = device_331AP;
+    return true;
+  }
+
+  return false;
+}
+
+int LPS::testWhoAmI(byte address)
+{
+  Wire.beginTransmission(address);
+  Wire.write(WHO_AM_I);
+  Wire.endTransmission();
+
+  Wire.requestFrom(address, (byte)1);
+  if (Wire.available())
+    return Wire.read();
+  else
+    return TEST_REG_NACK;
+}

--- /dev/null
+++ b/ParamotorFC/libraries/LPS/LPS.h
@@ -1,1 +1,110 @@
+#ifndef LPS_h
+#define LPS_h
 
+#include <Arduino.h> // for byte data type
+
+class LPS
+{
+  public:
+    enum deviceType { device_331AP, device_25H, device_auto };
+    enum sa0State { sa0_low, sa0_high, sa0_auto };
+
+    // register addresses
+    // Note: where register names differ between the register mapping table and
+    // the register descriptions in the datasheets, the names from the register
+    // descriptions are used here.
+    enum regAddr
+    {
+      REF_P_XL                = 0x08,
+      REF_P_L                 = 0x09,
+      REF_P_H                 = 0x0A,
+                              
+      WHO_AM_I                = 0x0F,
+                              
+      RES_CONF                = 0x10,
+                              
+      CTRL_REG1               = 0x20,
+      CTRL_REG2               = 0x21,
+      CTRL_REG3               = 0x22,
+      CTRL_REG4               = 0x23, // 25H
+              
+      STATUS_REG              = 0x27,
+                            
+      PRESS_OUT_XL            = 0x28,
+      PRESS_OUT_L             = 0x29,
+      PRESS_OUT_H             = 0x2A,
+
+      TEMP_OUT_L              = 0x2B,
+      TEMP_OUT_H              = 0x2C,
+      
+      FIFO_CTRL               = 0x2E, // 25H
+      FIFO_STATUS             = 0x2F, // 25H
+      
+      AMP_CTRL                = 0x30, // 331AP
+      
+      RPDS_L                  = 0x39, // 25H
+      RPDS_H                  = 0x3A, // 25H
+      
+      DELTA_PRESS_XL          = 0x3C, // 331AP
+      DELTA_PRESS_L           = 0x3D, // 331AP
+      DELTA_PRESS_H           = 0x3E, // 331AP
+      
+      
+      // dummy addresses for registers in different locations on different devices;
+      // the library translates these based on device type
+      // value with sign flipped is used as index into translated_regs array
+    
+      INTERRUPT_CFG    = -1,
+      INT_SOURCE       = -2,
+      THS_P_L          = -3,
+      THS_P_H          = -4,
+      // update dummy_reg_count if registers are added here!
+      
+      
+      // device-specific register addresses
+      
+      LPS331AP_INTERRUPT_CFG  = 0x23,
+      LPS331AP_INT_SOURCE     = 0x24,
+      LPS331AP_THS_P_L        = 0x25,
+      LPS331AP_THS_P_H        = 0x26,
+      
+      LPS25H_INTERRUPT_CFG    = 0x24,
+      LPS25H_INT_SOURCE       = 0x25,
+      LPS25H_THS_P_L          = 0x30,
+      LPS25H_THS_P_H          = 0x31,
+    };
+
+    LPS(void);
+
+    bool init(deviceType device = device_auto, byte sa0 = sa0_auto);
+    deviceType getDeviceType(void) { return _device; }
+    byte getAddress(void) { return address; }
+
+    void enableDefault(void);
+
+    void writeReg(int reg, byte value);
+    byte readReg(int reg);
+
+    float readPressureMillibars(void);
+    float readPressureInchesHg(void);
+    int32_t readPressureRaw(void);
+    float readTemperatureC(void);
+    float readTemperatureF(void);
+    int16_t readTemperatureRaw(void);
+
+    static float pressureToAltitudeMeters(float pressure_mbar, float altimeter_setting_mbar = 1013.25);
+    static float pressureToAltitudeFeet(float pressure_inHg, float altimeter_setting_inHg = 29.9213);
+
+  private:
+    deviceType _device; // chip type (331AP or 25H)
+    byte address;
+    
+    static const int dummy_reg_count = 4;
+    regAddr translated_regs[dummy_reg_count + 1]; // index 0 not used
+
+    bool detectDeviceAndAddress(deviceType device, sa0State sa0);
+    bool detectDevice(deviceType device);
+    int testWhoAmI(byte address);
+};
+
+#endif

--- /dev/null
+++ b/ParamotorFC/libraries/LPS/examples/SerialMetric/SerialMetric.ino
@@ -1,1 +1,36 @@
+#include <Wire.h>
+#include <LPS.h>
 
+LPS ps;
+
+void setup()
+{
+  Serial.begin(9600);
+  Wire.begin();
+
+  if (!ps.init())
+  {
+    Serial.println("Failed to autodetect pressure sensor!");
+    while (1);
+  }
+
+  ps.enableDefault();
+}
+
+void loop()
+{
+  float pressure = ps.readPressureMillibars();
+  float altitude = ps.pressureToAltitudeMeters(pressure);
+  float temperature = ps.readTemperatureC();
+  
+  Serial.print("p: ");
+  Serial.print(pressure);
+  Serial.print(" mbar\ta: ");
+  Serial.print(altitude);
+  Serial.print(" m\tt: ");
+  Serial.print(temperature);
+  Serial.println(" deg C");
+
+  delay(100);
+}
+

--- /dev/null
+++ b/ParamotorFC/libraries/LPS/examples/SerialUS/SerialUS.ino
@@ -1,1 +1,36 @@
+#include <Wire.h>
+#include <LPS.h>
 
+LPS ps;
+
+void setup()
+{
+  Serial.begin(9600);
+  Wire.begin();
+
+  if (!ps.init())
+  {
+    Serial.println("Failed to autodetect pressure sensor!");
+    while (1);
+  }
+
+  ps.enableDefault();
+}
+
+void loop()
+{
+  float pressure = ps.readPressureInchesHg();
+  float altitude = ps.pressureToAltitudeFeet(pressure);
+  float temperature = ps.readTemperatureF();
+
+  Serial.print("p: ");
+  Serial.print(pressure);
+  Serial.print(" inHg\ta: ");
+  Serial.print(altitude);
+  Serial.print(" ft\tt: ");
+  Serial.print(temperature);
+  Serial.println(" deg F");
+
+  delay(100);
+}
+

--- /dev/null
+++ b/ParamotorFC/libraries/LPS/keywords.txt
@@ -1,1 +1,61 @@
+LPS	KEYWORD1
 
+init	KEYWORD2
+getDeviceType	KEYWORD2
+getAddress	KEYWORD2
+enableDefault	KEYWORD2
+writeReg	KEYWORD2
+readReg	KEYWORD2
+readPressureMillibars	KEYWORD2
+readPressureInchesHg	KEYWORD2
+readPressureRaw	KEYWORD2
+readTemperatureC	KEYWORD2
+readTemperatureF	KEYWORD2
+readTemperatureRaw	KEYWORD2
+pressureToAltitudeMeters	KEYWORD2
+pressureToAltitudeFeet	KEYWORD2
+
+device_331AP	LITERAL1
+device_25H	LITERAL1
+device_auto	LITERAL1
+sa0_low	LITERAL1
+sa0_high	LITERAL1
+sa0_auto	LITERAL1
+SA0_LOW	LITERAL1
+SA0_HIGH	LITERAL1
+SA0_AUTO	LITERAL1
+REF_P_XL	LITERAL1
+REF_P_L	LITERAL1
+REF_P_H	LITERAL1
+WHO_AM_I	LITERAL1
+RES_CONF	LITERAL1
+CTRL_REG1	LITERAL1
+CTRL_REG2	LITERAL1
+CTRL_REG3	LITERAL1
+CTRL_REG4	LITERAL1
+STATUS_REG	LITERAL1
+PRESS_OUT_XL	LITERAL1
+PRESS_OUT_L	LITERAL1
+PRESS_OUT_H	LITERAL1
+TEMP_OUT_L	LITERAL1
+TEMP_OUT_H	LITERAL1
+FIFO_CTRL	LITERAL1
+FIFO_STATUS	LITERAL1
+AMP_CTRL	LITERAL1
+RPDS_L	LITERAL1
+RPDS_H	LITERAL1
+DELTA_PRESS_XL	LITERAL1
+DELTA_PRESS_L	LITERAL1
+DELTA_PRESS_H	LITERAL1
+INTERRUPT_CFG	LITERAL1
+INT_SOURCE	LITERAL1
+THS_P_L	LITERAL1
+THS_P_H	LITERAL1
+LPS331AP_INTERRUPT_CFG	LITERAL1
+LPS331AP_INT_SOURCE	LITERAL1
+LPS331AP_THS_P_L	LITERAL1
+LPS331AP_THS_P_H	LITERAL1
+LPS25H_INTERRUPT_CFG	LITERAL1
+LPS25H_INT_SOURCE	LITERAL1
+LPS25H_THS_P_L	LITERAL1
+LPS25H_THS_P_H	LITERAL1

--- /dev/null
+++ b/ParamotorFC/libraries/LSM303/LSM303.cpp
@@ -1,1 +1,570 @@
-
+#include <LSM303.h>

+#include <Wire.h>

+#include <math.h>

+

+// Defines ////////////////////////////////////////////////////////////////

+

+// The Arduino two-wire interface uses a 7-bit number for the address,

+// and sets the last bit correctly based on reads and writes

+#define D_SA0_HIGH_ADDRESS              0b0011101 // D with SA0 high

+#define D_SA0_LOW_ADDRESS               0b0011110 // D with SA0 low or non-D magnetometer

+#define NON_D_MAG_ADDRESS               0b0011110 // D with SA0 low or non-D magnetometer

+#define NON_D_ACC_SA0_LOW_ADDRESS       0b0011000 // non-D accelerometer with SA0 low

+#define NON_D_ACC_SA0_HIGH_ADDRESS      0b0011001 // non-D accelerometer with SA0 high

+

+#define TEST_REG_NACK -1

+

+#define D_WHO_ID    0x49

+#define DLM_WHO_ID  0x3C

+

+// Constructors ////////////////////////////////////////////////////////////////

+

+LSM303::LSM303(void)

+{

+  /*

+  These values lead to an assumed magnetometer bias of 0.

+  Use the Calibrate example program to determine appropriate values

+  for your particular unit. The Heading example demonstrates how to

+  adjust these values in your own sketch.

+  */

+  m_min = (LSM303::vector<int16_t>){-32767, -32767, -32767};

+  m_max = (LSM303::vector<int16_t>){+32767, +32767, +32767};

+

+  _device = device_auto;

+

+  io_timeout = 0;  // 0 = no timeout

+  did_timeout = false;

+}

+

+// Public Methods //////////////////////////////////////////////////////////////

+

+// Did a timeout occur in readAcc(), readMag(), or read() since the last call to timeoutOccurred()?

+bool LSM303::timeoutOccurred()

+{

+  bool tmp = did_timeout;

+  did_timeout = false;

+  return tmp;

+

+}

+

+void LSM303::setTimeout(unsigned int timeout)

+{

+  io_timeout = timeout;

+}

+

+unsigned int LSM303::getTimeout()

+{

+  return io_timeout;

+}

+

+bool LSM303::init(deviceType device, sa0State sa0)

+{

+  // determine device type if necessary

+  if (device == device_auto)

+  {

+    if (testReg(D_SA0_HIGH_ADDRESS, WHO_AM_I) == D_WHO_ID)

+    {

+      // device responds to address 0011101 with D ID; it's a D with SA0 high

+      device = device_D;

+      sa0 = sa0_high;

+    }

+    else if (testReg(D_SA0_LOW_ADDRESS, WHO_AM_I) == D_WHO_ID)

+    {

+      // device responds to address 0011110 with D ID; it's a D with SA0 low

+      device = device_D;

+      sa0 = sa0_low;

+    }

+    // Remaining possibilities: DLHC, DLM, or DLH. DLHC seems to respond to WHO_AM_I request the

+    // same way as DLM, even though this register isn't documented in its datasheet, so instead,

+    // guess if it's a DLHC based on acc address (Pololu boards pull SA0 low on DLM and DLH;

+    // DLHC doesn't have SA0 but uses same acc address as DLH/DLM with SA0 high).

+    else if (testReg(NON_D_ACC_SA0_HIGH_ADDRESS, CTRL_REG1_A) != TEST_REG_NACK)

+    {

+      // device responds to address 0011001; guess that it's a DLHC

+      device = device_DLHC;

+      sa0 = sa0_high;

+    }

+    // Remaining possibilities: DLM or DLH. Check acc with SA0 low address to make sure it's responsive

+    else if (testReg(NON_D_ACC_SA0_LOW_ADDRESS, CTRL_REG1_A) != TEST_REG_NACK)

+    {

+      // device responds to address 0011000 with DLM ID; guess that it's a DLM

+      sa0 = sa0_low;

+

+      // Now check WHO_AM_I_M

+      if (testReg(NON_D_MAG_ADDRESS, WHO_AM_I_M) == DLM_WHO_ID)

+      {

+        device = device_DLM;

+      }

+      else

+      {

+        device = device_DLH;

+      }

+    }

+    else

+    {

+      // device hasn't responded meaningfully, so give up

+      return false;

+    }

+  }

+

+  // determine SA0 if necessary

+  if (sa0 == sa0_auto)

+  {

+    if (device == device_D)

+    {

+      if (testReg(D_SA0_HIGH_ADDRESS, WHO_AM_I) == D_WHO_ID)

+      {

+        sa0 = sa0_high;

+      }

+      else if (testReg(D_SA0_LOW_ADDRESS, WHO_AM_I) == D_WHO_ID)

+      {

+        sa0 = sa0_low;

+      }

+      else

+      {

+        // no response on either possible address; give up

+        return false;

+      }

+    }

+    else if (device == device_DLM || device == device_DLH)

+    {

+      if (testReg(NON_D_ACC_SA0_HIGH_ADDRESS, CTRL_REG1_A) != TEST_REG_NACK)

+      {

+        sa0 = sa0_high;

+      }

+      else if (testReg(NON_D_ACC_SA0_LOW_ADDRESS, CTRL_REG1_A) != TEST_REG_NACK)

+      {

+        sa0 = sa0_low;

+      }

+      else

+      {

+        // no response on either possible address; give up

+        return false;

+      }

+    }

+  }

+

+  _device = device;

+

+  // set device addresses and translated register addresses

+  switch (device)

+  {

+    case device_D:

+      acc_address = mag_address = (sa0 == sa0_high) ? D_SA0_HIGH_ADDRESS : D_SA0_LOW_ADDRESS;

+      translated_regs[-OUT_X_L_M] = D_OUT_X_L_M;

+      translated_regs[-OUT_X_H_M] = D_OUT_X_H_M;

+      translated_regs[-OUT_Y_L_M] = D_OUT_Y_L_M;

+      translated_regs[-OUT_Y_H_M] = D_OUT_Y_H_M;

+      translated_regs[-OUT_Z_L_M] = D_OUT_Z_L_M;

+      translated_regs[-OUT_Z_H_M] = D_OUT_Z_H_M;

+      break;

+

+    case device_DLHC:

+      acc_address = NON_D_ACC_SA0_HIGH_ADDRESS; // DLHC doesn't have SA0 but uses same acc address as DLH/DLM with SA0 high

+      mag_address = NON_D_MAG_ADDRESS;

+      translated_regs[-OUT_X_H_M] = DLHC_OUT_X_H_M;

+      translated_regs[-OUT_X_L_M] = DLHC_OUT_X_L_M;

+      translated_regs[-OUT_Y_H_M] = DLHC_OUT_Y_H_M;

+      translated_regs[-OUT_Y_L_M] = DLHC_OUT_Y_L_M;

+      translated_regs[-OUT_Z_H_M] = DLHC_OUT_Z_H_M;

+      translated_regs[-OUT_Z_L_M] = DLHC_OUT_Z_L_M;

+      break;

+

+    case device_DLM:

+      acc_address = (sa0 == sa0_high) ? NON_D_ACC_SA0_HIGH_ADDRESS : NON_D_ACC_SA0_LOW_ADDRESS;

+      mag_address = NON_D_MAG_ADDRESS;

+      translated_regs[-OUT_X_H_M] = DLM_OUT_X_H_M;

+      translated_regs[-OUT_X_L_M] = DLM_OUT_X_L_M;

+      translated_regs[-OUT_Y_H_M] = DLM_OUT_Y_H_M;

+      translated_regs[-OUT_Y_L_M] = DLM_OUT_Y_L_M;

+      translated_regs[-OUT_Z_H_M] = DLM_OUT_Z_H_M;

+      translated_regs[-OUT_Z_L_M] = DLM_OUT_Z_L_M;

+      break;

+

+    case device_DLH:

+      acc_address = (sa0 == sa0_high) ? NON_D_ACC_SA0_HIGH_ADDRESS : NON_D_ACC_SA0_LOW_ADDRESS;

+      mag_address = NON_D_MAG_ADDRESS;

+      translated_regs[-OUT_X_H_M] = DLH_OUT_X_H_M;

+      translated_regs[-OUT_X_L_M] = DLH_OUT_X_L_M;

+      translated_regs[-OUT_Y_H_M] = DLH_OUT_Y_H_M;

+      translated_regs[-OUT_Y_L_M] = DLH_OUT_Y_L_M;

+      translated_regs[-OUT_Z_H_M] = DLH_OUT_Z_H_M;

+      translated_regs[-OUT_Z_L_M] = DLH_OUT_Z_L_M;

+      break;

+  }

+  return true;

+}

+

+/*

+Enables the LSM303's accelerometer and magnetometer. Also:

+- Sets sensor full scales (gain) to default power-on values, which are

+  +/- 2 g for accelerometer and +/- 1.3 gauss for magnetometer

+  (+/- 4 gauss on LSM303D).

+- Selects 50 Hz ODR (output data rate) for accelerometer and 7.5 Hz

+  ODR for magnetometer (6.25 Hz on LSM303D). (These are the ODR

+  settings for which the electrical characteristics are specified in

+  the datasheets.)

+- Enables high resolution modes (if available).

+Note that this function will also reset other settings controlled by

+the registers it writes to.

+*/

+void LSM303::enableDefault(void)

+{

+

+  if (_device == device_D)

+  {

+    // Accelerometer

+

+    // 0x57 = 0b01010111

+    // AFS = 0 (+/- 2 g full scale)

+    writeReg(CTRL2, 0x00);

+

+    // 0x57 = 0b01010111

+    // AODR = 0101 (50 Hz ODR); AZEN = AYEN = AXEN = 1 (all axes enabled)

+    writeReg(CTRL1, 0x57);

+

+    // Magnetometer

+

+    // 0x64 = 0b01100100

+    // M_RES = 11 (high resolution mode); M_ODR = 001 (6.25 Hz ODR)

+    writeReg(CTRL5, 0x64);

+

+    // 0x20 = 0b00100000

+    // MFS = 01 (+/- 4 gauss full scale)

+    writeReg(CTRL6, 0x20);

+

+    // 0x00 = 0b00000000

+    // MLP = 0 (low power mode off); MD = 00 (continuous-conversion mode)

+    writeReg(CTRL7, 0x00);

+  }

+  else if (_device == device_DLHC)

+  {

+    // Accelerometer

+

+    // 0x08 = 0b00001000

+    // FS = 00 (+/- 2 g full scale); HR = 1 (high resolution enable)

+    writeAccReg(CTRL_REG4_A, 0x08);

+

+    // 0x47 = 0b01000111

+    // ODR = 0100 (50 Hz ODR); LPen = 0 (normal mode); Zen = Yen = Xen = 1 (all axes enabled)

+    writeAccReg(CTRL_REG1_A, 0x47);

+

+    // Magnetometer

+

+    // 0x0C = 0b00001100

+    // DO = 011 (7.5 Hz ODR)

+    writeMagReg(CRA_REG_M, 0x0C);

+

+    // 0x20 = 0b00100000

+    // GN = 001 (+/- 1.3 gauss full scale)

+    writeMagReg(CRB_REG_M, 0x20);

+

+    // 0x00 = 0b00000000

+    // MD = 00 (continuous-conversion mode)

+    writeMagReg(MR_REG_M, 0x00);

+  }

+  else // DLM, DLH

+  {

+    // Accelerometer

+

+    // 0x00 = 0b00000000

+    // FS = 00 (+/- 2 g full scale)

+    writeAccReg(CTRL_REG4_A, 0x00);

+

+    // 0x27 = 0b00100111

+    // PM = 001 (normal mode); DR = 00 (50 Hz ODR); Zen = Yen = Xen = 1 (all axes enabled)

+    writeAccReg(CTRL_REG1_A, 0x27);

+

+    // Magnetometer

+

+    // 0x0C = 0b00001100

+    // DO = 011 (7.5 Hz ODR)

+    writeMagReg(CRA_REG_M, 0x0C);

+

+    // 0x20 = 0b00100000

+    // GN = 001 (+/- 1.3 gauss full scale)

+    writeMagReg(CRB_REG_M, 0x20);

+

+    // 0x00 = 0b00000000

+    // MD = 00 (continuous-conversion mode)

+    writeMagReg(MR_REG_M, 0x00);

+  }

+}

+

+// Writes an accelerometer register

+void LSM303::writeAccReg(regAddr reg, byte value)

+{

+  Wire.beginTransmission(acc_address);

+  Wire.write((byte)reg);

+  Wire.write(value);

+  last_status = Wire.endTransmission();

+}

+

+// Reads an accelerometer register

+byte LSM303::readAccReg(regAddr reg)

+{

+  byte value;

+

+  Wire.beginTransmission(acc_address);

+  Wire.write((byte)reg);

+  last_status = Wire.endTransmission();

+  Wire.requestFrom(acc_address, (byte)1);

+  value = Wire.read();

+  Wire.endTransmission();

+

+  return value;

+}

+

+// Writes a magnetometer register

+void LSM303::writeMagReg(regAddr reg, byte value)

+{

+  Wire.beginTransmission(mag_address);

+  Wire.write((byte)reg);

+  Wire.write(value);

+  last_status = Wire.endTransmission();

+}

+

+// Reads a magnetometer register

+byte LSM303::readMagReg(regAddr reg)

+{

+  byte value;

+

+  // if dummy register address (magnetometer Y/Z), look up actual translated address (based on device type)

+  if (reg < 0)

+  {

+    reg = translated_regs[-reg];

+  }

+

+  Wire.beginTransmission(mag_address);

+  Wire.write((byte)reg);

+  last_status = Wire.endTransmission();

+  Wire.requestFrom(mag_address, (byte)1);

+  value = Wire.read();

+  Wire.endTransmission();

+

+  return value;

+}

+

+void LSM303::writeReg(regAddr reg, byte value)

+{

+  // mag address == acc_address for LSM303D, so it doesn't really matter which one we use.

+  // Use writeMagReg so it can translate OUT_[XYZ]_[HL]_M

+  if (_device == device_D || reg < CTRL_REG1_A)

+  {

+    writeMagReg(reg, value);

+  }

+  else

+  {

+    writeAccReg(reg, value);

+  }

+}

+

+// Note that this function will not work for reading TEMP_OUT_H_M and TEMP_OUT_L_M on the DLHC.

+// To read those two registers, use readMagReg() instead.

+byte LSM303::readReg(regAddr reg)

+{

+  // mag address == acc_address for LSM303D, so it doesn't really matter which one we use.

+  // Use writeMagReg so it can translate OUT_[XYZ]_[HL]_M

+  if (_device == device_D || reg < CTRL_REG1_A)

+  {

+    return readMagReg(reg);

+  }

+  else

+  {

+    return readAccReg(reg);

+  }

+}

+

+// Reads the 3 accelerometer channels and stores them in vector a

+void LSM303::readAcc(void)

+{

+  Wire.beginTransmission(acc_address);

+  // assert the MSB of the address to get the accelerometer

+  // to do slave-transmit subaddress updating.

+  Wire.write(OUT_X_L_A | (1 << 7));

+  last_status = Wire.endTransmission();

+  Wire.requestFrom(acc_address, (byte)6);

+

+  unsigned int millis_start = millis();

+  while (Wire.available() < 6) {

+    if (io_timeout > 0 && ((unsigned int)millis() - millis_start) > io_timeout)

+    {

+      did_timeout = true;

+      return;

+    }

+  }

+

+  byte xla = Wire.read();

+  byte xha = Wire.read();

+  byte yla = Wire.read();

+  byte yha = Wire.read();

+  byte zla = Wire.read();

+  byte zha = Wire.read();

+

+  // combine high and low bytes

+  // This no longer drops the lowest 4 bits of the readings from the DLH/DLM/DLHC, which are always 0

+  // (12-bit resolution, left-aligned). The D has 16-bit resolution

+  a.x = (int16_t)(xha << 8 | xla);

+  a.y = (int16_t)(yha << 8 | yla);

+  a.z = (int16_t)(zha << 8 | zla);

+}

+

+// Reads the 3 magnetometer channels and stores them in vector m

+void LSM303::readMag(void)

+{

+  Wire.beginTransmission(mag_address);

+  // If LSM303D, assert MSB to enable subaddress updating

+  // OUT_X_L_M comes first on D, OUT_X_H_M on others

+  Wire.write((_device == device_D) ? translated_regs[-OUT_X_L_M] | (1 << 7) : translated_regs[-OUT_X_H_M]);

+  last_status = Wire.endTransmission();

+  Wire.requestFrom(mag_address, (byte)6);

+

+  unsigned int millis_start = millis();

+  while (Wire.available() < 6) {

+    if (io_timeout > 0 && ((unsigned int)millis() - millis_start) > io_timeout)

+    {

+      did_timeout = true;

+      return;

+    }

+  }

+

+  byte xlm, xhm, ylm, yhm, zlm, zhm;

+

+  if (_device == device_D)

+  {

+    /// D: X_L, X_H, Y_L, Y_H, Z_L, Z_H

+    xlm = Wire.read();

+    xhm = Wire.read();

+    ylm = Wire.read();

+    yhm = Wire.read();

+    zlm = Wire.read();

+    zhm = Wire.read();

+  }

+  else

+  {

+    // DLHC, DLM, DLH: X_H, X_L...

+    xhm = Wire.read();

+    xlm = Wire.read();

+

+    if (_device == device_DLH)

+    {

+      // DLH: ...Y_H, Y_L, Z_H, Z_L

+      yhm = Wire.read();

+      ylm = Wire.read();

+      zhm = Wire.read();

+      zlm = Wire.read();

+    }

+    else

+    {

+      // DLM, DLHC: ...Z_H, Z_L, Y_H, Y_L

+      zhm = Wire.read();

+      zlm = Wire.read();

+      yhm = Wire.read();

+      ylm = Wire.read();

+    }

+  }

+

+  // combine high and low bytes

+  m.x = (int16_t)(xhm << 8 | xlm);

+  m.y = (int16_t)(yhm << 8 | ylm);

+  m.z = (int16_t)(zhm << 8 | zlm);

+}

+

+// Reads all 6 channels of the LSM303 and stores them in the object variables

+void LSM303::read(void)

+{

+  readAcc();

+  readMag();

+}

+

+/*

+Returns the angular difference in the horizontal plane between a

+default vector and north, in degrees.

+

+The default vector here is chosen to point along the surface of the

+PCB, in the direction of the top of the text on the silkscreen.

+This is the +X axis on the Pololu LSM303D carrier and the -Y axis on

+the Pololu LSM303DLHC, LSM303DLM, and LSM303DLH carriers.

+*/

+float LSM303::heading(void)

+{

+  if (_device == device_D)

+  {

+    return heading((vector<int>){1, 0, 0});

+  }

+  else

+  {

+    return heading((vector<int>){0, -1, 0});

+  }

+}

+

+/*

+Returns the angular difference in the horizontal plane between the

+"from" vector and north, in degrees.

+

+Description of heading algorithm:

+Shift and scale the magnetic reading based on calibration data to find

+the North vector. Use the acceleration readings to determine the Up

+vector (gravity is measured as an upward acceleration). The cross

+product of North and Up vectors is East. The vectors East and North

+form a basis for the horizontal plane. The From vector is projected

+into the horizontal plane and the angle between the projected vector

+and horizontal north is returned.

+*/

+template <typename T> float LSM303::heading(vector<T> from)

+{

+    vector<int32_t> temp_m = {m.x, m.y, m.z};

+

+    // subtract offset (average of min and max) from magnetometer readings

+    temp_m.x -= ((int32_t)m_min.x + m_max.x) / 2;

+    temp_m.y -= ((int32_t)m_min.y + m_max.y) / 2;

+    temp_m.z -= ((int32_t)m_min.z + m_max.z) / 2;

+

+    // compute E and N

+    vector<float> E;

+    vector<float> N;

+    vector_cross(&temp_m, &a, &E);

+    vector_normalize(&E);

+    vector_cross(&a, &E, &N);

+    vector_normalize(&N);

+

+    // compute heading

+    float heading = atan2(vector_dot(&E, &from), vector_dot(&N, &from)) * 180 / M_PI;

+    if (heading < 0) heading += 360;

+    return heading;

+}

+

+template <typename Ta, typename Tb, typename To> void LSM303::vector_cross(const vector<Ta> *a,const vector<Tb> *b, vector<To> *out)

+{

+  out->x = (a->y * b->z) - (a->z * b->y);

+  out->y = (a->z * b->x) - (a->x * b->z);

+  out->z = (a->x * b->y) - (a->y * b->x);

+}

+

+template <typename Ta, typename Tb> float LSM303::vector_dot(const vector<Ta> *a, const vector<Tb> *b)

+{

+  return (a->x * b->x) + (a->y * b->y) + (a->z * b->z);

+}

+

+void LSM303::vector_normalize(vector<float> *a)

+{

+  float mag = sqrt(vector_dot(a, a));

+  a->x /= mag;

+  a->y /= mag;

+  a->z /= mag;

+}

+

+// Private Methods //////////////////////////////////////////////////////////////

+

+int LSM303::testReg(byte address, regAddr reg)

+{

+  Wire.beginTransmission(address);

+  Wire.write((byte)reg);

+  last_status = Wire.endTransmission();

+

+  Wire.requestFrom(address, (byte)1);

+  if (Wire.available())

+    return Wire.read();

+  else

+    return TEST_REG_NACK;

+}

--- /dev/null
+++ b/ParamotorFC/libraries/LSM303/LSM303.h
@@ -1,1 +1,219 @@
+#ifndef LSM303_h

+#define LSM303_h

+

+#include <Arduino.h> // for byte data type

+

+class LSM303

+{

+  public:

+    template <typename T> struct vector

+    {

+      T x, y, z;

+    };

+

+    enum deviceType { device_DLH, device_DLM, device_DLHC, device_D, device_auto };

+    enum sa0State { sa0_low, sa0_high, sa0_auto };

+

+    // register addresses

+    enum regAddr

+    {

+      TEMP_OUT_L        = 0x05, // D

+      TEMP_OUT_H        = 0x06, // D

+

+      STATUS_M          = 0x07, // D

+

+      INT_CTRL_M        = 0x12, // D

+      INT_SRC_M         = 0x13, // D

+      INT_THS_L_M       = 0x14, // D

+      INT_THS_H_M       = 0x15, // D

+

+      OFFSET_X_L_M      = 0x16, // D

+      OFFSET_X_H_M      = 0x17, // D

+      OFFSET_Y_L_M      = 0x18, // D

+      OFFSET_Y_H_M      = 0x19, // D

+      OFFSET_Z_L_M      = 0x1A, // D

+      OFFSET_Z_H_M      = 0x1B, // D

+      REFERENCE_X       = 0x1C, // D

+      REFERENCE_Y       = 0x1D, // D

+      REFERENCE_Z       = 0x1E, // D

+

+      CTRL0             = 0x1F, // D

+      CTRL1             = 0x20, // D

+      CTRL_REG1_A       = 0x20, // DLH, DLM, DLHC

+      CTRL2             = 0x21, // D

+      CTRL_REG2_A       = 0x21, // DLH, DLM, DLHC

+      CTRL3             = 0x22, // D

+      CTRL_REG3_A       = 0x22, // DLH, DLM, DLHC

+      CTRL4             = 0x23, // D

+      CTRL_REG4_A       = 0x23, // DLH, DLM, DLHC

+      CTRL5             = 0x24, // D

+      CTRL_REG5_A       = 0x24, // DLH, DLM, DLHC

+      CTRL6             = 0x25, // D

+      CTRL_REG6_A       = 0x25, // DLHC

+      HP_FILTER_RESET_A = 0x25, // DLH, DLM

+      CTRL7             = 0x26, // D

+      REFERENCE_A       = 0x26, // DLH, DLM, DLHC

+      STATUS_A          = 0x27, // D

+      STATUS_REG_A      = 0x27, // DLH, DLM, DLHC

+

+      OUT_X_L_A         = 0x28,

+      OUT_X_H_A         = 0x29,

+      OUT_Y_L_A         = 0x2A,

+      OUT_Y_H_A         = 0x2B,

+      OUT_Z_L_A         = 0x2C,

+      OUT_Z_H_A         = 0x2D,

+

+      FIFO_CTRL         = 0x2E, // D

+      FIFO_CTRL_REG_A   = 0x2E, // DLHC

+      FIFO_SRC          = 0x2F, // D

+      FIFO_SRC_REG_A    = 0x2F, // DLHC

+

+      IG_CFG1           = 0x30, // D

+      INT1_CFG_A        = 0x30, // DLH, DLM, DLHC

+      IG_SRC1           = 0x31, // D

+      INT1_SRC_A        = 0x31, // DLH, DLM, DLHC

+      IG_THS1           = 0x32, // D

+      INT1_THS_A        = 0x32, // DLH, DLM, DLHC

+      IG_DUR1           = 0x33, // D

+      INT1_DURATION_A   = 0x33, // DLH, DLM, DLHC

+      IG_CFG2           = 0x34, // D

+      INT2_CFG_A        = 0x34, // DLH, DLM, DLHC

+      IG_SRC2           = 0x35, // D

+      INT2_SRC_A        = 0x35, // DLH, DLM, DLHC

+      IG_THS2           = 0x36, // D

+      INT2_THS_A        = 0x36, // DLH, DLM, DLHC

+      IG_DUR2           = 0x37, // D

+      INT2_DURATION_A   = 0x37, // DLH, DLM, DLHC

+

+      CLICK_CFG         = 0x38, // D

+      CLICK_CFG_A       = 0x38, // DLHC

+      CLICK_SRC         = 0x39, // D

+      CLICK_SRC_A       = 0x39, // DLHC

+      CLICK_THS         = 0x3A, // D

+      CLICK_THS_A       = 0x3A, // DLHC

+      TIME_LIMIT        = 0x3B, // D

+      TIME_LIMIT_A      = 0x3B, // DLHC

+      TIME_LATENCY      = 0x3C, // D

+      TIME_LATENCY_A    = 0x3C, // DLHC

+      TIME_WINDOW       = 0x3D, // D

+      TIME_WINDOW_A     = 0x3D, // DLHC

+

+      Act_THS           = 0x3E, // D

+      Act_DUR           = 0x3F, // D

+

+      CRA_REG_M         = 0x00, // DLH, DLM, DLHC

+      CRB_REG_M         = 0x01, // DLH, DLM, DLHC

+      MR_REG_M          = 0x02, // DLH, DLM, DLHC

+

+      SR_REG_M          = 0x09, // DLH, DLM, DLHC

+      IRA_REG_M         = 0x0A, // DLH, DLM, DLHC

+      IRB_REG_M         = 0x0B, // DLH, DLM, DLHC

+      IRC_REG_M         = 0x0C, // DLH, DLM, DLHC

+

+      WHO_AM_I_M        = 0x0F, // DLM

+      WHO_AM_I          = 0x0F, // D

+

+      TEMP_OUT_H_M      = 0x31, // DLHC

+      TEMP_OUT_L_M      = 0x32, // DLHC

+

+

+      // dummy addresses for registers in different locations on different devices;

+      // the library translates these based on device type

+      // value with sign flipped is used as index into translated_regs array

+

+      OUT_X_H_M         = -1,

+      OUT_X_L_M         = -2,

+      OUT_Y_H_M         = -3,

+      OUT_Y_L_M         = -4,

+      OUT_Z_H_M         = -5,

+      OUT_Z_L_M         = -6,

+      // update dummy_reg_count if registers are added here!

+

+      // device-specific register addresses

+

+      DLH_OUT_X_H_M     = 0x03,

+      DLH_OUT_X_L_M     = 0x04,

+      DLH_OUT_Y_H_M     = 0x05,

+      DLH_OUT_Y_L_M     = 0x06,

+      DLH_OUT_Z_H_M     = 0x07,

+      DLH_OUT_Z_L_M     = 0x08,

+

+      DLM_OUT_X_H_M     = 0x03,

+      DLM_OUT_X_L_M     = 0x04,

+      DLM_OUT_Z_H_M     = 0x05,

+      DLM_OUT_Z_L_M     = 0x06,

+      DLM_OUT_Y_H_M     = 0x07,

+      DLM_OUT_Y_L_M     = 0x08,

+

+      DLHC_OUT_X_H_M    = 0x03,

+      DLHC_OUT_X_L_M    = 0x04,

+      DLHC_OUT_Z_H_M    = 0x05,

+      DLHC_OUT_Z_L_M    = 0x06,

+      DLHC_OUT_Y_H_M    = 0x07,

+      DLHC_OUT_Y_L_M    = 0x08,

+

+      D_OUT_X_L_M       = 0x08,

+      D_OUT_X_H_M       = 0x09,

+      D_OUT_Y_L_M       = 0x0A,

+      D_OUT_Y_H_M       = 0x0B,

+      D_OUT_Z_L_M       = 0x0C,

+      D_OUT_Z_H_M       = 0x0D

+    };

+

+    vector<int16_t> a; // accelerometer readings

+    vector<int16_t> m; // magnetometer readings

+    vector<int16_t> m_max; // maximum magnetometer values, used for calibration

+    vector<int16_t> m_min; // minimum magnetometer values, used for calibration

+

+    byte last_status; // status of last I2C transmission
+

+    LSM303(void);

+

+    bool init(deviceType device = device_auto, sa0State sa0 = sa0_auto);

+    byte getDeviceType(void) { return _device; }

+

+    void enableDefault(void);

+

+    void writeAccReg(regAddr reg, byte value);

+    byte readAccReg(regAddr reg);

+    void writeMagReg(regAddr reg, byte value);

+    byte readMagReg(regAddr reg);

+

+    void writeReg(regAddr reg, byte value);

+    byte readReg(regAddr reg);
+

+    void readAcc(void);

+    void readMag(void);

+    void read(void);

+

+    void setTimeout(unsigned int timeout);

+    unsigned int getTimeout(void);

+    bool timeoutOccurred(void);

+

+    float heading(void);

+    template <typename T> float heading(vector<T> from);

+

+    // vector functions

+    template <typename Ta, typename Tb, typename To> static void vector_cross(const vector<Ta> *a, const vector<Tb> *b, vector<To> *out);

+    template <typename Ta, typename Tb> static float vector_dot(const vector<Ta> *a,const vector<Tb> *b);

+    static void vector_normalize(vector<float> *a);

+

+  private:

+    deviceType _device; // chip type (DLH, DLM, or DLHC)

+    byte acc_address;

+    byte mag_address;

+

+    static const int dummy_reg_count = 6;

+    regAddr translated_regs[dummy_reg_count + 1]; // index 0 not used

+

+    unsigned int io_timeout;

+    bool did_timeout;

+

+    int testReg(byte address, regAddr reg);

+};

+

+#endif

+

+

+

 

--- /dev/null
+++ b/ParamotorFC/libraries/LSM303/examples/Calibrate/Calibrate.ino
@@ -1,1 +1,33 @@
+#include <Wire.h>
+#include <LSM303.h>
 
+LSM303 compass;
+LSM303::vector<int16_t> running_min = {32767, 32767, 32767}, running_max = {-32768, -32768, -32768};
+
+char report[80];
+
+void setup() {
+  Serial.begin(9600);
+  Wire.begin();
+  compass.init();
+  compass.enableDefault();
+}
+
+void loop() {  
+  compass.read();
+  
+  running_min.x = min(running_min.x, compass.m.x);
+  running_min.y = min(running_min.y, compass.m.y);
+  running_min.z = min(running_min.z, compass.m.z);
+
+  running_max.x = max(running_max.x, compass.m.x);
+  running_max.y = max(running_max.y, compass.m.y);
+  running_max.z = max(running_max.z, compass.m.z);
+  
+  snprintf(report, sizeof(report), "min: {%+6d, %+6d, %+6d}    max: {%+6d, %+6d, %+6d}",
+    running_min.x, running_min.y, running_min.z,
+    running_max.x, running_max.y, running_max.z);
+  Serial.println(report);
+  
+  delay(100);
+}

--- /dev/null
+++ b/ParamotorFC/libraries/LSM303/examples/Heading/Heading.ino
@@ -1,1 +1,46 @@
+#include <Wire.h>
+#include <LSM303.h>
 
+LSM303 compass;
+
+void setup() {
+  Serial.begin(9600);
+  Wire.begin();
+  compass.init();
+  compass.enableDefault();
+  
+  /*
+  Calibration values; the default values of +/-32767 for each axis
+  lead to an assumed magnetometer bias of 0. Use the Calibrate example
+  program to determine appropriate values for your particular unit.
+  */
+  compass.m_min = (LSM303::vector<int16_t>){-32767, -32767, -32767};
+  compass.m_max = (LSM303::vector<int16_t>){+32767, +32767, +32767};
+}
+
+void loop() {
+  compass.read();
+  
+  /*
+  When given no arguments, the heading() function returns the angular
+  difference in the horizontal plane between a default vector and
+  north, in degrees.
+  
+  The default vector is chosen by the library to point along the
+  surface of the PCB, in the direction of the top of the text on the
+  silkscreen. This is the +X axis on the Pololu LSM303D carrier and
+  the -Y axis on the Pololu LSM303DLHC, LSM303DLM, and LSM303DLH
+  carriers.
+  
+  To use a different vector as a reference, use the version of heading()
+  that takes a vector argument; for example, use
+  
+    compass.heading((LSM303::vector<int>){0, 0, 1});
+  
+  to use the +Z axis as a reference.
+  */
+  float heading = compass.heading();
+  

+  Serial.println(heading);
+  delay(100);
+}

--- /dev/null
+++ b/ParamotorFC/libraries/LSM303/examples/Serial/Serial.ino
@@ -1,1 +1,54 @@
+/*
+The sensor outputs provided by the library are the raw 16-bit values
+obtained by concatenating the 8-bit high and low accelerometer and
+magnetometer data registers. They can be converted to units of g and
+gauss using the conversion factors specified in the datasheet for your
+particular device and full scale setting (gain).
 
+Example: An LSM303D gives a magnetometer X axis reading of 1982 with
+its default full scale setting of +/- 4 gauss. The M_GN specification
+in the LSM303D datasheet (page 10) states a conversion factor of 0.160
+mgauss/LSB (least significant bit) at this FS setting, so the raw
+reading of -1982 corresponds to 1982 * 0.160 = 317.1 mgauss =
+0.3171 gauss.
+
+In the LSM303DLHC, LSM303DLM, and LSM303DLH, the acceleration data
+registers actually contain a left-aligned 12-bit number, so the lowest
+4 bits are always 0, and the values should be shifted right by 4 bits
+(divided by 16) to be consistent with the conversion factors specified
+in the datasheets.
+
+Example: An LSM303DLH gives an accelerometer Z axis reading of -16144
+with its default full scale setting of +/- 2 g. Dropping the lowest 4
+bits gives a 12-bit raw value of -1009. The LA_So specification in the
+LSM303DLH datasheet (page 11) states a conversion factor of 1 mg/digit
+at this FS setting, so the value of -1009 corresponds to -1009 * 1 =
+1009 mg = 1.009 g.
+*/
+
+#include <Wire.h>
+#include <LSM303.h>
+
+LSM303 compass;
+
+char report[80];
+
+void setup()
+{
+  Serial.begin(9600);
+  Wire.begin();
+  compass.init();
+  compass.enableDefault();
+}
+
+void loop()
+{
+  compass.read();
+
+  snprintf(report, sizeof(report), "A: %6d %6d %6d    M: %6d %6d %6d",
+    compass.a.x, compass.a.y, compass.a.z,
+    compass.m.x, compass.m.y, compass.m.z);
+  Serial.println(report);
+
+  delay(100);
+}

--- /dev/null
+++ b/ParamotorFC/libraries/LSM303/keywords.txt
@@ -1,1 +1,145 @@
+LSM303	KEYWORD1
 
+init	KEYWORD2
+getDeviceType	KEYWORD2
+enableDefault	KEYWORD2
+writeAccReg	KEYWORD2
+readAccReg	KEYWORD2
+writeMagReg	KEYWORD2
+readMagReg	KEYWORD2
+writeReg	KEYWORD2
+readReg	KEYWORD2
+readAcc	KEYWORD2
+readMag	KEYWORD2
+read	KEYWORD2
+setTimeout	KEYWORD2
+getTimeout	KEYWORD2
+timeoutOccurred	KEYWORD2
+heading	KEYWORD2
+vector_cross	KEYWORD2
+vector_dot	KEYWORD2
+vector_normalize	KEYWORD2
+
+device_DLH	LITERAL1
+device_DLM	LITERAL1
+device_DLHC	LITERAL1
+device_D	LITERAL1
+device_auto	LITERAL1
+sa0_low	LITERAL1
+sa0_high	LITERAL1
+sa0_auto	LITERAL1
+TEMP_OUT_L	LITERAL1
+TEMP_OUT_H	LITERAL1
+STATUS_M	LITERAL1
+INT_CTRL_M	LITERAL1
+INT_SRC_M	LITERAL1
+INT_THS_L_M	LITERAL1
+INT_THS_H_M	LITERAL1
+OFFSET_X_L_M	LITERAL1
+OFFSET_X_H_M	LITERAL1
+OFFSET_Y_L_M	LITERAL1
+OFFSET_Y_H_M	LITERAL1
+OFFSET_Z_L_M	LITERAL1
+OFFSET_Z_H_M	LITERAL1
+REFERENCE_X	LITERAL1
+REFERENCE_Y	LITERAL1
+REFERENCE_Z	LITERAL1
+CTRL0	LITERAL1
+CTRL1	LITERAL1
+CTRL_REG1_A	LITERAL1
+CTRL2	LITERAL1
+CTRL_REG2_A	LITERAL1
+CTRL3	LITERAL1
+CTRL_REG3_A	LITERAL1
+CTRL4	LITERAL1
+CTRL_REG4_A	LITERAL1
+CTRL5	LITERAL1
+CTRL_REG5_A	LITERAL1
+CTRL6	LITERAL1
+CTRL_REG6_A	LITERAL1
+HP_FILTER_RESET_A	LITERAL1
+CTRL7	LITERAL1
+REFERENCE_A	LITERAL1
+STATUS_A	LITERAL1
+STATUS_REG_A	LITERAL1
+OUT_X_L_A	LITERAL1
+OUT_X_H_A	LITERAL1
+OUT_Y_L_A	LITERAL1
+OUT_Y_H_A	LITERAL1
+OUT_Z_L_A	LITERAL1
+OUT_Z_H_A	LITERAL1
+FIFO_CTRL	LITERAL1
+FIFO_CTRL_REG_A	LITERAL1
+FIFO_SRC	LITERAL1
+FIFO_SRC_REG_A	LITERAL1
+IG_CFG1	LITERAL1
+INT1_CFG_A	LITERAL1
+IG_SRC1	LITERAL1
+INT1_SRC_A	LITERAL1
+IG_THS1	LITERAL1
+INT1_THS_A	LITERAL1
+IG_DUR1	LITERAL1
+INT1_DURATION_A	LITERAL1
+IG_CFG2	LITERAL1
+INT2_CFG_A	LITERAL1
+IG_SRC2	LITERAL1
+INT2_SRC_A	LITERAL1
+IG_THS2	LITERAL1
+INT2_THS_A	LITERAL1
+IG_DUR2	LITERAL1
+INT2_DURATION_A	LITERAL1
+CLICK_CFG	LITERAL1
+CLICK_CFG_A	LITERAL1
+CLICK_SRC	LITERAL1
+CLICK_SRC_A	LITERAL1
+CLICK_THS	LITERAL1
+CLICK_THS_A	LITERAL1
+TIME_LIMIT	LITERAL1
+TIME_LIMIT_A	LITERAL1
+TIME_LATENCY	LITERAL1
+TIME_LATENCY_A	LITERAL1
+TIME_WINDOW	LITERAL1
+TIME_WINDOW_A	LITERAL1
+Act_THS	LITERAL1
+Act_DUR	LITERAL1
+CRA_REG_M	LITERAL1
+CRB_REG_M	LITERAL1
+MR_REG_M	LITERAL1
+SR_REG_M	LITERAL1
+IRA_REG_M	LITERAL1
+IRB_REG_M	LITERAL1
+IRC_REG_M	LITERAL1
+WHO_AM_I_M	LITERAL1
+WHO_AM_I	LITERAL1
+TEMP_OUT_H_M	LITERAL1
+TEMP_OUT_L_M	LITERAL1
+OUT_X_H_M	LITERAL1
+OUT_X_L_M	LITERAL1
+OUT_Y_H_M	LITERAL1
+OUT_Y_L_M	LITERAL1
+OUT_Z_H_M	LITERAL1
+OUT_Z_L_M	LITERAL1
+DLH_OUT_X_H_M	LITERAL1
+DLH_OUT_X_L_M	LITERAL1
+DLH_OUT_Y_H_M	LITERAL1
+DLH_OUT_Y_L_M	LITERAL1
+DLH_OUT_Z_H_M	LITERAL1
+DLH_OUT_Z_L_M	LITERAL1
+DLM_OUT_X_H_M	LITERAL1
+DLM_OUT_X_L_M	LITERAL1
+DLM_OUT_Z_H_M	LITERAL1
+DLM_OUT_Z_L_M	LITERAL1
+DLM_OUT_Y_H_M	LITERAL1
+DLM_OUT_Y_L_M	LITERAL1
+DLHC_OUT_X_H_M	LITERAL1
+DLHC_OUT_X_L_M	LITERAL1
+DLHC_OUT_Z_H_M	LITERAL1
+DLHC_OUT_Z_L_M	LITERAL1
+DLHC_OUT_Y_H_M	LITERAL1
+DLHC_OUT_Y_L_M	LITERAL1
+D_OUT_X_L_M	LITERAL1
+D_OUT_X_H_M	LITERAL1
+D_OUT_Y_L_M	LITERAL1
+D_OUT_Y_H_M	LITERAL1
+D_OUT_Z_L_M	LITERAL1
+D_OUT_Z_H_M	LITERAL1

--- /dev/null
+++ b/ParamotorFC/matrix.ino
@@ -1,1 +1,24 @@
 
+/**************************************************/
+//Multiply two 3x3 matrixs. This function developed by Jordi can be easily adapted to multiple n*n matrix's. (Pero me da flojera!). 
+void Matrix_Multiply(float a[3][3], float b[3][3],float mat[3][3])
+{
+  float op[3]; 
+  for(int x=0; x<3; x++)
+  {
+    for(int y=0; y<3; y++)
+    {
+      for(int w=0; w<3; w++)
+      {
+       op[w]=a[x][w]*b[w][y];
+      } 
+      mat[x][y]=0;
+      mat[x][y]=op[0]+op[1]+op[2];
+      
+      float test=mat[x][y];
+    }
+  }
+}
+
+

+

comments