/****************************************************************************/ /* Copyright 2003-2012 MBARI */ /****************************************************************************/ /* Summary : Main Routine for Persistor CF2 to send serial data to MPU6050 */ /* Filename : serMPU.c */ /* Author : Robert Herlien (rah) */ /* Project : BEDS */ /* Revision : 1.0 */ /* Created : 04/18/2012 */ /* */ /* MBARI provides this documentation and code "as is", with no warranty, */ /* express or implied, of its quality or consistency. It is provided without*/ /* support and without obligation on the part of the Monterey Bay Aquarium */ /* Research Institute to assist in its use, correction, modification, or */ /* enhancement. This information should not be published or distributed to */ /* third parties without specific written permission from MBARI. */ /* */ /****************************************************************************/ /* Modification History: */ /* 18apr2012 rah - created */ /* $Log: oasis.c,v $ */ /****************************************************************************/ #include /* MBARI type definitions */ #include /* MBARI constants */ #include /* BEDS general definitions */ #include /* BEDS I/O definitions */ #include /* System logger */ #include // Persistor BIOS and I/O Definitions #include // Persistor PicoDOS Definitions #include #include #include #include #include #include /* From Invensense MotionApps Stack */ #include "log.h" #undef MPL_LOG_TAG #define MPL_LOG_TAG "uMPL-main" // MPL interface #include "ml.h" #include "mldl.h" #include "umpl-states.h" /********************************/ /* Global Data */ /********************************/ Global TUPort *tuport = NULL; Global MBool syslogToConsole = TRUE; /********************************/ /* Module Local Data */ /********************************/ MLocal int loop_delay = 100; MLocal uint packetCount = 0; MLocal TUChParams TUParms = {8, 'N', true, 38400L, tpuAutoPriority, tpuAutoPriority, 256, 256, 512}; /********************************/ /* Forward Declarations */ /********************************/ MLocal void waitForUmplAppBegin(void); MLocal void angleAndAccelSend(void); /************************************************************************/ /* Function : platform_init */ /* Purpose : Initialize hardware-specific platform stuff */ /* Inputs : None */ /* Outputs : None */ /************************************************************************/ MLocal void platform_init(void) { short rxch, txch; ioInit(); /* Initialize I/O system and ticker */ PZCacheSetup(2, calloc, free); sysLogInit(); /* Initialize sysLogger */ SCIRxSetBuffered(true); SCITxSetBuffered(true); TUInit(calloc, free); twimInit(); /* Initialize I2C interface */ rxch = TPUChanFromPin(33); txch = TPUChanFromPin(34); PIOSet(29); // enable MAX322 transmitter (/OFF) PIOClear(30); // enable MAX322 receivers (/EN) /* Open the Aux1 serial port on the R216AU card. Uses TPU12 for Rx, TPU13 for Tx*/ if ((tuport = TUOpen(12, 13, 115200L, &TUParms)) == NULL) { printf("Failure to open comms port. Exiting\n"); exit(1); } } /************************************************************************/ /* Function : setAccelRange */ /* Purpose : Set the full-scale range of the accelerometers */ /* Inputs : FS Range as a float */ /* Outputs : None */ /* Comments : Written by Bob Herlien (rah), MBARI, 13Apr2012 */ /************************************************************************/ MLocal void setAccelRange(float range) { int i; long calMatrix[9]; signed char orientation[9]; inv_get_accel_cal_matrix(calMatrix); for (i = 0; i < 9; i++) orientation[i] = (signed char)(calMatrix[i] & 0xff); inv_set_accel_calibration(range, orientation); } /************************************************************************/ /* Function : umplAppRun */ /* Purpose : Main run routine stolen from AVR UC3 main.c */ /* Inputs : None */ /* Outputs : None */ /************************************************************************/ MLocal void umplAppRun(void) { inv_error_t result = INV_SUCCESS; int_fast8_t processed; MPL_LOGI(INV_VERSION "\n"); sysLogPrintf(INV_VERSION); result = umplInit(NULL); printf("umplInit rtn code = %hd\n", (short)result); if (result != INV_SUCCESS) MPL_LOGE("umplInit Error %d\n",result); sysLogPrintf("umplInit rtn code = %hd", (short)result); result = umplStartMPU(); printf("umplStartMPU rtn code = %hd\n", (short)result); sysLogPrintf("umplStartMPU rtn code = %hd", (short)result); if (result != INV_SUCCESS) MPL_LOGE("umplStartMPU Error %d\n",result); inv_set_no_motion_time(2.0f); /* 2 seconds */ inv_set_no_motion_thresh(2.0f); /* 2 degrees/sec*/ // setAccelRange(16.0f); RTCDelayMicroSeconds(1000000L); /* -------------------- L O O P --------------------- */ for(;;) { RTCDelayMicroSeconds(1000L * (ulong)loop_delay); if (kbhit()) { kbflush(); if (QRconfirm("Abort? ", TRUE, FALSE)) exit(0); } /* MPU-6000 Interrupt * In this sample app, we check the interrupt in the main loop. * However, you can configure your microcontroller to trigger * this interrupt asynchronously. */ if (ioCheckMPUInt()) { umplNotifyInterrupt(INTSRC_MPU); #ifdef VERBOSE_INTERR MPL_LOGI("INT MANTIS\n"); #endif } /* umplOnTimer: * This should be run either at a fixed rate corresponding to * the FIFO Rate (see umpl-setup / inv_set_fifo_rate) or each time * an MPU interrupt has occurred. * It is a time-consuming call, so it is advised not to run it from * an ISR. */ PIOSet(TPU5); umplOnTimer(&processed); PIOClear(TPU5); /* quatSend: * Sends a quaternion packet over the serial port. Used to support * the cube demo in the Debug console, and other apps. (mouse, teapot, etc). */ if (umplGetState() == UMPL_RUN || umplGetState() == UMPL_ACCEL_ONLY || umplGetState() == UMPL_LPACCEL_ONLY) { if (processed) angleAndAccelSend(); } /* umplGetState() == UMPL_RUN */ } /* for (;;) */ } MLocal void angleAndAccelSend(void) { inv_error_t result = INV_ERROR; long longeuler[3]; long longaccelw[3]; long longaccel[3]; long longquat[4]; unsigned char out[38]; memset(out, 0, 38); packetCount++; out[0] = (unsigned char)((packetCount >> 8) & 0xff); out[1] = (unsigned char)(packetCount & 0xff); out[2] = (unsigned char)(inv_get_motion_state() & 0xff); PIOSet(TPU10); #if 0 /* longeuler: send a fixed point representation of the Euler angles * One degree = 2^16. We send the 3 MSBs of each angle, which still * yields a precision of .004 degrees. */ if (inv_get_euler_angles(longeuler) == INV_SUCCESS) { result = INV_SUCCESS; /* If anything works, send packet */ // Send the least significant three bytes of each angle out[3] = (unsigned char) ((uint16_t)(longeuler[0] >> 24) & 0xff); out[4] = (unsigned char) ((uint16_t)(longeuler[0] >> 16) & 0xff); out[5] = (unsigned char) ((uint16_t)(longeuler[0] >> 8) & 0xff); out[6] = (unsigned char) ((uint16_t)(longeuler[1] >> 24) & 0xff); out[7] = (unsigned char) ((uint16_t)(longeuler[1] >> 16) & 0xff); out[8] = (unsigned char) ((uint16_t)(longeuler[1] >> 8) & 0xff); out[9] = (unsigned char) ((uint16_t)(longeuler[2] >> 24) & 0xff); out[10] = (unsigned char) ((uint16_t)(longeuler[2] >> 16) & 0xff); out[11] = (unsigned char) ((uint16_t)(longeuler[2] >> 8) & 0xff); } /* longaccel: send a fixed point representation of the accelerations X,Y,Z * in world coordinates with gravity removed. One g = 2^16. By sending * the LSB 3 bytes for each coord, we can retain full precision while * representing up to 256 g, which is more than the chip can sense. */ if (inv_get_linear_accel(longaccel) == INV_SUCCESS) { result = INV_SUCCESS; /* If anything works, send packet */ // Send the most significant three bytes of each acceleration component. out[12] = (unsigned char) ((uint16_t)(longaccel[0] >> 16) & 0xff); out[13] = (unsigned char) ((uint16_t)(longaccel[0] >> 8) & 0xff); out[14] = (unsigned char) ((uint16_t)(longaccel[0] & 0xff)); out[15] = (unsigned char) ((uint16_t)(longaccel[1] >> 16) & 0xff); out[16] = (unsigned char) ((uint16_t)(longaccel[1] >> 8) & 0xff); out[17] = (unsigned char) ((uint16_t)(longaccel[1] & 0xff)); out[18] = (unsigned char) ((uint16_t)(longaccel[2] >> 16) & 0xff); out[19] = (unsigned char) ((uint16_t)(longaccel[2] >> 8) & 0xff); out[20] = (unsigned char) ((uint16_t)(longaccel[2] & 0xff)); } #endif if (inv_get_linear_accel_in_world(longaccelw) == INV_SUCCESS) { result = INV_SUCCESS; /* If anything works, send packet */ // Send the most significant three bytes of each acceleration component. out[21] = (unsigned char) ((uint16_t)(longaccelw[0] >> 16) & 0xff); out[22] = (unsigned char) ((uint16_t)(longaccelw[0] >> 8) & 0xff); out[23] = (unsigned char) ((uint16_t)(longaccelw[0] & 0xff)); out[24] = (unsigned char) ((uint16_t)(longaccelw[1] >> 16) & 0xff); out[25] = (unsigned char) ((uint16_t)(longaccelw[1] >> 8) & 0xff); out[26] = (unsigned char) ((uint16_t)(longaccelw[1] & 0xff)); out[27] = (unsigned char) ((uint16_t)(longaccelw[2] >> 16) & 0xff); out[28] = (unsigned char) ((uint16_t)(longaccelw[2] >> 8) & 0xff); out[29] = (unsigned char) ((uint16_t)(longaccelw[2] & 0xff)); } /* longquat: send a fixed point representation of the sensor fusion output. * The quaternion behaves well when less significant bits are truncated. * We get the 32 bit element representation with inv_get_quaternion, * and send the most significant 16 bits of each element in the packet. */ if (inv_get_quaternion(longquat) == INV_SUCCESS) { result = INV_SUCCESS; /* If anything works, send packet */ // Send the most significant two bytes of each quaternion component. out[30] = (unsigned char) (longquat[0] >> 24); out[31] = (unsigned char) ((uint16_t)(longquat[0] >> 16) % 256); out[32] = (unsigned char) (longquat[1] >> 24); out[33] = (unsigned char) ((uint16_t)(longquat[1] >> 16) % 256); out[34] = (unsigned char) (longquat[2] >> 24); out[35] = (unsigned char) ((uint16_t)(longquat[2] >> 16) % 256); out[36] = (unsigned char) (longquat[3] >> 24); out[37] = (unsigned char) ((uint16_t)(longquat[3] >> 16) % 256); } PIOClear(TPU10); result = sendPacket(PACKET_TYPE_ANGLE_ACCEL, out); } /************************************************************************/ /* Function : waitForUmplAppBegin */ /* Purpose : Wait for a char on the serial port to begin the app */ /* Inputs : None */ /* Outputs : None */ /************************************************************************/ MLocal void waitForUmplAppBegin(void) { MPL_LOGI("waitForUmplAppBegin\n"); sysLogPrintf("waitForUmplAppBegin"); TUTxPrintf(tuport, "Waiting for input char...\n"); while(TURxQueuedCount(tuport) == 0) if (kbhit()) { kbflush(); if (QRconfirm("Abort? ", TRUE, FALSE)) exit(0); } TURxGetByte(tuport, TRUE); /* Wait for any byte on serial port*/ ioNavPwr(TRUE); /* Turn on power to Invensense */ // RTCDelayMicroSeconds(500000); /* Delay 1/2 second for invense pwrup*/ umplAppRun(); // does not return } /************************************************************************/ /* Function : main */ /* Purpose : Main entry point */ /* Inputs : argc, argv */ /* Outputs : None */ /* Comments : Never returns */ /************************************************************************/ int main(int argc, char **argv) { #pragma unused(argc, argv) // Identify the progam and build printf("\nProgram: %s: %s %s \n", __FILE__, __DATE__, __TIME__); // Identify the device and its firmware printf("Persistor CF%d SN:%ld BIOS:%d.%02d PicoDOS:%d.%02d\n", CFX, BIOSGVT.CFxSerNum, BIOSGVT.BIOSVersion, BIOSGVT.BIOSRelease, BIOSGVT.PICOVersion, BIOSGVT.PICORelease); platform_init(); waitForUmplAppBegin(); /* Should never get here */ BIOSReset(); // full hardware reset return(0); } //____ main() ____//