version 1.0
Dependencies: CMSIS_DSP_401 GPS MPU9150_DMP PID QuaternionMath Servo mbed
Fork of SolarOnFoils_MainModule_20150518 by
main.cpp
- Committer:
- Dannis_mbed
- Date:
- 2015-06-26
- Revision:
- 1:b4a0d63db637
- Parent:
- 0:81b21910454e
- Child:
- 2:f6d058931b17
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);*/