Revision 0:3312872854c4, committed 2019-03-04

Comitter:

altb

Date:

Mon Mar 04 11:03:51 2019 +0000

Child:

1:c680da75a614

Commit message:

New Lib Folder

Changed in this revision

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/DiffCounter.cpp	Mon Mar 04 11:03:51 2019 +0000
@@ -0,0 +1,40 @@
+/*  
+    DiffCounter Class, differentiate encoder counts for cuboid, applies LP filter and unwrapping
+    
+              b*(1 - z^-1)                      s
+      G(z) = -------------  <-- tustin --  ----------- = G(s)
+              1 - a*z^-1                      T*s + 1
+*/
+
+#include "DiffCounter.h"
+#define   pi 3.141592653589793
+using namespace std;
+
+DiffCounter::DiffCounter(float T, float Ts)
+{   
+    b = 2.0/(2.0*(double)T + (double)Ts);
+    a = -(2.0*(double)T - (double)Ts)/(2.0*(double)T + (double)Ts);
+    incPast = 0;
+    vel = 0.0;
+    inc2rad = 2.0*pi/(4.0*6400.0);   // incr encoder with 6400inc/rev
+}
+
+DiffCounter::~DiffCounter() {}
+
+void DiffCounter::reset(float initValue, short inc)
+{
+    vel = (double)initValue;
+    incPast = inc;
+}
+
+float DiffCounter::doStep(short inc)
+{
+    long del = (long)(inc - incPast);
+    incPast = inc;
+    if(del < -16000)
+        del += 0xFFFF;
+    if(del > 16000)
+        del -= 0xFFFF;
+    vel = b*(double)del*inc2rad - a*vel;
+    return (float)vel;
+}
\ No newline at end of file

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/DiffCounter.h	Mon Mar 04 11:03:51 2019 +0000
@@ -0,0 +1,29 @@
+#ifndef DIFFCOUNTER_H_
+#define DIFFCOUNTER_H_
+
+class DiffCounter
+{
+public:
+
+    DiffCounter(float T, float Ts);
+    
+    float operator()(short inc) {
+        return doStep(inc);
+    }
+    
+    virtual     ~DiffCounter();
+    
+    void        reset(float initValue, short inc);
+    float       doStep(short inc);
+
+private:
+
+    double b;
+    double a;
+    short incPast;
+    double vel;
+    double inc2rad;
+
+};
+
+#endif /* DIFFCOUNTER_H_ */
\ No newline at end of file

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/EncoderCounter.cpp	Mon Mar 04 11:03:51 2019 +0000
@@ -0,0 +1,139 @@
+/*
+ * EncoderCounter.cpp
+ * Copyright (c) 2017, ZHAW
+ * All rights reserved.
+ */
+
+#include "EncoderCounter.h"
+
+using namespace std;
+
+/**
+ * Creates and initializes the driver to read the quadrature
+ * encoder counter of the STM32 microcontroller.
+ * @param a the input pin for the channel A.
+ * @param b the input pin for the channel B.
+ */
+EncoderCounter::EncoderCounter(PinName a, PinName b) {
+    
+    // check pins
+    
+    if ((a == PA_6) && (b == PC_7)) {
+        
+        // pinmap OK for TIM3 CH1 and CH2
+        
+        TIM = TIM3;
+        
+        // configure reset and clock control registers
+        
+        RCC->AHB1ENR |= RCC_AHB1ENR_GPIOCEN;    // manually enable port C (port A enabled by mbed library)
+        
+        // configure general purpose I/O registers
+        
+        GPIOA->MODER &= ~GPIO_MODER_MODER6;     // reset port A6
+        GPIOA->MODER |= GPIO_MODER_MODER6_1;    // set alternate mode of port A6
+        GPIOA->PUPDR &= ~GPIO_PUPDR_PUPDR6;     // reset pull-up/pull-down on port A6
+        GPIOA->PUPDR |= GPIO_PUPDR_PUPDR6_1;    // set input as pull-down
+        GPIOA->AFR[0] &= ~(0xF << 4*6);         // reset alternate function of port A6
+        GPIOA->AFR[0] |= 2 << 4*6;              // set alternate funtion 2 of port A6
+        
+        GPIOC->MODER &= ~GPIO_MODER_MODER7;     // reset port C7
+        GPIOC->MODER |= GPIO_MODER_MODER7_1;    // set alternate mode of port C7
+        GPIOC->PUPDR &= ~GPIO_PUPDR_PUPDR7;     // reset pull-up/pull-down on port C7
+        GPIOC->PUPDR |= GPIO_PUPDR_PUPDR7_1;    // set input as pull-down
+        GPIOC->AFR[0] &= ~0xF0000000;           // reset alternate function of port C7
+        GPIOC->AFR[0] |= 2 << 4*7;              // set alternate funtion 2 of port C7
+        
+        // configure reset and clock control registers
+        
+        RCC->APB1RSTR |= RCC_APB1RSTR_TIM3RST;  //reset TIM3 controller
+        RCC->APB1RSTR &= ~RCC_APB1RSTR_TIM3RST;
+        
+        RCC->APB1ENR |= RCC_APB1ENR_TIM3EN;     // TIM3 clock enable
+        
+    } else if ((a == PB_6) && (b == PB_7)) {
+        
+        // pinmap OK for TIM4 CH1 and CH2
+        
+        TIM = TIM4;
+        
+        // configure reset and clock control registers
+        
+        RCC->AHB1ENR |= RCC_AHB1ENR_GPIOBEN;    // manually enable port B (port A enabled by mbed library)
+        
+        // configure general purpose I/O registers
+        
+        GPIOB->MODER &= ~GPIO_MODER_MODER6;     // reset port B6
+        GPIOB->MODER |= GPIO_MODER_MODER6_1;    // set alternate mode of port B6
+        GPIOB->PUPDR &= ~GPIO_PUPDR_PUPDR6;     // reset pull-up/pull-down on port B6
+        GPIOB->PUPDR |= GPIO_PUPDR_PUPDR6_1;    // set input as pull-down
+        GPIOB->AFR[0] &= ~(0xF << 4*6);         // reset alternate function of port B6
+        GPIOB->AFR[0] |= 2 << 4*6;              // set alternate funtion 2 of port B6
+        
+        GPIOB->MODER &= ~GPIO_MODER_MODER7;     // reset port B7
+        GPIOB->MODER |= GPIO_MODER_MODER7_1;    // set alternate mode of port B7
+        GPIOB->PUPDR &= ~GPIO_PUPDR_PUPDR7;     // reset pull-up/pull-down on port B7
+        GPIOB->PUPDR |= GPIO_PUPDR_PUPDR7_1;    // set input as pull-down
+        GPIOB->AFR[0] &= ~0xF0000000;           // reset alternate function of port B7
+        GPIOB->AFR[0] |= 2 << 4*7;              // set alternate funtion 2 of port B7
+        
+        // configure reset and clock control registers
+        
+        RCC->APB1RSTR |= RCC_APB1RSTR_TIM4RST;  //reset TIM4 controller
+        RCC->APB1RSTR &= ~RCC_APB1RSTR_TIM4RST;
+        
+        RCC->APB1ENR |= RCC_APB1ENR_TIM4EN;     // TIM4 clock enable
+        
+    } else {
+        
+        printf("pinmap not found for peripheral\n");
+    }
+    
+    // configure general purpose timer 3 or 4
+    
+    TIM->CR1 = 0x0000;          // counter disable
+    TIM->CR2 = 0x0000;          // reset master mode selection
+    TIM->SMCR = TIM_SMCR_SMS_1 | TIM_SMCR_SMS_0; // counting on both TI1 & TI2 edges
+    TIM->CCMR1 = TIM_CCMR1_CC2S_0 | TIM_CCMR1_CC1S_0;
+    TIM->CCMR2 = 0x0000;        // reset capture mode register 2
+    TIM->CCER = TIM_CCER_CC2E | TIM_CCER_CC1E;
+    TIM->CNT = 0x0000;          // reset counter value
+    TIM->ARR = 0xFFFF;          // auto reload register
+    TIM->CR1 = TIM_CR1_CEN;     // counter enable
+}
+
+EncoderCounter::~EncoderCounter() {}
+
+/**
+ * Resets the counter value to zero.
+ */
+void EncoderCounter::reset() {
+    
+    TIM->CNT = 0x0000;
+}
+
+/**
+ * Resets the counter value to a given offset value.
+ * @param offset the offset value to reset the counter to.
+ */
+void EncoderCounter::reset(short offset) {
+    
+    TIM->CNT = -offset;
+}
+
+/**
+ * Reads the quadrature encoder counter value.
+ * @return the quadrature encoder counter as a signed 16-bit integer value.
+ */
+short EncoderCounter::read() {
+    
+    return (short)(-TIM->CNT);
+}
+
+/**
+ * The empty operator is a shorthand notation of the <code>read()</code> method.
+ */
+EncoderCounter::operator short() {
+    
+    return read();
+}

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/EncoderCounter.h	Mon Mar 04 11:03:51 2019 +0000
@@ -0,0 +1,33 @@
+/*
+ * EncoderCounter.h
+ * Copyright (c) 2017, ZHAW
+ * All rights reserved.
+ */
+
+#ifndef ENCODER_COUNTER_H_
+#define ENCODER_COUNTER_H_
+
+#include <cstdlib>
+#include <mbed.h>
+
+/**
+ * This class implements a driver to read the quadrature
+ * encoder counter of the STM32 microcontroller.
+ */
+class EncoderCounter {
+    
+    public:
+        
+                    EncoderCounter(PinName a, PinName b);
+        virtual     ~EncoderCounter();
+        void        reset();
+        void        reset(short offset);
+        short       read();
+                    operator short();
+        
+    private:
+        
+        TIM_TypeDef*    TIM;
+};
+
+#endif /* ENCODER_COUNTER_H_ */

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/GPS.cpp	Mon Mar 04 11:03:51 2019 +0000
@@ -0,0 +1,95 @@
+#include "GPS.h"
+#include "math.h"
+#include "string"
+#include "mbed.h"
+
+using namespace std;
+
+GPS::GPS(PinName P1,PinName P2,float TS) : logGPS(P1, P2),thread(osPriorityLow, 4096) {  
+    
+    // start thread and timer interrupt
+    logGPS.baud(9600);
+    logGPS.attach(callback(this, &GPS::Rx_interrupt), Serial::RxIrq);
+    thread.start(callback(this, &GPS::get_data));
+    ticker.attach(callback(this, &GPS::sendSignal), TS);
+    buffer_filled = false;
+    rx_in = 0;
+}
+
+GPS::~GPS() {}
+
+void GPS::get_data(void){
+    float dum;
+    string str2;
+    while(1) {
+        thread.signal_wait(signal);
+        //string str="blabla $GNGGA,124720.00,4729.84871,N,00843.83588,E,1,06,2.77,406.1,M,47.4,M,,*4B";
+        str=string(buf);
+        size_t found=str.find("GNGGA");
+        if(found!=string::npos){
+            int cou=1;
+            str.replace(0,found+6, "\0");
+            do{
+                int en=str.find(",");
+                str2=str.substr(0,en);
+                switch(cou){
+                    case 2:{
+                        float north = atof(str2.c_str());
+                        dum=floor(north/100.0);
+                        north=dum+((north/100.0)-dum)/0.6;
+                        pos0_xyz[0]=north;
+                        break;
+                        }
+                    case 4:{
+                        float ew = atof(str2.c_str());
+                        dum=floor(ew/100.0);
+                        ew=dum+((ew/100.0)-dum)/0.6;
+                        pos0_xyz[1]=ew;
+                        break;
+                        }
+                    case 6:{
+                        int isvalid = atof(str2.c_str());
+                        break;
+                        }
+                    case 7:{
+                        int numSat = atof(str2.c_str());
+                        break;
+                        }
+                    case 9:{
+                        float alt = atof(str2.c_str());
+                        pos0_xyz[2]=alt;
+                        break;
+                        }
+                    }
+                str.replace(0,en+1,"\0");
+                cou++;
+                }while(cou<10 && str.length()>2);
+             }// if found
+        }//while(1)       
+    }
+void GPS::sendSignal() {
+    
+    thread.signal_set(signal);
+}
+
+void GPS::get_position(void){
+    pos_xyz[0]=pos0_xyz[0];
+    pos_xyz[1]=pos0_xyz[1];
+    pos_xyz[2]=pos0_xyz[2];
+    }
+
+
+
+void GPS::Rx_interrupt() {
+    while ((logGPS.readable()) ){
+        buf[rx_in++] = logGPS.getc(); 
+        if(rx_in==255)
+            buffer_filled = true;
+    }
+}
+
+void GPS::return_string(string *st){
+    *st = string(buf);
+    return;    
+    
+    }
\ No newline at end of file

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/GPS.h	Mon Mar 04 11:03:51 2019 +0000
@@ -0,0 +1,32 @@
+#include "mbed.h"
+#include "Signal.h"
+#define buffer_size 255
+#include "string"
+#include "PID_Cntrl.h"
+#define EARTH_RADIUS 6378137
+
+class GPS{
+public:
+    GPS(PinName,PinName,float);
+    virtual ~GPS();
+    void get_data(void);
+    double pos_xyz[3];
+    void get_position(void);
+    RawSerial logGPS;
+    void return_string(string *);
+private:
+    Signal signal;
+    Thread thread;
+    Ticker ticker;
+    Mutex mutex;      // mutex to lock critical sections
+    double pos0_xyz[3];
+    uint8_t rx_in;
+    char c;
+    bool buffer_filled;
+    void sendSignal();
+    void Rx_interrupt();
+    char buf[buffer_size];
+    string str;
+    double ph_th0[2];
+        
+};

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/LinearCharacteristics.cpp	Mon Mar 04 11:03:51 2019 +0000
@@ -0,0 +1,49 @@
+#include "LinearCharacteristics.h"
+
+using namespace std;
+
+LinearCharacteristics::LinearCharacteristics(float gain,float offset){    // standard lin characteristics
+    this->gain = gain;
+    this->offset = offset;
+}
+
+LinearCharacteristics::LinearCharacteristics(float xmin,float xmax, float ymin, float ymax){    // standard lin characteristics
+    this->gain = (ymax - ymin)/(xmax - xmin);
+    this->offset = xmax - ymax/this->gain;
+    this->ulim = 999999.0;
+    this->llim = -999999.0;
+    
+}
+LinearCharacteristics::LinearCharacteristics(float xmin,float xmax, float ymin, float ymax,float ll, float ul){    // standard lin characteristics
+    this->gain = (ymax - ymin)/(xmax - xmin);
+    this->offset = xmax - ymax/this->gain;
+    this->llim = ll;
+    this->ulim = ul;
+    
+}
+
+LinearCharacteristics::~LinearCharacteristics() {}
+
+
+float LinearCharacteristics::evaluate(float x)
+{   
+float dum = this->gain*(x - this->offset);
+if(dum > this->ulim)
+    dum = this->ulim;
+if(dum < this->llim)
+    dum = this->llim;
+return dum;
+    }
+
+void LinearCharacteristics::setup(float xmin,float xmax, float ymin, float ymax){    // standard lin characteristics
+    this->gain = (ymax - ymin)/(xmax - xmin);
+    this->offset = xmax - ymax/this->gain;
+    this->ulim = 999999.0;
+    this->llim = -999999.0;   
+}
+void LinearCharacteristics::setup(float xmin,float xmax, float ymin, float ymax,float ll, float ul){    // standard lin characteristics
+    this->gain = (ymax - ymin)/(xmax - xmin);
+    this->offset = xmax - ymax/this->gain;
+    this->llim = ll;
+    this->ulim = ul; 
+}

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/LinearCharacteristics.h	Mon Mar 04 11:03:51 2019 +0000
@@ -0,0 +1,33 @@
+// Linear Characteristics for different purposes (map Voltage to acc etc.)
+
+
+#ifndef LINEAR_CHARACTERISTICS_H_
+#define LINEAR_CHARACTERISTICS_H_   
+
+
+class LinearCharacteristics{
+     public:
+            LinearCharacteristics(){};
+            LinearCharacteristics(float, float);
+            LinearCharacteristics(float, float, float, float);
+            LinearCharacteristics(float, float, float, float, float, float);
+            float evaluate(float);
+            void setup(float, float, float, float);
+            void setup(float, float, float, float, float, float);
+            float operator()(float x){
+                return evaluate(x);
+                } 
+                //...
+                virtual     ~LinearCharacteristics();
+                // here: the calculation function
+    
+    private:
+        // here: private functions and values...
+        float gain;
+        float offset;
+        float ulim;
+        float llim;
+};
+
+
+#endif      // LINEAR_CHARACTERISTICS_H_
\ No newline at end of file

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/PolynomialCharacteristics.cpp	Mon Mar 04 11:03:51 2019 +0000
@@ -0,0 +1,27 @@
+#include "PolynomialCharacteristics.h"
+#include "math.h"
+using namespace std;
+PolynomialCharacteristics::PolynomialCharacteristics(float *P,uint8_t degree,float ll, float ul){    // standard lin characteristics
+    this->P = (float*)malloc(degree*sizeof(float));
+    for(int k=0;k<=degree;k++)
+        this->P[k] =P[k];
+    this->degree = degree;
+    this->llim = ll;
+    this->ulim = ul;
+    
+}
+
+PolynomialCharacteristics::~PolynomialCharacteristics() {}
+
+
+float PolynomialCharacteristics::evaluate(float x)
+{   
+float dum = 0.0;
+for(int k=0;k<=degree;k++)
+    dum = P[k]*pow(x,(float)k);
+if(dum > this->ulim)
+    dum = this->ulim;
+if(dum < this->llim)
+    dum = this->llim;
+return dum;
+    }

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/PolynomialCharacteristics.h	Mon Mar 04 11:03:51 2019 +0000
@@ -0,0 +1,28 @@
+// Polynomial Characteristics for different purposes (map Voltage to acc etc.)
+
+
+#ifndef POLYNOMIALCHARACTERISTICS_H_
+#define POLYNOMIALCHARACTERISTICS_H_
+#include "mbed.h"
+
+class PolynomialCharacteristics{
+     public:
+            PolynomialCharacteristics(float *, uint8_t, float, float);
+            float evaluate(float);
+            float operator()(float x){
+                return evaluate(x);
+                } 
+                //...
+                virtual     ~PolynomialCharacteristics();
+                // here: the calculation function
+    
+    private:
+        // here: private functions and values...
+        float *P;
+        float degree;
+        float ulim;
+        float llim;
+};
+
+
+#endif      // LINEAR_CHARACTERISTICS_H_
\ No newline at end of file

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/RCin.cpp	Mon Mar 04 11:03:51 2019 +0000
@@ -0,0 +1,76 @@
+#include "RCin.h"
+
+
+using namespace std;
+
+RCin::RCin(PinName pin) : pwm1(pin){
+    pwm1.fall(callback(this, &RCin::fall_edge));
+    pwm1.rise(callback(this, &RCin::rise_edge));
+    local_ti.start();
+    local_ti.reset();
+    }
+
+// *****************************************************************************
+void RCin::map_Channels(void){
+    CH1_2_PM1.setup(600.0,1400.0,-1.0,1.0);
+    CH2_2_PM1.setup(1400.0,600.0,-1.0,1.0);
+    CH3_2_PM1.setup(600.0,1400.0,-1.0,1.0);
+    CH4_2_PM1.setup(600.0,1400.0,-1.0,1.0);
+    }
+
+// *****************************************************************************
+void RCin::rise_edge(void)
+    {
+     local_ti.reset();
+     }
+
+// *****************************************************************************
+void RCin::fall_edge(void)
+   {
+        uint32_t time_us = local_ti.read_us();
+        if(time_us > 2500){
+            if(cou<200)
+                cou++;
+            chnr = 0;
+            map_pwm_2_PM1();
+        }
+        else
+        {
+            pwms[++chnr] = time_us;
+            test_pwms[cou][chnr-1]=time_us;
+        }
+    }
+// *****************************************************************************
+uint8_t RCin::get_flightmode(void){
+    if(isAlive){
+        flightmode_changed = (old_flightmode != current_flightmode);
+        old_flightmode = current_flightmode;
+        return current_flightmode;
+        }
+    else 
+        return 0;
+    }
+// *****************************************************************************
+void RCin::map_pwm_2_PM1(void){
+    PM1[1]=CH1_2_PM1((float)pwms[1]);
+    PM1[2]=CH2_2_PM1((float)pwms[2]);
+    PM1[3]=CH3_2_PM1((float)pwms[3]);
+    PM1[4]=CH4_2_PM1((float)pwms[4]);
+    if(pwms[5] >620 && pwms[5] < 720)
+        current_flightmode = LOITER;
+    else if(pwms[5] >720 && pwms[5] < 850)
+        current_flightmode = AUTO;
+    else if(pwms[5] >870 && pwms[5] < 970)
+        current_flightmode = RTL;
+    else if(pwms[5] >1000 && pwms[5] < 1100)
+        current_flightmode = STABILIZED;
+    else if(pwms[5] >1100 && pwms[5] < 1200)
+        current_flightmode = ACRO;
+    else if(pwms[5] >1200 && pwms[5] < 1350)
+        current_flightmode = LAND;
+    else
+        current_flightmode = STABILIZED;
+    if(pwms[3]>550)
+        isAlive = true;
+
+    }
\ No newline at end of file

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/RCin.h	Mon Mar 04 11:03:51 2019 +0000
@@ -0,0 +1,34 @@
+#include "LinearCharacteristics.h"
+#include "mbed.h"
+#include "define_constants.h"
+
+class RCin {
+public:
+
+RCin(PinName pin);
+void fall_edge(void);
+void rise_edge(void);
+uint8_t get_flightmode(void);
+bool flightmode_changed;
+
+float PM1[15];
+LinearCharacteristics CH1_2_PM1;
+LinearCharacteristics CH2_2_PM1;
+LinearCharacteristics CH3_2_PM1;
+LinearCharacteristics CH4_2_PM1;
+
+void map_Channels(void);
+uint16_t pwms[255];
+uint16_t test_pwms[200][12];
+uint16_t cou;
+bool isAlive;
+    
+private:
+    PinName pin;
+    InterruptIn pwm1;
+    Timer local_ti;
+    uint8_t chnr;
+    uint8_t old_flightmode;
+    void map_pwm_2_PM1(void);
+    uint8_t current_flightmode;
+};

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Signal.cpp	Mon Mar 04 11:03:51 2019 +0000
@@ -0,0 +1,54 @@
+/*
+ * Signal.cpp
+ * Copyright (c) 2017, ZHAW
+ * All rights reserved.
+ */
+
+#include "Signal.h"
+
+using namespace std;
+
+int32_t Signal::signals = 0;
+
+/**
+ * Creates a signal object and assignes a unique flag.
+ */
+Signal::Signal() {
+    
+    mutex.lock();
+    
+    int32_t n = 0;
+    while ((((1 << n) & signals) > 0) && (n < 30)) n++;
+    signal = (1 << n);
+    
+    mutex.unlock();
+}
+
+/**
+ * Deletes the signal object and releases the assigned flag.
+ */
+Signal::~Signal() {
+    
+    mutex.lock();
+    
+    signals &= ~signal;
+    
+    mutex.unlock();
+}
+
+/**
+ * Gets the assigned signal flag.
+ */
+int32_t Signal::read() {
+    
+    return signal;
+}
+
+/**
+ * The empty operator is a shorthand notation of the <code>read()</code> method.
+ */
+Signal::operator int32_t() {
+    
+    return read();
+}
+

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Signal.h	Mon Mar 04 11:03:51 2019 +0000
@@ -0,0 +1,34 @@
+/*
+ * Signal.h
+ * Copyright (c) 2017, ZHAW
+ * All rights reserved.
+ */
+
+#ifndef SIGNAL_H_
+#define SIGNAL_H_
+
+#include <cstdlib>
+#include <stdint.h>
+#include <mbed.h>
+
+/**
+ * This class manages the handling of unique signal flags to trigger rtos threads.
+ */
+class Signal {
+    
+    public:
+        
+                        Signal();
+        virtual         ~Signal();
+        virtual int32_t read();
+                        operator int32_t();
+        
+    private:
+        
+        static int32_t  signals;    // variable that holds all assigned signal flags
+        int32_t         signal;     // signal flag of this object
+        Mutex           mutex;      // mutex to lock critical sections
+};
+
+#endif /* SIGNAL_H_ */
+

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Unwrapper.cpp	Mon Mar 04 11:03:51 2019 +0000
@@ -0,0 +1,30 @@
+/*  
+*/
+
+#include "Unwrapper.h"
+#define   pi 3.141592653589793
+using namespace std;
+
+Unwrapper::Unwrapper(double i2r)
+{   
+    inc2rad = i2r;
+    last_value = 0;
+}
+
+Unwrapper::~Unwrapper() {}
+
+void Unwrapper::reset(void)
+{
+    last_value = 0;
+}
+
+double Unwrapper::doStep(short inc)
+{
+    long temp = inc;
+    if((temp - last_value) > 32000)
+        temp -= 0xFFFF;
+    else if((temp - last_value) < -32000)
+        temp += 0xFFFF;
+    last_value = temp;
+    return (temp*inc2rad);
+}
\ No newline at end of file

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Unwrapper.h	Mon Mar 04 11:03:51 2019 +0000
@@ -0,0 +1,26 @@
+/*  
+*/
+
+using namespace std;
+
+class Unwrapper
+{
+public:
+
+    Unwrapper(double);
+    
+    double operator()(short inc) {
+        return doStep(inc);
+    }
+    
+    virtual     ~Unwrapper();
+    
+    void        reset(void);
+    double       doStep(short inc);
+
+private:
+
+    long last_value;
+    double inc2rad;
+
+};
\ No newline at end of file

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Unwrapper_2pi.cpp	Mon Mar 04 11:03:51 2019 +0000
@@ -0,0 +1,35 @@
+/*  
+*/
+
+#include "Unwrapper_2pi.h"
+#define   pi 3.141592653589793
+using namespace std;
+
+Unwrapper_2pi::Unwrapper_2pi(void)
+{   
+    last_value = 0.0;
+    turns = 0;
+}
+
+Unwrapper_2pi::~Unwrapper_2pi() {}
+
+void Unwrapper_2pi::reset(void)
+{
+    last_value = 0.0;
+    turns = 0;
+}
+
+float Unwrapper_2pi::doStep(float in)
+{
+    float temp = in + 2*pi*(float)turns;
+    if((temp - last_value) > pi){
+        temp -= 2*pi;
+        turns--;
+        }
+    else if((temp - last_value) < -pi){
+        temp += 2*pi;
+        turns++;
+        }
+    last_value = temp;
+    return (temp);
+}
\ No newline at end of file

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Unwrapper_2pi.h	Mon Mar 04 11:03:51 2019 +0000
@@ -0,0 +1,26 @@
+/*  
+*/
+
+using namespace std;
+
+class Unwrapper_2pi
+{
+public:
+
+    Unwrapper_2pi(void);
+    
+    float operator()(float in) {
+        return doStep(in);
+    }
+    
+    virtual     ~Unwrapper_2pi();
+    
+    void        reset(void);
+    float       doStep(float inc);
+
+private:
+
+    long turns;
+    float last_value;
+
+};
\ No newline at end of file

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/define_constants.h	Mon Mar 04 11:03:51 2019 +0000
@@ -0,0 +1,32 @@
+// -------- States -------
+#define INIT 0
+#define  INIT_MOTORS 5
+#define WAIT_ARMING 10
+#define ARMING 20
+#define START_MOTORS 30
+#define FLIGHT 40
+#define  MOTOR_STOPPED 50
+
+// -------- Motor Types -------
+#define AIRGEAR350 1              // small TIgerMotor 4er set
+
+// --------- UAV Types --------
+#define QUAD1 1     // Ruprechts 1kg quad Plus
+#define X 1         // 
+#define PLUS 2         // 
+
+
+// --------- Flight Modes  --------
+#define STABILIZED 1
+#define ACRO 2
+#define LAND 3
+#define LOITER 4
+#define AUTO 5
+#define RTL 6
+
+// --------- Assumed UAV States  --------
+#define ONGROUND 1
+#define LIFTOFF 2
+#define FLYING 3
+#define CRASHED 4
+#define LANDING 5