HAN Solarboat
version 1.0
Dependencies: CMSIS_DSP_401 GPS MPU9150_DMP PID QuaternionMath Servo mbed
Fork of
SolarOnFoils_MainModule_20150518
by 
Dannis Brugman
Diff: main.cpp
- Revision:
- 0:81b21910454e
- Child:
- 1:b4a0d63db637
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp Tue Jun 23 13:55:28 2015 +0000
@@ -0,0 +1,828 @@
+//////////////////////////////////////////////////////////////////////////////////////
+// //
+// 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 ACTUATOR_PORT_ID 101
+#define ACTUATOR_STARB_ID 102
+#define HEIGHT_PORT_ID 201
+#define HEIGHT_STARB_ID 202
+#define GYRO_GPS_ID 205
+#define EXT_UI_ID 20
+#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 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.cShowScreen(_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 on id#1003
+ if(CANbus.read(msg)){
+ 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() > 500)
+ {
+ timer.reset();
+ svSoF.vSetPitch(q1); // call function class SystemVar
+ mLCDMenu.cShowScreen(_PITCHTEST);
+ 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.cShowScreen(_INIT);
+ 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.cShowScreen(_IMUREADY);
+ else mLCDMenu.cShowScreen(_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.cShowScreen(_CANID101OK); break;
+ case STARB_ACT_DIAGN: if((msg.id == ACTUATOR_STARB_ID)&&(msg.data[0] == 0xFF)) mLCDMenu.cShowScreen(_CANID102OK); break;
+ case PORT_HGHT_DIAGN: if((msg.id == HEIGHT_PORT_ID)&&(msg.data[0] == 0xFF)) mLCDMenu.cShowScreen(_CANID201OK); break;
+ case STARB_HGHT_DIAGN: if((msg.id == HEIGHT_STARB_ID)&&(msg.data[0] == 0xFF)) mLCDMenu.cShowScreen(_CANID202OK); break;
+ case GYRO_GPS_DIAGN: if((msg.id == GYRO_GPS_ID)&&(msg.data[0] == 0xFF)) mLCDMenu.cShowScreen(_CANID205OK); break;
+ case EXT_UI_DIAGN: if((msg.id == EXT_UI_ID)&&(msg.data[0] == 0xFF)) mLCDMenu.cShowScreen(_CANID1001OK); break;
+ } //end switch
+ vLCD_update();
+ bReceived = true;
+ } //end if
+ else if(iTry == 1){
+ switch(id){
+ case PORT_ACT_DIAGN: mLCDMenu.cShowScreen(_CANID101FAIL); break;
+ case STARB_ACT_DIAGN: mLCDMenu.cShowScreen(_CANID102FAIL); break;
+ case PORT_HGHT_DIAGN: mLCDMenu.cShowScreen(_CANID201FAIL); break;
+ case STARB_HGHT_DIAGN: mLCDMenu.cShowScreen(_CANID202FAIL); break;
+ case GYRO_GPS_DIAGN: mLCDMenu.cShowScreen(_CANID205FAIL); break;
+ case EXT_UI_DIAGN: mLCDMenu.cShowScreen(_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.cShowScreen(_CANNORESPONSE);
+ 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)
+ //debug.printf("negatief \r\n");
+ //else if(rSwitch.Get()>=0)
+ //debug.printf("positief \r\n");
+ // --- show for development ---
+ mLCDMenu.cShowScreen(_PULSE_IS);
+ vLCD_update();
+
+ //vLCD_printInt_I2C(thisGet, 1, 11);
+}
+
+//////////////////////////////////////////////////////////////////////////////////////
+// rotary-encoder switch confirmed //
+//////////////////////////////////////////////////////////////////////////////////////
+void vSwitchConfirmed (void)
+{
+ static char cTurns[2] ={0}; //[value][direction]
+ enc_pressed = false;
+
+ cTurns[0] = 10;//abs(rSwitch.Get()); // send absolute value
+ if(rSwitch.Get()>=0) cTurns[1] = CW; // CW
+ else cTurns[1] = CCW; // CCW
+ CANbus.write(CANMessage(ACTUATOR_STARB_ID, cTurns, 2)); // send message to port actuator
+ CANbus.write(CANMessage(ACTUATOR_PORT_ID, cTurns, 2)); // send message to port actuator
+ // reset switch count
+ vResetSwitch();
+ // get value from rotary encoder
+ thisGet = rSwitch.Get();
+ // --- show for development ---
+ //vLCD_printInt_I2C(thisGet, 1, 11);
+}
+
+//////////////////////////////////////////////////////////////////////////////////////
+// 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);*/