HAN Solarboat / Mbed 2 deprecated SolarOnFoils_MainModule_20150518

version 1.0

Dependencies:   CMSIS_DSP_401 GPS MPU9150_DMP PID QuaternionMath Servo mbed

Fork of SolarOnFoils_MainModule_20150518 by Dannis Brugman

main.cpp@2:f6d058931b17, 2015-08-11 (annotated)

Committer:
Dannis_mbed
Date:
Tue Aug 11 08:38:55 2015 +0000
Revision:
2:f6d058931b17
Parent:
1:b4a0d63db637 
Test version mainmodule

Who changed what in which revision?

UserRevisionLine numberNew contents of line
Dannis_mbed 0:81b21910454e 1 //////////////////////////////////////////////////////////////////////////////////////
Dannis_mbed 0:81b21910454e 2 // //
Dannis_mbed 0:81b21910454e 3 // File : main.cpp //
Dannis_mbed 0:81b21910454e 4 // Version : 0.1 //
Dannis_mbed 0:81b21910454e 5 // Date : 25 march 2015 //
Dannis_mbed 0:81b21910454e 6 // Author : Dany Brugman //
Dannis_mbed 0:81b21910454e 7 // Comment : Function to write data to a 2x16 LCD by I2C //
Dannis_mbed 0:81b21910454e 8 // using a MCP23017 port expander. //
Dannis_mbed 0:81b21910454e 9 // //
Dannis_mbed 0:81b21910454e 10 // Changelog : //
Dannis_mbed 0:81b21910454e 11 // Date: Name: Comment: //
Dannis_mbed 0:81b21910454e 12 // 25/03/2015 DNB First version //
Dannis_mbed 0:81b21910454e 13 // //
Dannis_mbed 0:81b21910454e 14 //////////////////////////////////////////////////////////////////////////////////////
Dannis_mbed 0:81b21910454e 15
Dannis_mbed 0:81b21910454e 16 //////////////////////////////////////////////////////////////////////////////////////
Dannis_mbed 0:81b21910454e 17 // includes //
Dannis_mbed 0:81b21910454e 18 //////////////////////////////////////////////////////////////////////////////////////
Dannis_mbed 0:81b21910454e 19
Dannis_mbed 0:81b21910454e 20 #include "mbed.h"
Dannis_mbed 0:81b21910454e 21 #include <stdio.h>
Dannis_mbed 0:81b21910454e 22 #include <stdlib.h>
Dannis_mbed 0:81b21910454e 23 #include <string.h>
Dannis_mbed 0:81b21910454e 24 #include "LCD_I2C.h"
Dannis_mbed 0:81b21910454e 25 #include "uart.h"
Dannis_mbed 0:81b21910454e 26 #include "GPS.h"
Dannis_mbed 0:81b21910454e 27 #include "MPU9150.h"
Dannis_mbed 0:81b21910454e 28 #include "Quaternion.h"
Dannis_mbed 0:81b21910454e 29 #include "PID.h"
Dannis_mbed 0:81b21910454e 30 #include "Servo.h"
Dannis_mbed 0:81b21910454e 31 #include "mRotaryEncoder.h"
Dannis_mbed 0:81b21910454e 32 #include "MainModule.h"
Dannis_mbed 0:81b21910454e 33 #include "menu.h"
Dannis_mbed 0:81b21910454e 34 #include "systemVar.h"
Dannis_mbed 0:81b21910454e 35
Dannis_mbed 0:81b21910454e 36 //////////////////////////////////////////////////////////////////////////////////////
Dannis_mbed 0:81b21910454e 37 // defines //
Dannis_mbed 0:81b21910454e 38 //////////////////////////////////////////////////////////////////////////////////////
Dannis_mbed 0:81b21910454e 39
Dannis_mbed 0:81b21910454e 40 #define CLEAR "\033[2J"
Dannis_mbed 0:81b21910454e 41 #define PROJECT "\033[1;10fSolar on Foils\n"
Dannis_mbed 0:81b21910454e 42 #define VERSION "\033[2;10fVersion 1.0\n"
Dannis_mbed 0:81b21910454e 43
Dannis_mbed 1:b4a0d63db637 44 #define EXT_UI_ID 20
Dannis_mbed 0:81b21910454e 45 #define ACTUATOR_PORT_ID 101
Dannis_mbed 0:81b21910454e 46 #define ACTUATOR_STARB_ID 102
Dannis_mbed 1:b4a0d63db637 47 #define ACTUATOR_PORT_RUN 103
Dannis_mbed 1:b4a0d63db637 48 #define ACTUATOR_STARB_RUN 104
Dannis_mbed 0:81b21910454e 49 #define HEIGHT_PORT_ID 201
Dannis_mbed 0:81b21910454e 50 #define HEIGHT_STARB_ID 202
Dannis_mbed 1:b4a0d63db637 51 #define HEIGHT_PORT_DATA 203
Dannis_mbed 1:b4a0d63db637 52 #define HEIGHT_STARB_DATA 204
Dannis_mbed 0:81b21910454e 53 #define GYRO_GPS_ID 205
Dannis_mbed 1:b4a0d63db637 54 #define EXT_UI_HEIGHT 1020
Dannis_mbed 0:81b21910454e 55 #define MESSAGE_ALL 2000
Dannis_mbed 0:81b21910454e 56 #define PORT_ACT_DIAGN 2001
Dannis_mbed 0:81b21910454e 57 #define STARB_ACT_DIAGN 2002
Dannis_mbed 0:81b21910454e 58 #define PORT_HGHT_DIAGN 2003
Dannis_mbed 0:81b21910454e 59 #define STARB_HGHT_DIAGN 2004
Dannis_mbed 0:81b21910454e 60 #define GYRO_GPS_DIAGN 2005
Dannis_mbed 0:81b21910454e 61 #define EXT_UI_DIAGN 2006
Dannis_mbed 0:81b21910454e 62
Dannis_mbed 0:81b21910454e 63 #define CCW 0
Dannis_mbed 0:81b21910454e 64 #define CW 1
Dannis_mbed 0:81b21910454e 65
Dannis_mbed 0:81b21910454e 66 #define V_MAX 1.85
Dannis_mbed 0:81b21910454e 67 #define NSAMPLES 10000 //current samples for ref.
Dannis_mbed 0:81b21910454e 68
Dannis_mbed 0:81b21910454e 69 #define ROLL_IN_MIN -90
Dannis_mbed 0:81b21910454e 70 #define ROLL_IN_MAX 90
Dannis_mbed 0:81b21910454e 71 #define ROLL_OUT_MIN -0.1
Dannis_mbed 0:81b21910454e 72 #define ROLL_OUT_MAX 0.1
Dannis_mbed 0:81b21910454e 73
Dannis_mbed 0:81b21910454e 74 #define Kc 1.0
Dannis_mbed 0:81b21910454e 75 #define Ti 0.0
Dannis_mbed 0:81b21910454e 76 #define Td 0.0
Dannis_mbed 0:81b21910454e 77 #define RATE 0.01
Dannis_mbed 0:81b21910454e 78
Dannis_mbed 0:81b21910454e 79 enum MSG_t
Dannis_mbed 0:81b21910454e 80 {
Dannis_mbed 0:81b21910454e 81 MSG1 = 0, // message 1
Dannis_mbed 0:81b21910454e 82 MSG2, // message 2
Dannis_mbed 0:81b21910454e 83 MSG3 // message 3
Dannis_mbed 0:81b21910454e 84 };
Dannis_mbed 0:81b21910454e 85
Dannis_mbed 0:81b21910454e 86 //////////////////////////////////////////////////////////////////////////////////////
Dannis_mbed 0:81b21910454e 87 // port declaration //
Dannis_mbed 0:81b21910454e 88 //////////////////////////////////////////////////////////////////////////////////////
Dannis_mbed 0:81b21910454e 89
Dannis_mbed 0:81b21910454e 90 DigitalOut myLed(LED1);
Dannis_mbed 0:81b21910454e 91 DigitalOut myLed2(LED2);
Dannis_mbed 0:81b21910454e 92
Dannis_mbed 0:81b21910454e 93 DigitalOut aliveLed(p23);
Dannis_mbed 0:81b21910454e 94 DigitalOut statusLed(p26);
Dannis_mbed 0:81b21910454e 95 DigitalOut emergencyLed(p25);
Dannis_mbed 0:81b21910454e 96 DigitalOut motorRelais(p20);
Dannis_mbed 0:81b21910454e 97
Dannis_mbed 0:81b21910454e 98 AnalogIn currentSensor(p15);
Dannis_mbed 0:81b21910454e 99
Dannis_mbed 0:81b21910454e 100 InterruptIn intAGM(p8);
Dannis_mbed 0:81b21910454e 101
Dannis_mbed 0:81b21910454e 102 I2C i2c(p9, p10);
Dannis_mbed 0:81b21910454e 103 GPS gps(p13, p14);
Dannis_mbed 0:81b21910454e 104 // I2C Rx, I2C Tx, Int
Dannis_mbed 0:81b21910454e 105 MPU9150 imu(p10, p9, p8);
Dannis_mbed 0:81b21910454e 106 //mRotaryEncoder(PinName pinA, PinName pinB, PinName pinSW, PinMode pullMode=PullUp, int debounceTime_us=1000)
Dannis_mbed 0:81b21910454e 107 mRotaryEncoder rSwitch(p12, p11, p7);
Dannis_mbed 0:81b21910454e 108 CAN CANbus(p30, p29);
Dannis_mbed 0:81b21910454e 109 Serial debug(USBTX, USBRX);
Dannis_mbed 0:81b21910454e 110
Dannis_mbed 0:81b21910454e 111 PID rollPID(Kc,Ti,Td,RATE); // declare a PID for roll adjustment
Dannis_mbed 0:81b21910454e 112
Dannis_mbed 2:f6d058931b17 113 LCD_I2C LCD(p9, p10, 0x40);
Dannis_mbed 2:f6d058931b17 114
Dannis_mbed 0:81b21910454e 115 //////////////////////////////////////////////////////////////////////////////////////
Dannis_mbed 0:81b21910454e 116 // function prototypes //
Dannis_mbed 0:81b21910454e 117 //////////////////////////////////////////////////////////////////////////////////////
Dannis_mbed 0:81b21910454e 118
Dannis_mbed 0:81b21910454e 119
Dannis_mbed 0:81b21910454e 120 //////////////////////////////////////////////////////////////////////////////////////
Dannis_mbed 0:81b21910454e 121 // global variables //
Dannis_mbed 0:81b21910454e 122 //////////////////////////////////////////////////////////////////////////////////////
Dannis_mbed 0:81b21910454e 123
Dannis_mbed 0:81b21910454e 124 int i = 0;
Dannis_mbed 0:81b21910454e 125 int value = 0;
Dannis_mbed 0:81b21910454e 126 int lastGet;
Dannis_mbed 0:81b21910454e 127 int thisGet;
Dannis_mbed 0:81b21910454e 128 int iFilter;
Dannis_mbed 0:81b21910454e 129 uint32_t uiFilterId;
Dannis_mbed 1:b4a0d63db637 130
Dannis_mbed 0:81b21910454e 131
Dannis_mbed 0:81b21910454e 132 bool enc_pressed = false; // Button of rotaryencoder was pressed
Dannis_mbed 0:81b21910454e 133 bool enc_rotated = false; // rotary encoder was totaded left or right
Dannis_mbed 0:81b21910454e 134 bool bSystemFail = false; // sytem fail
Dannis_mbed 0:81b21910454e 135
Dannis_mbed 0:81b21910454e 136 char cBuffer[200];
Dannis_mbed 0:81b21910454e 137 char cMessage;
Dannis_mbed 0:81b21910454e 138 char text[16];
Dannis_mbed 1:b4a0d63db637 139 char cPTurns[2] ={0};
Dannis_mbed 1:b4a0d63db637 140 char cSTurns[2] ={0};
Dannis_mbed 0:81b21910454e 141
Dannis_mbed 0:81b21910454e 142 float fRoll, fPitch, fEulerRoll;
Dannis_mbed 0:81b21910454e 143 float fRollDiff;
Dannis_mbed 0:81b21910454e 144 float fPreviousTime;
Dannis_mbed 0:81b21910454e 145
Dannis_mbed 0:81b21910454e 146 double dCurrent = 0.0;
Dannis_mbed 0:81b21910454e 147 double dRefCurrent = 0.0;
Dannis_mbed 0:81b21910454e 148
Dannis_mbed 0:81b21910454e 149 Quaternion q1;
Dannis_mbed 0:81b21910454e 150
Dannis_mbed 0:81b21910454e 151 Ticker CANTicker;
Dannis_mbed 0:81b21910454e 152 Ticker tAliveOn;
Dannis_mbed 0:81b21910454e 153 Ticker tEmergencyLedOn;
Dannis_mbed 0:81b21910454e 154 Ticker tMPU;
Dannis_mbed 0:81b21910454e 155
Dannis_mbed 0:81b21910454e 156 Timeout tAliveOff;
Dannis_mbed 0:81b21910454e 157 Timeout tEmergencyLedOff;
Dannis_mbed 0:81b21910454e 158 Timer actualTime;
Dannis_mbed 0:81b21910454e 159 Timer timer;
Dannis_mbed 0:81b21910454e 160
Dannis_mbed 0:81b21910454e 161 Menu mLCDMenu;
Dannis_mbed 0:81b21910454e 162 SystemVar svSoF;
Dannis_mbed 0:81b21910454e 163
Dannis_mbed 0:81b21910454e 164 MSG_t messageToSend;
Dannis_mbed 0:81b21910454e 165 //////////////////////////////////////////////////////////////////////////////////////
Dannis_mbed 0:81b21910454e 166 // main //
Dannis_mbed 0:81b21910454e 167 //////////////////////////////////////////////////////////////////////////////////////
Dannis_mbed 0:81b21910454e 168
Dannis_mbed 0:81b21910454e 169 int main()
Dannis_mbed 0:81b21910454e 170 {
Dannis_mbed 0:81b21910454e 171 //////////////////////////////////////////////////////////////////////////////////////
Dannis_mbed 0:81b21910454e 172 // init //
Dannis_mbed 0:81b21910454e 173 //////////////////////////////////////////////////////////////////////////////////////
Dannis_mbed 0:81b21910454e 174 vInit();
Dannis_mbed 0:81b21910454e 175 // init CANbus
Dannis_mbed 0:81b21910454e 176 vInitCANBus();
Dannis_mbed 0:81b21910454e 177 // init Gyro
Dannis_mbed 0:81b21910454e 178 vInitImu();
Dannis_mbed 0:81b21910454e 179
Dannis_mbed 0:81b21910454e 180
Dannis_mbed 0:81b21910454e 181 CANMessage msg;
Dannis_mbed 0:81b21910454e 182 messageToSend = MSG1;
Dannis_mbed 0:81b21910454e 183
Dannis_mbed 0:81b21910454e 184
Dannis_mbed 0:81b21910454e 185 //acquiring v_ref
Dannis_mbed 0:81b21910454e 186 for (int i=0; i<NSAMPLES; i++)
Dannis_mbed 0:81b21910454e 187 {
Dannis_mbed 0:81b21910454e 188 dRefCurrent += currentSensor;
Dannis_mbed 0:81b21910454e 189 }
Dannis_mbed 0:81b21910454e 190 dRefCurrent /= NSAMPLES;
Dannis_mbed 0:81b21910454e 191 //debug.printf("RefCurrent : %f\n\r", dRefCurrent);
Dannis_mbed 0:81b21910454e 192
Dannis_mbed 0:81b21910454e 193 timer.start(); //timer to test imu roll angle
Dannis_mbed 0:81b21910454e 194 actualTime.start(); //crash timer
Dannis_mbed 0:81b21910454e 195 // clear line -1-
Dannis_mbed 2:f6d058931b17 196 LCD.vLCD_printPos_I2C((unsigned char*)" ", 1, 1);
Dannis_mbed 0:81b21910454e 197
Dannis_mbed 0:81b21910454e 198 //////////////////////////////////////////////////////////////////////////////////////
Dannis_mbed 0:81b21910454e 199 // endless loop //
Dannis_mbed 0:81b21910454e 200 //////////////////////////////////////////////////////////////////////////////////////
Dannis_mbed 0:81b21910454e 201 while (1)
Dannis_mbed 0:81b21910454e 202 {
Dannis_mbed 0:81b21910454e 203 // rotary switch has been rotated?
Dannis_mbed 0:81b21910454e 204 if (enc_rotated) vSwitchRotated();
Dannis_mbed 0:81b21910454e 205
Dannis_mbed 0:81b21910454e 206 // rotary switch confirm pressed?
Dannis_mbed 0:81b21910454e 207 if (enc_pressed) vSwitchConfirmed();
Dannis_mbed 0:81b21910454e 208
Dannis_mbed 1:b4a0d63db637 209 // answer CAN
Dannis_mbed 1:b4a0d63db637 210 if(CANbus.read(msg)){
Dannis_mbed 1:b4a0d63db637 211 if (msg.id == EXT_UI_HEIGHT) svSoF.vVarHeightFoilBorne((uint32_t) msg.data[0]);
Dannis_mbed 1:b4a0d63db637 212 }
Dannis_mbed 0:81b21910454e 213 if(CANbus.read(msg)){
Dannis_mbed 1:b4a0d63db637 214 debug.printf("ID: %i.\t", msg.id);
Dannis_mbed 1:b4a0d63db637 215 if ((msg.id == HEIGHT_PORT_DATA)&&(mLCDMenu.bGetReadHeight() == true))
Dannis_mbed 1:b4a0d63db637 216 {
Dannis_mbed 1:b4a0d63db637 217 svSoF.vSetPHeight((uint32_t) msg.data[0]);
Dannis_mbed 1:b4a0d63db637 218 debug.printf("height received: %c cm\r\n", msg.data[0]);
Dannis_mbed 1:b4a0d63db637 219 }
Dannis_mbed 1:b4a0d63db637 220 //if ((msg.id == ACTUATOR_PORT_ID)&&(msg.data[0] == 0xFF)) statusLed = ! statusLed;
Dannis_mbed 0:81b21910454e 221 //debug.printf("ID#102 received: %d answer.\r\n", msg.data[0]);
Dannis_mbed 0:81b21910454e 222 }
Dannis_mbed 0:81b21910454e 223
Dannis_mbed 1:b4a0d63db637 224 if(timer.read_ms() > 250)
Dannis_mbed 0:81b21910454e 225 {
Dannis_mbed 0:81b21910454e 226 timer.reset();
Dannis_mbed 1:b4a0d63db637 227 if(svSoF.uiGetRoll() >= 4)
Dannis_mbed 1:b4a0d63db637 228 {
Dannis_mbed 1:b4a0d63db637 229 if(svSoF.iGetRollPolarity() == 0)
Dannis_mbed 1:b4a0d63db637 230 {
Dannis_mbed 1:b4a0d63db637 231 cPTurns[1] = 1;
Dannis_mbed 1:b4a0d63db637 232 cSTurns[1] = 0;
Dannis_mbed 1:b4a0d63db637 233 }
Dannis_mbed 1:b4a0d63db637 234 else
Dannis_mbed 1:b4a0d63db637 235 {
Dannis_mbed 1:b4a0d63db637 236 cPTurns[1] = 0;
Dannis_mbed 1:b4a0d63db637 237 cSTurns[1] = 1;
Dannis_mbed 1:b4a0d63db637 238 }
Dannis_mbed 1:b4a0d63db637 239 cPTurns[0] = 1;
Dannis_mbed 1:b4a0d63db637 240 cSTurns[0] = 1;
Dannis_mbed 1:b4a0d63db637 241
Dannis_mbed 1:b4a0d63db637 242 CANbus.write(CANMessage(ACTUATOR_PORT_RUN, cPTurns, 2)); // send message to port actuator
Dannis_mbed 1:b4a0d63db637 243 wait(0.15);
Dannis_mbed 1:b4a0d63db637 244 CANbus.write(CANMessage(ACTUATOR_STARB_RUN, cSTurns, 2)); // send message to port actuator
Dannis_mbed 1:b4a0d63db637 245
Dannis_mbed 1:b4a0d63db637 246 }//end if
Dannis_mbed 0:81b21910454e 247 svSoF.vSetPitch(q1); // call function class SystemVar
Dannis_mbed 1:b4a0d63db637 248 svSoF.vSetRoll(q1);
Dannis_mbed 1:b4a0d63db637 249 mLCDMenu.vShowScreen(mLCDMenu.getScreen());
Dannis_mbed 2:f6d058931b17 250 LCD.vLCD_update();
Dannis_mbed 0:81b21910454e 251
Dannis_mbed 0:81b21910454e 252 //debug.printf("Roll: %c\t", svSoF.getValue());
Dannis_mbed 0:81b21910454e 253 //debug.printf("Roll: %f\t", getRollAngle(q1));
Dannis_mbed 0:81b21910454e 254 //debug.printf("Pitch: %f\t", getPitchAngle(q1));
Dannis_mbed 0:81b21910454e 255 //debug.printf("Time: %f\r\n", actualTime.read());
Dannis_mbed 0:81b21910454e 256 }
Dannis_mbed 0:81b21910454e 257 /*for (int samples = 0; samples<NSAMPLES; samples++) dCurrent += currentSensor;
Dannis_mbed 0:81b21910454e 258
Dannis_mbed 0:81b21910454e 259 dCurrent /= NSAMPLES;
Dannis_mbed 0:81b21910454e 260 //debug.printf("RefCurrent : %f\n\r", dRefCurrent);
Dannis_mbed 0:81b21910454e 261 //debug.printf("dCurrent : %f\n\r", dCurrent);
Dannis_mbed 0:81b21910454e 262 dCurrent = (dCurrent-dRefCurrent)*V_MAX/0.185;
Dannis_mbed 0:81b21910454e 263
Dannis_mbed 0:81b21910454e 264 debug.printf("Current mA : %f\n\r", (dCurrent*1000));*/
Dannis_mbed 0:81b21910454e 265 wait(0.2);
Dannis_mbed 0:81b21910454e 266
Dannis_mbed 0:81b21910454e 267 }
Dannis_mbed 0:81b21910454e 268
Dannis_mbed 0:81b21910454e 269 } // END MAIN
Dannis_mbed 0:81b21910454e 270
Dannis_mbed 0:81b21910454e 271 //////////////////////////////////////////////////////////////////////////////////////
Dannis_mbed 0:81b21910454e 272 // Init SOF module //
Dannis_mbed 0:81b21910454e 273 //////////////////////////////////////////////////////////////////////////////////////
Dannis_mbed 0:81b21910454e 274 void vInit (void)
Dannis_mbed 0:81b21910454e 275 {
Dannis_mbed 0:81b21910454e 276 // I2C init
Dannis_mbed 0:81b21910454e 277 i2c.frequency(100000);
Dannis_mbed 0:81b21910454e 278 //Port A is databus - Output
Dannis_mbed 2:f6d058931b17 279 // mcp23017.direction(PORT_A, PORT_DIR_OUT);
Dannis_mbed 0:81b21910454e 280 //Port B is controlbus - Output
Dannis_mbed 2:f6d058931b17 281 // mcp23017.direction(PORT_B, PORT_DIR_OUT);
Dannis_mbed 0:81b21910454e 282
Dannis_mbed 0:81b21910454e 283 debug.baud(115200);
Dannis_mbed 0:81b21910454e 284 // initialize LCD
Dannis_mbed 2:f6d058931b17 285 //vLCD_init_I2C();
Dannis_mbed 1:b4a0d63db637 286 mLCDMenu.vSelectMenu(_MENU0_0);
Dannis_mbed 2:f6d058931b17 287 LCD.vLCD_update();
Dannis_mbed 0:81b21910454e 288
Dannis_mbed 0:81b21910454e 289 // led's out
Dannis_mbed 0:81b21910454e 290 aliveLed = 1; wait(0.1); aliveLed = 0;
Dannis_mbed 0:81b21910454e 291 statusLed = 1; wait(0.1); statusLed = 0;
Dannis_mbed 0:81b21910454e 292 emergencyLed = 1; wait(0.1); emergencyLed = 0;
Dannis_mbed 0:81b21910454e 293
Dannis_mbed 0:81b21910454e 294 wait(0.2);
Dannis_mbed 0:81b21910454e 295 motorRelais = 1;
Dannis_mbed 0:81b21910454e 296 wait(1.0);
Dannis_mbed 0:81b21910454e 297 motorRelais = 0;
Dannis_mbed 0:81b21910454e 298
Dannis_mbed 0:81b21910454e 299 //Int-Handler rotary encoder switch
Dannis_mbed 0:81b21910454e 300 // call trigger_sw() when button of rotary-encoder is pressed
Dannis_mbed 0:81b21910454e 301 rSwitch.attachSW(&trigger_sw);
Dannis_mbed 0:81b21910454e 302 // call trigger_rot() when the shaft is rotaded left or right
Dannis_mbed 0:81b21910454e 303 rSwitch.attachROT(&trigger_rotated);
Dannis_mbed 0:81b21910454e 304 lastGet = 0;
Dannis_mbed 0:81b21910454e 305 // set encrotated, so position is displayed on startup
Dannis_mbed 0:81b21910454e 306 enc_rotated = true;
Dannis_mbed 0:81b21910454e 307 // blink alive LED
Dannis_mbed 0:81b21910454e 308 aliveLed = 1;
Dannis_mbed 0:81b21910454e 309 tAliveOff.attach(&vBlinkOnce_cb, 0.05);
Dannis_mbed 0:81b21910454e 310 tAliveOn.attach(&vBlinkOnce_cb, 5);
Dannis_mbed 0:81b21910454e 311 }
Dannis_mbed 0:81b21910454e 312
Dannis_mbed 0:81b21910454e 313 //////////////////////////////////////////////////////////////////////////////////////
Dannis_mbed 0:81b21910454e 314 // Init MPU 9150 //
Dannis_mbed 0:81b21910454e 315 //////////////////////////////////////////////////////////////////////////////////////
Dannis_mbed 0:81b21910454e 316 void vInitImu (void)
Dannis_mbed 0:81b21910454e 317 {
Dannis_mbed 1:b4a0d63db637 318 if(imu.isReady()) mLCDMenu.vShowScreen(_IMUREADY);
Dannis_mbed 1:b4a0d63db637 319 else mLCDMenu.vShowScreen(_IMUFAIL);
Dannis_mbed 2:f6d058931b17 320 LCD.vLCD_update();
Dannis_mbed 0:81b21910454e 321
Dannis_mbed 0:81b21910454e 322 imu.initialiseDMP();
Dannis_mbed 0:81b21910454e 323 imu.setFifoReset(true);
Dannis_mbed 0:81b21910454e 324 imu.setDMPEnabled(true);
Dannis_mbed 0:81b21910454e 325 intAGM.rise(&vGetMPUBuffer);
Dannis_mbed 0:81b21910454e 326 //tMPU.attach_us(&vGetMPUBuffer,50000);
Dannis_mbed 0:81b21910454e 327 }
Dannis_mbed 0:81b21910454e 328
Dannis_mbed 0:81b21910454e 329 //////////////////////////////////////////////////////////////////////////////////////
Dannis_mbed 0:81b21910454e 330 // Init CANbus //
Dannis_mbed 0:81b21910454e 331 //////////////////////////////////////////////////////////////////////////////////////
Dannis_mbed 0:81b21910454e 332 void vInitCANBus (void)
Dannis_mbed 0:81b21910454e 333 {
Dannis_mbed 0:81b21910454e 334 CANbus.frequency(250000);
Dannis_mbed 0:81b21910454e 335 debug.printf("vInitCANBus\r\n");
Dannis_mbed 0:81b21910454e 336 CANMessage msg;
Dannis_mbed 0:81b21910454e 337 int i, iTry = 10;
Dannis_mbed 0:81b21910454e 338 uint32_t id = MESSAGE_ALL;
Dannis_mbed 0:81b21910454e 339 bool bReceived = false;
Dannis_mbed 0:81b21910454e 340 cMessage = 0x00;
Dannis_mbed 0:81b21910454e 341 // check CAN system
Dannis_mbed 0:81b21910454e 342 for (i = 6; i > 0 ; i--)
Dannis_mbed 0:81b21910454e 343 {
Dannis_mbed 0:81b21910454e 344 debug.printf("for loop: %i\r\n", i);
Dannis_mbed 0:81b21910454e 345 bReceived = false;
Dannis_mbed 0:81b21910454e 346 id++; // increase id
Dannis_mbed 0:81b21910454e 347 while ((iTry > 0)&&(!bReceived))
Dannis_mbed 0:81b21910454e 348 {
Dannis_mbed 0:81b21910454e 349 CANbus.write(CANMessage(id, &cMessage, 1));
Dannis_mbed 0:81b21910454e 350 wait(0.15);
Dannis_mbed 0:81b21910454e 351 switch(id){ // set adress to filter
Dannis_mbed 0:81b21910454e 352 case PORT_ACT_DIAGN: uiFilterId = ACTUATOR_PORT_ID; break;
Dannis_mbed 0:81b21910454e 353 case STARB_ACT_DIAGN: uiFilterId = ACTUATOR_STARB_ID; break;
Dannis_mbed 0:81b21910454e 354 case PORT_HGHT_DIAGN: uiFilterId = HEIGHT_PORT_ID; break;
Dannis_mbed 0:81b21910454e 355 case STARB_HGHT_DIAGN: uiFilterId = HEIGHT_STARB_ID; break;
Dannis_mbed 0:81b21910454e 356 case GYRO_GPS_DIAGN: uiFilterId = GYRO_GPS_ID; break;
Dannis_mbed 0:81b21910454e 357 case EXT_UI_DIAGN: uiFilterId = EXT_UI_ID; break;
Dannis_mbed 0:81b21910454e 358 }//end switch
Dannis_mbed 0:81b21910454e 359 iFilter = CANbus.filter(0x200, 0x7FF, CANStandard);
Dannis_mbed 0:81b21910454e 360 if(CANbus.read(msg, iFilter)){
Dannis_mbed 0:81b21910454e 361 debug.printf("can.read(msg) ID: %i\r\n", id);
Dannis_mbed 0:81b21910454e 362 debug.printf("can.msg id: %i, %i\r\n", msg.id, msg.data[0]);
Dannis_mbed 0:81b21910454e 363 switch(id){
Dannis_mbed 1:b4a0d63db637 364 case PORT_ACT_DIAGN: if((msg.id == ACTUATOR_PORT_ID)&&(msg.data[0] == 0xFF)) mLCDMenu.vShowScreen(_CANID101OK); break;
Dannis_mbed 1:b4a0d63db637 365 case STARB_ACT_DIAGN: if((msg.id == ACTUATOR_STARB_ID)&&(msg.data[0] == 0xFF)) mLCDMenu.vShowScreen(_CANID102OK); break;
Dannis_mbed 1:b4a0d63db637 366 case PORT_HGHT_DIAGN: if((msg.id == HEIGHT_PORT_ID)&&(msg.data[0] == 0xFF)) mLCDMenu.vShowScreen(_CANID201OK); break;
Dannis_mbed 1:b4a0d63db637 367 case STARB_HGHT_DIAGN: if((msg.id == HEIGHT_STARB_ID)&&(msg.data[0] == 0xFF)) mLCDMenu.vShowScreen(_CANID202OK); break;
Dannis_mbed 1:b4a0d63db637 368 case GYRO_GPS_DIAGN: if((msg.id == GYRO_GPS_ID)&&(msg.data[0] == 0xFF)) mLCDMenu.vShowScreen(_CANID205OK); break;
Dannis_mbed 1:b4a0d63db637 369 case EXT_UI_DIAGN: if((msg.id == EXT_UI_ID)&&(msg.data[0] == 0xFF)) mLCDMenu.vShowScreen(_CANID1001OK); break;
Dannis_mbed 0:81b21910454e 370 } //end switch
Dannis_mbed 2:f6d058931b17 371 LCD.vLCD_update();
Dannis_mbed 0:81b21910454e 372 bReceived = true;
Dannis_mbed 0:81b21910454e 373 } //end if
Dannis_mbed 0:81b21910454e 374 else if(iTry == 1){
Dannis_mbed 0:81b21910454e 375 switch(id){
Dannis_mbed 1:b4a0d63db637 376 case PORT_ACT_DIAGN: mLCDMenu.vShowScreen(_CANID101FAIL); break;
Dannis_mbed 1:b4a0d63db637 377 case STARB_ACT_DIAGN: mLCDMenu.vShowScreen(_CANID102FAIL); break;
Dannis_mbed 1:b4a0d63db637 378 case PORT_HGHT_DIAGN: mLCDMenu.vShowScreen(_CANID201FAIL); break;
Dannis_mbed 1:b4a0d63db637 379 case STARB_HGHT_DIAGN: mLCDMenu.vShowScreen(_CANID202FAIL); break;
Dannis_mbed 1:b4a0d63db637 380 case GYRO_GPS_DIAGN: mLCDMenu.vShowScreen(_CANID205FAIL); break;
Dannis_mbed 1:b4a0d63db637 381 case EXT_UI_DIAGN: mLCDMenu.vShowScreen(_CANID1001FAIL); break;
Dannis_mbed 0:81b21910454e 382 } //end switch
Dannis_mbed 2:f6d058931b17 383 LCD.vLCD_update();
Dannis_mbed 0:81b21910454e 384 } //end if
Dannis_mbed 0:81b21910454e 385 if(CANbus.read(msg)){}//unwanted message
Dannis_mbed 0:81b21910454e 386 if(!bReceived) iTry--;
Dannis_mbed 0:81b21910454e 387 debug.printf("Tries: %i\r\n", iTry);
Dannis_mbed 0:81b21910454e 388 } //end while
Dannis_mbed 0:81b21910454e 389 wait(0.5); // some time to read message
Dannis_mbed 0:81b21910454e 390 iTry = 10; // reset try counter
Dannis_mbed 0:81b21910454e 391 // *****afhandelen can fail ****
Dannis_mbed 0:81b21910454e 392 if (iTry != 0) iTry = 10;
Dannis_mbed 0:81b21910454e 393 else bSystemFail = true;
Dannis_mbed 0:81b21910454e 394 } //end for
Dannis_mbed 0:81b21910454e 395
Dannis_mbed 0:81b21910454e 396 // in case system fail
Dannis_mbed 0:81b21910454e 397 if(bSystemFail){
Dannis_mbed 0:81b21910454e 398 vCallEmergencyLed();
Dannis_mbed 1:b4a0d63db637 399 mLCDMenu.vShowScreen(_CANNORESPONSE);
Dannis_mbed 1:b4a0d63db637 400 }
Dannis_mbed 1:b4a0d63db637 401 cMessage = (char) svSoF.iGetHeightFoilBorne();
Dannis_mbed 1:b4a0d63db637 402 CANbus.write(CANMessage(EXT_UI_HEIGHT, &cMessage, 1));
Dannis_mbed 1:b4a0d63db637 403
Dannis_mbed 1:b4a0d63db637 404 mLCDMenu.vSelectMenu(_MENU0_0);
Dannis_mbed 2:f6d058931b17 405 LCD.vLCD_update();
Dannis_mbed 0:81b21910454e 406 debug.printf("end can init");
Dannis_mbed 0:81b21910454e 407 }
Dannis_mbed 0:81b21910454e 408
Dannis_mbed 0:81b21910454e 409 // call vCANBusSend every 1 second test
Dannis_mbed 0:81b21910454e 410 // CANTicker.attach(&vCANBusSend, 1);
Dannis_mbed 0:81b21910454e 411
Dannis_mbed 0:81b21910454e 412 //////////////////////////////////////////////////////////////////////////////////////
Dannis_mbed 0:81b21910454e 413 // Blink alive led //
Dannis_mbed 0:81b21910454e 414 //////////////////////////////////////////////////////////////////////////////////////
Dannis_mbed 0:81b21910454e 415 void vBlinkOff_cb (void)
Dannis_mbed 0:81b21910454e 416 {
Dannis_mbed 0:81b21910454e 417 aliveLed = 0;
Dannis_mbed 0:81b21910454e 418 }
Dannis_mbed 0:81b21910454e 419
Dannis_mbed 0:81b21910454e 420 //////////////////////////////////////////////////////////////////////////////////////
Dannis_mbed 0:81b21910454e 421 // Blink emergecy led //
Dannis_mbed 0:81b21910454e 422 //////////////////////////////////////////////////////////////////////////////////////
Dannis_mbed 0:81b21910454e 423 void vEmergencyOff_cb (void)
Dannis_mbed 0:81b21910454e 424 {
Dannis_mbed 0:81b21910454e 425 emergencyLed = 0;
Dannis_mbed 0:81b21910454e 426 }
Dannis_mbed 0:81b21910454e 427
Dannis_mbed 0:81b21910454e 428 //////////////////////////////////////////////////////////////////////////////////////
Dannis_mbed 0:81b21910454e 429 // Blink status led once //
Dannis_mbed 0:81b21910454e 430 //////////////////////////////////////////////////////////////////////////////////////
Dannis_mbed 0:81b21910454e 431 void vBlinkOnce_cb (void)
Dannis_mbed 0:81b21910454e 432 {
Dannis_mbed 0:81b21910454e 433 aliveLed = 1;
Dannis_mbed 0:81b21910454e 434 tAliveOff.detach();
Dannis_mbed 0:81b21910454e 435 tAliveOff.attach(&vBlinkOff_cb, 0.05);
Dannis_mbed 0:81b21910454e 436 }
Dannis_mbed 0:81b21910454e 437
Dannis_mbed 0:81b21910454e 438 //////////////////////////////////////////////////////////////////////////////////////
Dannis_mbed 0:81b21910454e 439 // Blink emergency led once //
Dannis_mbed 0:81b21910454e 440 //////////////////////////////////////////////////////////////////////////////////////
Dannis_mbed 0:81b21910454e 441 void vEmergencyOnce_cb (void)
Dannis_mbed 0:81b21910454e 442 {
Dannis_mbed 0:81b21910454e 443 emergencyLed = 1;
Dannis_mbed 0:81b21910454e 444 tEmergencyLedOff.detach();
Dannis_mbed 0:81b21910454e 445 tEmergencyLedOff.attach(&vEmergencyOff_cb, 0.05);
Dannis_mbed 0:81b21910454e 446
Dannis_mbed 0:81b21910454e 447 if (!bSystemFail)
Dannis_mbed 0:81b21910454e 448 {
Dannis_mbed 0:81b21910454e 449 tEmergencyLedOn.detach();
Dannis_mbed 0:81b21910454e 450 tEmergencyLedOff.detach();
Dannis_mbed 0:81b21910454e 451 aliveLed = 0;
Dannis_mbed 0:81b21910454e 452 }
Dannis_mbed 0:81b21910454e 453 }
Dannis_mbed 0:81b21910454e 454
Dannis_mbed 0:81b21910454e 455 //////////////////////////////////////////////////////////////////////////////////////
Dannis_mbed 0:81b21910454e 456 // Call emergency led //
Dannis_mbed 0:81b21910454e 457 //////////////////////////////////////////////////////////////////////////////////////
Dannis_mbed 0:81b21910454e 458 void vCallEmergencyLed(void)
Dannis_mbed 0:81b21910454e 459 {
Dannis_mbed 0:81b21910454e 460 aliveLed = 1;
Dannis_mbed 0:81b21910454e 461 tEmergencyLedOff.attach(&vEmergencyOnce_cb, 0.05);
Dannis_mbed 0:81b21910454e 462 tEmergencyLedOn.attach(&vEmergencyOnce_cb, 0.5);
Dannis_mbed 0:81b21910454e 463 }
Dannis_mbed 0:81b21910454e 464
Dannis_mbed 0:81b21910454e 465 //////////////////////////////////////////////////////////////////////////////////////
Dannis_mbed 0:81b21910454e 466 // calculate Roll from quaternion //
Dannis_mbed 0:81b21910454e 467 //////////////////////////////////////////////////////////////////////////////////////
Dannis_mbed 0:81b21910454e 468 float getRollAngle(Quaternion q1)
Dannis_mbed 0:81b21910454e 469 {
Dannis_mbed 0:81b21910454e 470 float fRoll;
Dannis_mbed 0:81b21910454e 471 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);
Dannis_mbed 0:81b21910454e 472 fRoll *= 180/3.14;
Dannis_mbed 0:81b21910454e 473 return (fRoll);
Dannis_mbed 0:81b21910454e 474 }
Dannis_mbed 0:81b21910454e 475
Dannis_mbed 0:81b21910454e 476 //////////////////////////////////////////////////////////////////////////////////////
Dannis_mbed 0:81b21910454e 477 // calculate Pitch from quaternion //
Dannis_mbed 0:81b21910454e 478 //////////////////////////////////////////////////////////////////////////////////////
Dannis_mbed 0:81b21910454e 479 float getPitchAngle(Quaternion q1)
Dannis_mbed 0:81b21910454e 480 {
Dannis_mbed 0:81b21910454e 481 Vector3 euler = q1.getEulerAngles();
Dannis_mbed 0:81b21910454e 482 //debug.printf("$ Pitch = %f #\t", euler.y);
Dannis_mbed 0:81b21910454e 483 float fPitch;
Dannis_mbed 0:81b21910454e 484 //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)))));
Dannis_mbed 0:81b21910454e 485 //fPitch = acos(-(q1.w*q1.w) - (q1.v.x*q1.v.x) + (q1.v.y*q1.v.y) + (q1.v.z*q1.v.z));
Dannis_mbed 0:81b21910454e 486 //fPitch = -asin(2*q1.w*q1.v.y - 2*q1.v.x*q1.v.z);
Dannis_mbed 0:81b21910454e 487 fPitch = euler.y;
Dannis_mbed 0:81b21910454e 488 fPitch *= 180/3.14;
Dannis_mbed 0:81b21910454e 489 return (fPitch);
Dannis_mbed 0:81b21910454e 490
Dannis_mbed 0:81b21910454e 491 }
Dannis_mbed 0:81b21910454e 492
Dannis_mbed 0:81b21910454e 493 //////////////////////////////////////////////////////////////////////////////////////
Dannis_mbed 0:81b21910454e 494 // interrup-Handler for button on rotary-encoder //
Dannis_mbed 0:81b21910454e 495 //////////////////////////////////////////////////////////////////////////////////////
Dannis_mbed 0:81b21910454e 496 void trigger_sw(void)
Dannis_mbed 0:81b21910454e 497 {
Dannis_mbed 0:81b21910454e 498 enc_pressed = true; // just set the flag, rest is done outside isr
Dannis_mbed 0:81b21910454e 499 }
Dannis_mbed 0:81b21910454e 500
Dannis_mbed 0:81b21910454e 501 //////////////////////////////////////////////////////////////////////////////////////
Dannis_mbed 0:81b21910454e 502 // interrup-Handler for rotary-encoder rotation //
Dannis_mbed 0:81b21910454e 503 //////////////////////////////////////////////////////////////////////////////////////
Dannis_mbed 0:81b21910454e 504 void trigger_rotated(void)
Dannis_mbed 0:81b21910454e 505 {
Dannis_mbed 0:81b21910454e 506 enc_rotated = true; // just set the flag, rest is done outside isr
Dannis_mbed 0:81b21910454e 507 }
Dannis_mbed 0:81b21910454e 508
Dannis_mbed 0:81b21910454e 509 //////////////////////////////////////////////////////////////////////////////////////
Dannis_mbed 0:81b21910454e 510 // reset for rotary-encoder switch //
Dannis_mbed 0:81b21910454e 511 //////////////////////////////////////////////////////////////////////////////////////
Dannis_mbed 0:81b21910454e 512 void vResetSwitch (void)
Dannis_mbed 0:81b21910454e 513 {
Dannis_mbed 0:81b21910454e 514 rSwitch.Set(0);
Dannis_mbed 0:81b21910454e 515 }
Dannis_mbed 0:81b21910454e 516
Dannis_mbed 0:81b21910454e 517 //////////////////////////////////////////////////////////////////////////////////////
Dannis_mbed 0:81b21910454e 518 // set for rotary-encoder switch //
Dannis_mbed 0:81b21910454e 519 //////////////////////////////////////////////////////////////////////////////////////
Dannis_mbed 0:81b21910454e 520 void vSetSwitch (int value)
Dannis_mbed 0:81b21910454e 521 {
Dannis_mbed 0:81b21910454e 522 rSwitch.Set(value);
Dannis_mbed 0:81b21910454e 523 }
Dannis_mbed 0:81b21910454e 524
Dannis_mbed 0:81b21910454e 525 //////////////////////////////////////////////////////////////////////////////////////
Dannis_mbed 0:81b21910454e 526 // rotary-encoder switch rotated //
Dannis_mbed 0:81b21910454e 527 //////////////////////////////////////////////////////////////////////////////////////
Dannis_mbed 0:81b21910454e 528 void vSwitchRotated (void)
Dannis_mbed 0:81b21910454e 529 {
Dannis_mbed 0:81b21910454e 530 enc_rotated = false;
Dannis_mbed 1:b4a0d63db637 531
Dannis_mbed 0:81b21910454e 532 // get value from rotary encoder
Dannis_mbed 0:81b21910454e 533 thisGet = rSwitch.Get();
Dannis_mbed 1:b4a0d63db637 534
Dannis_mbed 1:b4a0d63db637 535 if(rSwitch.Get()<0) mLCDMenu.vRotaryDown();
Dannis_mbed 1:b4a0d63db637 536 else if(rSwitch.Get()>=0) mLCDMenu.vRotaryUp();
Dannis_mbed 1:b4a0d63db637 537
Dannis_mbed 1:b4a0d63db637 538 // reset switch count
Dannis_mbed 1:b4a0d63db637 539 vResetSwitch();
Dannis_mbed 2:f6d058931b17 540 LCD.vLCD_update();
Dannis_mbed 0:81b21910454e 541 }
Dannis_mbed 0:81b21910454e 542
Dannis_mbed 0:81b21910454e 543 //////////////////////////////////////////////////////////////////////////////////////
Dannis_mbed 0:81b21910454e 544 // rotary-encoder switch confirmed //
Dannis_mbed 0:81b21910454e 545 //////////////////////////////////////////////////////////////////////////////////////
Dannis_mbed 0:81b21910454e 546 void vSwitchConfirmed (void)
Dannis_mbed 0:81b21910454e 547 {
Dannis_mbed 0:81b21910454e 548 enc_pressed = false;
Dannis_mbed 0:81b21910454e 549
Dannis_mbed 1:b4a0d63db637 550 mLCDMenu.vRotaryConfirm();
Dannis_mbed 1:b4a0d63db637 551
Dannis_mbed 0:81b21910454e 552 // reset switch count
Dannis_mbed 0:81b21910454e 553 vResetSwitch();
Dannis_mbed 1:b4a0d63db637 554
Dannis_mbed 2:f6d058931b17 555 LCD.vLCD_update();
Dannis_mbed 0:81b21910454e 556 }
Dannis_mbed 0:81b21910454e 557
Dannis_mbed 0:81b21910454e 558 //////////////////////////////////////////////////////////////////////////////////////
Dannis_mbed 0:81b21910454e 559 // CANbus send not used //
Dannis_mbed 0:81b21910454e 560 //////////////////////////////////////////////////////////////////////////////////////
Dannis_mbed 0:81b21910454e 561 void vCANBusSend(void)
Dannis_mbed 0:81b21910454e 562 {
Dannis_mbed 0:81b21910454e 563 static char counter = 0;
Dannis_mbed 0:81b21910454e 564 static char counterTwo = 0;
Dannis_mbed 0:81b21910454e 565 static char random[8] = {0};
Dannis_mbed 0:81b21910454e 566 //vLCD_printPos_I2C((unsigned char*)"CANbus send ", 2, 1);
Dannis_mbed 0:81b21910454e 567 switch(messageToSend){
Dannis_mbed 0:81b21910454e 568 case MSG1:
Dannis_mbed 0:81b21910454e 569 if(CANbus.write(CANMessage(1336, &counter, 1)))
Dannis_mbed 0:81b21910454e 570 {
Dannis_mbed 0:81b21910454e 571 counter++;
Dannis_mbed 0:81b21910454e 572 messageToSend = MSG2;
Dannis_mbed 0:81b21910454e 573 break;
Dannis_mbed 0:81b21910454e 574 }
Dannis_mbed 0:81b21910454e 575 case MSG2:
Dannis_mbed 0:81b21910454e 576 if(CANbus.write(CANMessage(1003, &counterTwo, 1)))
Dannis_mbed 0:81b21910454e 577 {
Dannis_mbed 0:81b21910454e 578 counterTwo++;
Dannis_mbed 0:81b21910454e 579 messageToSend = MSG3;
Dannis_mbed 0:81b21910454e 580 break;
Dannis_mbed 0:81b21910454e 581 }
Dannis_mbed 0:81b21910454e 582 case MSG3:
Dannis_mbed 0:81b21910454e 583 if(CANbus.write(CANMessage(1236, random, 4)))
Dannis_mbed 0:81b21910454e 584 {
Dannis_mbed 0:81b21910454e 585 random[0] = rand()% 10 + 1;
Dannis_mbed 0:81b21910454e 586 random[1] = rand()% 10 + 20;
Dannis_mbed 0:81b21910454e 587 random[2] = rand()% 100 + 1;
Dannis_mbed 0:81b21910454e 588 random[3] = 00;
Dannis_mbed 0:81b21910454e 589 //messageToSend = MSG1;
Dannis_mbed 0:81b21910454e 590 break;
Dannis_mbed 0:81b21910454e 591 }
Dannis_mbed 0:81b21910454e 592 } // end switch case
Dannis_mbed 0:81b21910454e 593
Dannis_mbed 0:81b21910454e 594 myLed = !myLed;
Dannis_mbed 0:81b21910454e 595 }
Dannis_mbed 0:81b21910454e 596
Dannis_mbed 0:81b21910454e 597 //////////////////////////////////////////////////////////////////////////////////////
Dannis_mbed 0:81b21910454e 598 // Get data from MPU fifo buffer //
Dannis_mbed 0:81b21910454e 599 //////////////////////////////////////////////////////////////////////////////////////
Dannis_mbed 0:81b21910454e 600 void vGetMPUBuffer(void)
Dannis_mbed 0:81b21910454e 601 {
Dannis_mbed 0:81b21910454e 602 int iFifoCount;
Dannis_mbed 0:81b21910454e 603 if (imu.getInterruptFifoOverflow())
Dannis_mbed 0:81b21910454e 604 imu.setFifoReset(true);
Dannis_mbed 0:81b21910454e 605 iFifoCount = imu.getFifoCount();
Dannis_mbed 0:81b21910454e 606 //debug.printf("fifocount: %i\r\n", iFifoCount);
Dannis_mbed 0:81b21910454e 607 if(imu.getFifoCount() >= 48){
Dannis_mbed 0:81b21910454e 608 if(iFifoCount % 48 != 0) imu.getFifoBuffer(cBuffer, iFifoCount % 48);
Dannis_mbed 0:81b21910454e 609 //imu.getFifoBuffer(cBuffer, iFifoCount- 48);
Dannis_mbed 0:81b21910454e 610 imu.getFifoBuffer(cBuffer, 48);
Dannis_mbed 0:81b21910454e 611 //myLed2 = !myLed2;
Dannis_mbed 0:81b21910454e 612 q1.decode(cBuffer);// quaternion vector to buffer
Dannis_mbed 0:81b21910454e 613 }
Dannis_mbed 0:81b21910454e 614 }
Dannis_mbed 0:81b21910454e 615
Dannis_mbed 0:81b21910454e 616 //////////////////////////////////////////////////////////////////////////////////////
Dannis_mbed 0:81b21910454e 617 // EOF //
Dannis_mbed 0:81b21910454e 618 //////////////////////////////////////////////////////////////////////////////////////
Dannis_mbed 0:81b21910454e 619
Dannis_mbed 0:81b21910454e 620 /*if (fifoCount == 1024) // test
Dannis_mbed 0:81b21910454e 621 {mpu.resetFIFO();} // reset so we can continue cleanly
Dannis_mbed 0:81b21910454e 622
Dannis_mbed 0:81b21910454e 623 else if (fifoCount >= 42) // otherwise, check for DMP data ready interrupt (this should happen frequently)
Dannis_mbed 0:81b21910454e 624 {
Dannis_mbed 0:81b21910454e 625 if ((fifoCount % stMPU9150->packetSize) != 0) // test
Dannis_mbed 0:81b21910454e 626 {mpu.getFIFOBytes(fifoBuffer, (fifoCount % stMPU9150->packetSize));} // test
Dannis_mbed 0:81b21910454e 627
Dannis_mbed 0:81b21910454e 628 while (mpu.getFIFOCount() >= 42)
Dannis_mbed 0:81b21910454e 629 {mpu.getFIFOBytes(fifoBuffer, stMPU9150->packetSize);} // read a packet from FIFO
Dannis_mbed 0:81b21910454e 630
Dannis_mbed 0:81b21910454e 631 mpu.dmpGetQuaternion(&stMPU9150->q, fifoBuffer);*/