File content as of revision 1:b4a0d63db637:

//////////////////////////////////////////////////////////////////////////////////////
//                                                                                  //
//      File        : main.cpp                                                      //
//      Version     : 0.1                                                           //
//      Date        : 25 march 2015                                                 //
//      Author      : Dany Brugman                                                  //
//      Comment     : Function to write data to a 2x16 LCD by I2C                   //
//                    using a MCP23017 port expander.                               //
//                                                                                  //
//      Changelog   :                                                               //
//      Date:           Name:       Comment:                                        //
//      25/03/2015      DNB         First version                                   //
//                                                                                  //
//////////////////////////////////////////////////////////////////////////////////////

//////////////////////////////////////////////////////////////////////////////////////
// includes                                                                         //
//////////////////////////////////////////////////////////////////////////////////////

#include "mbed.h"
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include "MCP23017.h"
#include "LCD_I2C.h"
#include "uart.h"
#include "GPS.h"
#include "MPU9150.h"
#include "Quaternion.h"
#include "PID.h"
#include "Servo.h"
#include "mRotaryEncoder.h"
#include "MainModule.h"
//#include "LCD_Menu.h"
#include "menu.h"
#include "systemVar.h"

//////////////////////////////////////////////////////////////////////////////////////
// defines                                                                          //
//////////////////////////////////////////////////////////////////////////////////////

#define CLEAR "\033[2J"
#define PROJECT "\033[1;10fSolar on Foils\n"
#define VERSION "\033[2;10fVersion 1.0\n"

#define EXT_UI_ID               20
#define ACTUATOR_PORT_ID        101
#define ACTUATOR_STARB_ID       102
#define ACTUATOR_PORT_RUN       103
#define ACTUATOR_STARB_RUN      104
#define HEIGHT_PORT_ID          201
#define HEIGHT_STARB_ID         202
#define HEIGHT_PORT_DATA        203
#define HEIGHT_STARB_DATA       204
#define GYRO_GPS_ID             205
#define EXT_UI_HEIGHT           1020 
#define MESSAGE_ALL             2000
#define PORT_ACT_DIAGN          2001
#define STARB_ACT_DIAGN         2002
#define PORT_HGHT_DIAGN         2003
#define STARB_HGHT_DIAGN        2004
#define GYRO_GPS_DIAGN          2005
#define EXT_UI_DIAGN            2006

#define CCW                     0
#define CW                      1

#define V_MAX                   1.85
#define NSAMPLES                10000    //current samples for ref.

#define ROLL_IN_MIN -90
#define ROLL_IN_MAX 90
#define ROLL_OUT_MIN -0.1
#define ROLL_OUT_MAX 0.1

#define Kc 1.0
#define Ti 0.0
#define Td 0.0
#define RATE 0.01

enum MSG_t
{
    MSG1 = 0,       //  message 1
    MSG2,           //  message 2
    MSG3            //  message 3  
};

//////////////////////////////////////////////////////////////////////////////////////
// port declaration                                                                 //
//////////////////////////////////////////////////////////////////////////////////////

DigitalOut myLed(LED1);
DigitalOut myLed2(LED2);

DigitalOut aliveLed(p23);
DigitalOut statusLed(p26);
DigitalOut emergencyLed(p25);
DigitalOut motorRelais(p20);

AnalogIn currentSensor(p15);

InterruptIn intAGM(p8);

I2C i2c(p9, p10);
GPS gps(p13, p14);
// I2C Rx, I2C Tx, Int
MPU9150 imu(p10, p9, p8);  
//mRotaryEncoder(PinName pinA, PinName pinB, PinName pinSW, PinMode pullMode=PullUp, int debounceTime_us=1000)
mRotaryEncoder  rSwitch(p12, p11, p7);
CAN CANbus(p30, p29);
MCP23017 mcp23017 = MCP23017(i2c, 0x40);
Serial debug(USBTX, USBRX);

PID rollPID(Kc,Ti,Td,RATE); // declare a PID for roll adjustment

//////////////////////////////////////////////////////////////////////////////////////
// function prototypes                                                              //
//////////////////////////////////////////////////////////////////////////////////////


//////////////////////////////////////////////////////////////////////////////////////
// global variables                                                                 //
//////////////////////////////////////////////////////////////////////////////////////

int i = 0;
int value = 0;
int lastGet;
int thisGet;
int iFilter;
uint32_t uiFilterId;
  

bool enc_pressed = false;       // Button of rotaryencoder was pressed
bool enc_rotated = false;       // rotary encoder was totaded left or right
bool bSystemFail = false;       // sytem fail

char buffer_GPIOB;
char cBuffer[200];
char cMessage;
char text[16];
char cPTurns[2] ={0};
char cSTurns[2] ={0};
//char buffer[16];

float   fRoll, fPitch, fEulerRoll;
float   fRollDiff;
float   fPreviousTime;

double  dCurrent    = 0.0;
double  dRefCurrent = 0.0;

Quaternion q1;

Ticker  CANTicker;
Ticker  tAliveOn;
Ticker  tEmergencyLedOn;
Ticker  tMPU;

Timeout tAliveOff;
Timeout tEmergencyLedOff;
Timer   actualTime;
Timer   timer;

Menu mLCDMenu;
SystemVar svSoF;

MSG_t messageToSend;
//////////////////////////////////////////////////////////////////////////////////////
// main                                                                             //
//////////////////////////////////////////////////////////////////////////////////////

int main()
{
//////////////////////////////////////////////////////////////////////////////////////
// init                                                                             //
//////////////////////////////////////////////////////////////////////////////////////      
    vInit();        
    // init CANbus
    vInitCANBus();
    // init Gyro
    vInitImu();
    
       
    CANMessage msg;
    messageToSend = MSG1;
    
    
    //acquiring v_ref
    for (int i=0; i<NSAMPLES; i++) 
    {
        dRefCurrent += currentSensor;
    }
    dRefCurrent /= NSAMPLES;
    //debug.printf("RefCurrent : %f\n\r", dRefCurrent);
    
    timer.start();      //timer to test imu roll angle
    actualTime.start(); //crash timer 
    // clear line -1-
    vLCD_printPos_I2C((unsigned char*)"                 ", 1, 1);
    
    // put text on lcd
    
    //mLCDMenu.vShowScreen(_FOILBORNE);
    //vLCD_printPos_I2C((unsigned char*)mLCDMenu.getLine1(), 2, 1);
    
//////////////////////////////////////////////////////////////////////////////////////
// endless loop                                                                     //
//////////////////////////////////////////////////////////////////////////////////////
    while (1)
    {
        // rotary switch has been rotated?
        if (enc_rotated) vSwitchRotated();

        // rotary switch confirm pressed?
        if (enc_pressed) vSwitchConfirmed();
        
        // answer CAN
        if(CANbus.read(msg)){
            if (msg.id == EXT_UI_HEIGHT) svSoF.vVarHeightFoilBorne((uint32_t) msg.data[0]);
        }    
        if(CANbus.read(msg)){
            debug.printf("ID: %i.\t", msg.id);
            if ((msg.id == HEIGHT_PORT_DATA)&&(mLCDMenu.bGetReadHeight() == true))
            { 
                svSoF.vSetPHeight((uint32_t) msg.data[0]);
                debug.printf("height received: %c cm\r\n", msg.data[0]);
            }
            //if ((msg.id == ACTUATOR_PORT_ID)&&(msg.data[0] == 0xFF)) statusLed = ! statusLed;
            //debug.printf("ID#102 received: %d answer.\r\n", msg.data[0]);
        }
        
        if(timer.read_ms() > 250)
        {
            timer.reset();
            if(svSoF.uiGetRoll() >= 4)
            {
                if(svSoF.iGetRollPolarity() == 0)
                {
                   cPTurns[1] = 1;
                   cSTurns[1] = 0;
                }
                else
                {
                   cPTurns[1] = 0;
                   cSTurns[1] = 1;
                } 
                cPTurns[0] = 1;
                cSTurns[0] = 1;
            
                CANbus.write(CANMessage(ACTUATOR_PORT_RUN, cPTurns, 2)); // send message to port actuator
                wait(0.15);
                CANbus.write(CANMessage(ACTUATOR_STARB_RUN, cSTurns, 2)); // send message to port actuator           
                
            }//end if
            svSoF.vSetPitch(q1);                                 // call function class SystemVar
            svSoF.vSetRoll(q1);
            mLCDMenu.vShowScreen(mLCDMenu.getScreen());
            vLCD_update();
            
            //debug.printf("Roll: %c\t", svSoF.getValue());
            //debug.printf("Roll: %f\t", getRollAngle(q1));
            //debug.printf("Pitch: %f\t", getPitchAngle(q1));
            //debug.printf("Time: %f\r\n", actualTime.read());
        }
        /*for (int samples = 0; samples<NSAMPLES; samples++) dCurrent += currentSensor;
            
            dCurrent /= NSAMPLES;
            //debug.printf("RefCurrent : %f\n\r", dRefCurrent);
            //debug.printf("dCurrent   : %f\n\r", dCurrent);
            dCurrent = (dCurrent-dRefCurrent)*V_MAX/0.185;
        
        debug.printf("Current mA : %f\n\r", (dCurrent*1000));*/
        wait(0.2);
       
    }
    
} // END MAIN 

//////////////////////////////////////////////////////////////////////////////////////
// delay function for LCD                                                           //
//////////////////////////////////////////////////////////////////////////////////////

void vLCD_delay_I2C (unsigned int t_delay)
{
    unsigned int i;
    
    for (i=0; i < t_delay; i++);
}

//////////////////////////////////////////////////////////////////////////////////////
// write commando to LCD                                                            //
//////////////////////////////////////////////////////////////////////////////////////

void vLCD_cmd_I2C   (unsigned char commando)
{
__disable_irq();    // Disable Interrupts

    // write commando to LCD
    mcp23017.write(PORT_A, commando);
    
    // LCD_RS = 0
    buffer_GPIOB = buffer_GPIOB & 0xFD;   // RS = 0
    // write to LCD
    mcp23017.write(PORT_B, buffer_GPIOB);
    
    // LCD_EN = 1
    buffer_GPIOB = buffer_GPIOB | 0x01;   // EN = 1
    // write to LCD
    mcp23017.write(PORT_B, buffer_GPIOB);
    
    // LCD_EN = 0
    buffer_GPIOB = buffer_GPIOB & 0xFE;   // EN = 0
    // write to LCD
    mcp23017.write(PORT_B, buffer_GPIOB);
    
__enable_irq();     // Enable Interrupts
}

//////////////////////////////////////////////////////////////////////////////////////
// write single char to LCD                                                         //
//////////////////////////////////////////////////////////////////////////////////////

void vLCD_data_I2C  (unsigned char data)
{
__disable_irq();    // Disable Interrupts
 
    // write data to LCD
    mcp23017.write(PORT_A, data);
    
    // LCD_RS = 1
    buffer_GPIOB = buffer_GPIOB | 0x02;   // RS = 1
    // write to LCD
    mcp23017.write(PORT_B, buffer_GPIOB);
    
    // LCD_EN = 1
    buffer_GPIOB = buffer_GPIOB | 0x01;   // EN = 1
    // write to LCD
    mcp23017.write(PORT_B, buffer_GPIOB);
    
    // LCD_EN = 0
    buffer_GPIOB = buffer_GPIOB & 0xFE;   // EN = 0
    // write to LCD
    mcp23017.write(PORT_B, buffer_GPIOB);
    
__enable_irq();     // Enable Interrupts
}

//////////////////////////////////////////////////////////////////////////////////////
// Initialize LCD                                                                   //
//////////////////////////////////////////////////////////////////////////////////////

void vLCD_init_I2C  (void)
{
    vLCD_cmd_I2C (0x00);
    // vLCD_delay_I2C (65535)
    vLCD_delay_I2C (100);
    
    vLCD_cmd_I2C (0x38);
    // vLCD_delay_I2C (65535)
    vLCD_delay_I2C (100);
    
    vLCD_cmd_I2C (0x38);
    //vLCD_delay_I2C (65535)
    vLCD_delay_I2C (100);
    
    vLCD_cmd_I2C (0x38);
    //vLCD_delay_I2C (65535)
    vLCD_delay_I2C (100);
    
    // Display ON / OFF
    vLCD_cmd_I2C (0x08);
    
    //vLCD_delay_I2C (65535)
    vLCD_delay_I2C (100);
    
    // Clear Display
    vLCD_cmd_I2C (0x01);
    
    //vLCD_delay_I2C (65535)
    vLCD_delay_I2C (100);
    
    // Entry Mode Set
    vLCD_cmd_I2C (0x06);
    
    //vLCD_delay_I2C (65535)
    vLCD_delay_I2C (100);
    
    // Display ON / OFF
    vLCD_cmd_I2C (0x0C);
    
    //vLCD_delay_I2C (65535)
    vLCD_delay_I2C (100);
}

//////////////////////////////////////////////////////////////////////////////////////
// Write string to LCD                                                              //
//////////////////////////////////////////////////////////////////////////////////////

void vLCD_print_I2C (unsigned char *string, unsigned char line)
{
    unsigned int i;
    
    // select target line to write to
    if (line == 1)
    {
        vLCD_cmd_I2C (0x80);
    }
    else if (line == 2)
    {
        vLCD_cmd_I2C (0xC0);
    }
    else
    {
        return;    // End of function: Error in the past value of line
    }
    
    // write data to selected line
    for (i=0; i<16; i++)
    {
        if (string [i] != 0x00) 
        {
            vLCD_data_I2C (string [i]);
        }
        else
            vLCD_data_I2C (' ');
    }
}

//////////////////////////////////////////////////////////////////////////////////////
// Write string to LCD                                                              //
//////////////////////////////////////////////////////////////////////////////////////

void vLCD_printPos_I2C (unsigned char *string, unsigned char line, unsigned char character)
{
    unsigned int i;
    
    // select target line and position to write to
    if (line == 1)
    {
        vLCD_cmd_I2C (0x80+(character-1));
    }
    
    else if (line == 2)
    {
        vLCD_cmd_I2C (0xC0+(character-1));
    }
    
    else
    {
        return;    // End of function: Error in the past value of line
    }
    
    // write data to selected line and position
    for (i=0; i<16; i++)
    {
        if (string [i] != 0x00)
        {
            vLCD_data_I2C (string [i]);
        }
        else
            vLCD_data_I2C (' ');
    }
}

//////////////////////////////////////////////////////////////////////////////////////
// update LCD                                                                       //
//////////////////////////////////////////////////////////////////////////////////////
void vLCD_clear_I2C (void)
{
    // Clear Display
    vLCD_cmd_I2C (0x01);
    //vLCD_delay_I2C (65535)
    vLCD_delay_I2C (100);
    // Display ON / OFF
    vLCD_cmd_I2C (0x0C);
    //vLCD_delay_I2C (65535)
    vLCD_delay_I2C (100);
}

//////////////////////////////////////////////////////////////////////////////////////
// update LCD                                                                       //
//////////////////////////////////////////////////////////////////////////////////////
void vLCD_update (void)
{
   vLCD_print_I2C((unsigned char*)mLCDMenu.getLine1(), 1);
   vLCD_print_I2C((unsigned char*)mLCDMenu.getLine2(), 2);
   //vLCD_printPos_I2C((unsigned char*)mLCDMenu.getLine1(), 1, 1);
   //vLCD_printPos_I2C((unsigned char*)mLCDMenu.getLine2(), 2, 1);
}

//////////////////////////////////////////////////////////////////////////////////////
// Init SOF module                                                                  //
//////////////////////////////////////////////////////////////////////////////////////
void vInit (void)
{
    // I2C init
    i2c.frequency(100000);
    //Port A is databus - Output
    mcp23017.direction(PORT_A, PORT_DIR_OUT);
    //Port B is controlbus - Output
    mcp23017.direction(PORT_B, PORT_DIR_OUT);
   
    debug.baud(115200);
    // initialize LCD
    vLCD_init_I2C();
    mLCDMenu.vSelectMenu(_MENU0_0);
    vLCD_update();
      
    // led's out
    aliveLed = 1; wait(0.1); aliveLed = 0;
    statusLed = 1; wait(0.1); statusLed = 0;
    emergencyLed = 1; wait(0.1); emergencyLed = 0;
        
    wait(0.2);
    motorRelais = 1;
    wait(1.0);
    motorRelais = 0;
    
    //Int-Handler rotary encoder switch
    // call trigger_sw() when button of rotary-encoder is pressed
    rSwitch.attachSW(&trigger_sw);
    // call trigger_rot() when the shaft is rotaded left or right
    rSwitch.attachROT(&trigger_rotated);
    lastGet = 0;
    // set encrotated, so position is displayed on startup
    enc_rotated = true;
    // blink alive LED
    aliveLed = 1;
    tAliveOff.attach(&vBlinkOnce_cb, 0.05);
    tAliveOn.attach(&vBlinkOnce_cb, 5);   
}

//////////////////////////////////////////////////////////////////////////////////////
// Init MPU 9150                                                                    //
//////////////////////////////////////////////////////////////////////////////////////
void vInitImu (void)
{
    if(imu.isReady()) mLCDMenu.vShowScreen(_IMUREADY);
    else  mLCDMenu.vShowScreen(_IMUFAIL);
    vLCD_update();
        
    imu.initialiseDMP();
    imu.setFifoReset(true);    
    imu.setDMPEnabled(true);
    intAGM.rise(&vGetMPUBuffer);
    //tMPU.attach_us(&vGetMPUBuffer,50000); 
}

//////////////////////////////////////////////////////////////////////////////////////
// Init CANbus                                                                      //
//////////////////////////////////////////////////////////////////////////////////////
void vInitCANBus (void)
{
    CANbus.frequency(250000);
    debug.printf("vInitCANBus\r\n");
    CANMessage msg;
    int i, iTry = 10;
    uint32_t id = MESSAGE_ALL;
    bool bReceived = false;
    cMessage = 0x00;
    // check CAN system
    for (i = 6; i > 0 ; i--)
    {
        debug.printf("for loop: %i\r\n", i);
        bReceived = false;
        id++;   // increase id
        while ((iTry > 0)&&(!bReceived))
        {
            CANbus.write(CANMessage(id, &cMessage, 1));
            wait(0.15);
            switch(id){     // set adress to filter 
                case PORT_ACT_DIAGN:         uiFilterId = ACTUATOR_PORT_ID; break;
                case STARB_ACT_DIAGN:        uiFilterId = ACTUATOR_STARB_ID; break;
                case PORT_HGHT_DIAGN:        uiFilterId = HEIGHT_PORT_ID; break;
                case STARB_HGHT_DIAGN:       uiFilterId = HEIGHT_STARB_ID; break;
                case GYRO_GPS_DIAGN:         uiFilterId = GYRO_GPS_ID; break;
                case EXT_UI_DIAGN:           uiFilterId = EXT_UI_ID; break;
            }//end switch
            iFilter = CANbus.filter(0x200, 0x7FF, CANStandard);
            if(CANbus.read(msg, iFilter)){
                debug.printf("can.read(msg) ID: %i\r\n", id);
                debug.printf("can.msg id: %i, %i\r\n", msg.id, msg.data[0]);
                switch(id){
                    case PORT_ACT_DIAGN:        if((msg.id == ACTUATOR_PORT_ID)&&(msg.data[0] == 0xFF)) mLCDMenu.vShowScreen(_CANID101OK); break;
                    case STARB_ACT_DIAGN:       if((msg.id == ACTUATOR_STARB_ID)&&(msg.data[0] == 0xFF)) mLCDMenu.vShowScreen(_CANID102OK); break;
                    case PORT_HGHT_DIAGN:       if((msg.id == HEIGHT_PORT_ID)&&(msg.data[0] == 0xFF)) mLCDMenu.vShowScreen(_CANID201OK); break;
                    case STARB_HGHT_DIAGN:      if((msg.id == HEIGHT_STARB_ID)&&(msg.data[0] == 0xFF)) mLCDMenu.vShowScreen(_CANID202OK); break;
                    case GYRO_GPS_DIAGN:        if((msg.id == GYRO_GPS_ID)&&(msg.data[0] == 0xFF)) mLCDMenu.vShowScreen(_CANID205OK); break;
                    case EXT_UI_DIAGN:          if((msg.id == EXT_UI_ID)&&(msg.data[0] == 0xFF)) mLCDMenu.vShowScreen(_CANID1001OK); break;                                                                                      
                } //end switch
                vLCD_update();
                bReceived = true;
            } //end if
            else if(iTry == 1){
                switch(id){
                    case PORT_ACT_DIAGN:        mLCDMenu.vShowScreen(_CANID101FAIL); break;
                    case STARB_ACT_DIAGN:       mLCDMenu.vShowScreen(_CANID102FAIL); break;
                    case PORT_HGHT_DIAGN:       mLCDMenu.vShowScreen(_CANID201FAIL); break;
                    case STARB_HGHT_DIAGN:      mLCDMenu.vShowScreen(_CANID202FAIL); break;
                    case GYRO_GPS_DIAGN:        mLCDMenu.vShowScreen(_CANID205FAIL); break;
                    case EXT_UI_DIAGN:          mLCDMenu.vShowScreen(_CANID1001FAIL); break;                                                                                      
                } //end switch
                vLCD_update();
            } //end if
            if(CANbus.read(msg)){}//unwanted message
            if(!bReceived) iTry--;
            debug.printf("Tries: %i\r\n", iTry);                
        } //end while
        wait(0.5); // some time to read message
        iTry = 10; // reset try counter
// *****afhandelen can fail ****        
        if (iTry != 0) iTry = 10;
        else bSystemFail = true;
    } //end for

    // in case system fail
    if(bSystemFail){ 
        vCallEmergencyLed();
        mLCDMenu.vShowScreen(_CANNORESPONSE);
    }
    cMessage = (char) svSoF.iGetHeightFoilBorne();
    CANbus.write(CANMessage(EXT_UI_HEIGHT, &cMessage, 1));
    
    mLCDMenu.vSelectMenu(_MENU0_0);
    vLCD_update();
    debug.printf("end can init");  
}

// call vCANBusSend every 1 second test
//    CANTicker.attach(&vCANBusSend, 1);

//////////////////////////////////////////////////////////////////////////////////////
// Blink alive led                                                                  //
//////////////////////////////////////////////////////////////////////////////////////
void vBlinkOff_cb (void)
{
    aliveLed = 0;
}

//////////////////////////////////////////////////////////////////////////////////////
// Blink emergecy led                                                                  //
//////////////////////////////////////////////////////////////////////////////////////
void vEmergencyOff_cb (void)
{
    emergencyLed = 0;
}

//////////////////////////////////////////////////////////////////////////////////////
// Blink status led once                                                            //
//////////////////////////////////////////////////////////////////////////////////////
void vBlinkOnce_cb (void)
{
    aliveLed = 1;
    tAliveOff.detach();
    tAliveOff.attach(&vBlinkOff_cb, 0.05);
}

//////////////////////////////////////////////////////////////////////////////////////
// Blink emergency led once                                                            //
//////////////////////////////////////////////////////////////////////////////////////
void vEmergencyOnce_cb (void)
{
    emergencyLed = 1;
    tEmergencyLedOff.detach();
    tEmergencyLedOff.attach(&vEmergencyOff_cb, 0.05);
    
    if (!bSystemFail)
    {
        tEmergencyLedOn.detach();
        tEmergencyLedOff.detach();
        aliveLed = 0;    
    }
}

//////////////////////////////////////////////////////////////////////////////////////
// Call emergency led                                                               //
//////////////////////////////////////////////////////////////////////////////////////
void vCallEmergencyLed(void)
{
        aliveLed = 1;
        tEmergencyLedOff.attach(&vEmergencyOnce_cb, 0.05);
        tEmergencyLedOn.attach(&vEmergencyOnce_cb, 0.5);    
}

//////////////////////////////////////////////////////////////////////////////////////
// Writing integer to LCD                                                           //
//////////////////////////////////////////////////////////////////////////////////////
void vLCD_printInt_I2C (int value, unsigned char line, unsigned char character)
{
        static char buffer[32];
        // convert int to char
        sprintf(buffer, "%.2d", value);
        // write converted int to LCD
    vLCD_printPos_I2C((unsigned char*)buffer, line, character);
}

//////////////////////////////////////////////////////////////////////////////////////
// calculate Roll from quaternion                                                   //
//////////////////////////////////////////////////////////////////////////////////////
float getRollAngle(Quaternion q1)
{
    float fRoll;
    fRoll = atan2(2*(q1.v.x*q1.v.y + q1.w*q1.v.y), q1.w*q1.w + q1.v.x*q1.v.x - q1.v.y*q1.v.y - q1.v.y*q1.v.y);
    fRoll *= 180/3.14;
    return (fRoll);
}

//////////////////////////////////////////////////////////////////////////////////////
// calculate Pitch from quaternion                                                   //
//////////////////////////////////////////////////////////////////////////////////////
float getPitchAngle(Quaternion q1)
{
    Vector3 euler = q1.getEulerAngles();
    //debug.printf("$ Pitch = %f #\t", euler.y);
    float fPitch;
    //fPitch = atan2((2*((q1.v.x*q1.v.z) - (q1.w*q1.v.y))),(sqrt((2*((q1.w*q1.v.x) + (q1.v.y*q1.v.z)))*(2*((q1.w*q1.v.x) + (q1.v.y*q1.v.z)))) + (((q1.w*q1.w) - (q1.v.x*q1.v.x) - (q1.v.y*q1.v.y) + (q1.v.z*q1.v.z))*((q1.w*q1.w) - (q1.v.x*q1.v.x) - (q1.v.y*q1.v.y) + (q1.v.z*q1.v.z)))));
    //fPitch = acos(-(q1.w*q1.w) - (q1.v.x*q1.v.x) + (q1.v.y*q1.v.y) + (q1.v.z*q1.v.z));
    //fPitch = -asin(2*q1.w*q1.v.y - 2*q1.v.x*q1.v.z);
    fPitch = euler.y;
    fPitch *= 180/3.14;
    return (fPitch);
    
}

//////////////////////////////////////////////////////////////////////////////////////
// interrup-Handler for button on rotary-encoder                                    //
//////////////////////////////////////////////////////////////////////////////////////
void trigger_sw(void) 
{
    enc_pressed = true;               // just set the flag, rest is done outside isr
}

//////////////////////////////////////////////////////////////////////////////////////
// interrup-Handler for rotary-encoder rotation                                     //
//////////////////////////////////////////////////////////////////////////////////////
void trigger_rotated(void) 
{
    enc_rotated = true;               // just set the flag, rest is done outside isr
}

//////////////////////////////////////////////////////////////////////////////////////
// reset for rotary-encoder switch                                                  //
//////////////////////////////////////////////////////////////////////////////////////
void vResetSwitch (void)
{
    rSwitch.Set(0);
}

//////////////////////////////////////////////////////////////////////////////////////
// set for rotary-encoder switch                                                    //
//////////////////////////////////////////////////////////////////////////////////////
void vSetSwitch (int value)
{
    rSwitch.Set(value);   
}

//////////////////////////////////////////////////////////////////////////////////////
// rotary-encoder switch rotated                                                    //
//////////////////////////////////////////////////////////////////////////////////////
void vSwitchRotated (void)
{
    enc_rotated = false;
    
    // get value from rotary encoder
    thisGet = rSwitch.Get();
    
    if(rSwitch.Get()<0) mLCDMenu.vRotaryDown();
    else if(rSwitch.Get()>=0) mLCDMenu.vRotaryUp();
    
    // reset switch count
    vResetSwitch();
    vLCD_update();
}

//////////////////////////////////////////////////////////////////////////////////////
// rotary-encoder switch confirmed                                                  //
//////////////////////////////////////////////////////////////////////////////////////
void vSwitchConfirmed (void)
{
    enc_pressed = false;
    
    mLCDMenu.vRotaryConfirm();
    
    // reset switch count
    vResetSwitch();
    
    vLCD_update();
}

//////////////////////////////////////////////////////////////////////////////////////
// CANbus send  not used                                                            //
//////////////////////////////////////////////////////////////////////////////////////
void vCANBusSend(void) 
{
    static char counter = 0;
    static char counterTwo = 0;
    static char random[8] = {0};
    //vLCD_printPos_I2C((unsigned char*)"CANbus send     ", 2, 1);
    switch(messageToSend){
        case MSG1:
        if(CANbus.write(CANMessage(1336, &counter, 1))) 
        {
            counter++;
            messageToSend = MSG2;
            break;
        }
        case MSG2: 
        if(CANbus.write(CANMessage(1003, &counterTwo, 1))) 
        {
            counterTwo++;
            messageToSend = MSG3;
            break;
        }
        case MSG3:
        if(CANbus.write(CANMessage(1236, random, 4))) 
        {
            random[0] = rand()% 10 + 1;
            random[1] = rand()% 10 + 20;
            random[2] = rand()% 100 + 1;
            random[3] = 00;
            //messageToSend = MSG1;
            break;
        }
    }   // end switch case
     
    myLed = !myLed;
}

//////////////////////////////////////////////////////////////////////////////////////
// Get data from MPU fifo buffer                                                    //
//////////////////////////////////////////////////////////////////////////////////////
void vGetMPUBuffer(void)
{
    int iFifoCount;
    if (imu.getInterruptFifoOverflow())
        imu.setFifoReset(true);
    iFifoCount = imu.getFifoCount();
    //debug.printf("fifocount: %i\r\n", iFifoCount);
    if(imu.getFifoCount() >= 48){
        if(iFifoCount % 48 != 0) imu.getFifoBuffer(cBuffer,  iFifoCount % 48);   
    //imu.getFifoBuffer(cBuffer,  iFifoCount- 48);
    imu.getFifoBuffer(cBuffer,  48);
        //myLed2 = !myLed2;
        q1.decode(cBuffer);// quaternion vector to buffer
     }  
}

//////////////////////////////////////////////////////////////////////////////////////
// EOF                                                                              //
//////////////////////////////////////////////////////////////////////////////////////

    /*if (fifoCount == 1024)                                                                                                                                  // test
        {mpu.resetFIFO();}            // reset so we can continue cleanly
   
    else if (fifoCount >= 42)                // otherwise, check for DMP data ready interrupt (this should happen frequently)
    {
        if ((fifoCount % stMPU9150->packetSize) != 0)                                                                                                       // test
            {mpu.getFIFOBytes(fifoBuffer, (fifoCount % stMPU9150->packetSize));}                                                                            // test
       
        while (mpu.getFIFOCount() >= 42)
            {mpu.getFIFOBytes(fifoBuffer, stMPU9150->packetSize);}            // read a packet from FIFO
       
        mpu.dmpGetQuaternion(&stMPU9150->q, fifoBuffer);*/