/****************************************************************************/ /* Summary : Motion Library System Layer (mlsl) for Invensense MotionApps */ /* Filename : mlsl_cf2.c */ /* Author : Robert Herlien (rah) */ /* Project : BEDS (Benthic Event Detection System */ /* $Revision$ */ /* Created : 04/30/2012 */ /****************************************************************************/ /* Modification History: */ /* 30apr2012 rah - created */ /****************************************************************************/ #include /* MBARI type definitions */ #include /* MBARI constants */ #include /* BEDS general definitions */ #include /* Persistor BIOS definitions */ #include /* Persistor PicoDOS definitions */ #include #include #include #include /* From mlsl_at32.c */ #include "mpu.h" #if defined CONFIG_MPU_SENSORS_MPU6050A2 # include "mpu6050a2.h" #elif defined CONFIG_MPU_SENSORS_MPU6050B1 # include "mpu6050b1.h" #elif defined CONFIG_MPU_SENSORS_MPU3050 # include "mpu3050.h" #else #error Invalid or undefined CONFIG_MPU_SENSORS_MPUxxxx #endif #include "mlsl.h" #include "mlinclude.h" //#include "stdint_invensense.h" #ifdef DEBUG #define _SerialError(...) MPL_LOGE(__VA_ARGS__) #define _SerialDebug(...) MPL_LOGD(__VA_ARGS__) #else #define _SerialError(...) do {} while(0) #define _SerialDebug(...) do {} while(0) #endif /* End of includes from mlsl_at32.c */ #define SCL_PIN 26 #define SDA_PIN 27 #define PWR_PIN 25 #define DelayHalfus() LoopModeDelay(LoopDelayHalfus) typedef enum {FirstByte = 0, MiddleByte, LastByte} WhichByte; MLocal short LoopDelayHalfus = BIOSGVT.LoopDelay1us/2; void mlslInit(void); /************************************************************************/ /* Function : mlslInit */ /* Purpose : Initialize this module */ /* Inputs : None */ /* Outputs : None */ /************************************************************************/ void mlslInit(void) { PIOSet(SCL_PIN); PIOSet(SDA_PIN); LoopDelayHalfus = BIOSGVT.LoopDelay1us/2; if (LoopDelayHalfus < 1) LoopDelayHalfus = 1; } /* mlslInit() */ /************************************************************************/ /* Function : sendStartBit */ /* Purpose : Send start bit */ /* Inputs : None */ /* Outputs : None */ /************************************************************************/ static void sendStartBit(void) { PinSet(SCL_PIN); PinSet(SDA_PIN); Delay1us(); PinClear(SDA_PIN); Delay1us(); PinClear(SCL_PIN); } /* sendStartBit() */ /************************************************************************/ /* Function : sendStopBit */ /* Purpose : Send stop bit */ /* Inputs : None */ /* Outputs : None */ /************************************************************************/ static void sendStopBit(void) { DelayHalfus(); PinSet(SCL_PIN); Delay1us(); PinSet(SDA_PIN); Delay1us(); } /* sendStopBit() */ /************************************************************************/ /* Function : writeOneByte */ /* Purpose : Write one byte to the MPU6050 */ /* Inputs : Byte to send */ /* Outputs : TRUE if ACKed OK, else FALSE */ /************************************************************************/ static bool writeOneByte(ushort byte) { ushort sendByte, i; bool ackOK = TRUE; for (i = 0, sendByte = byte; i < 8; i++) { DelayHalfus(); /* Data Hold, satisfy clock low time*/ if (sendByte & 0x80) PinSet(SDA_PIN); /* Send next data bit */ else PinClear(SDA_PIN); Delay1us(); PinSet(SCL_PIN); /* Clock high */ Delay1us(); PinClear(SCL_PIN); /* Clock low */ sendByte <<= 1; } DelayHalfus(); /* Data Hold time */ PinRead(SDA_PIN); /* Set data pin as input */ Delay1us(); PinSet(SCL_PIN); /* Clock high */ Delay1us(); if (PinRead(SDA_PIN)!= 0) /* Get ACK */ ackOK = FALSE; Delay1us(); PinClear(SCL_PIN); /* Clock low */ PinClear(SDA_PIN); /* Data pin low */ return(ackOK); } /* writeOneByte() */ /************************************************************************/ /* Function : readOneByte */ /* Purpose : Read one byte from the MPU6050 */ /* Inputs : None */ /* Outputs : Byte read */ /************************************************************************/ static Byte readOneByte(void) { ushort i, readByte; PinRead(SDA_PIN); /* Set data pin as input */ for (i = 0, readByte = 0; i < 8; i++) { readByte <<= 1; DelayHalfus(); /* Clock low for 1.5 us */ Delay1us(); if (PinRead(SDA_PIN)) /* Read data bit */ readByte |= 1; PinSet(SCL_PIN); /* Clock high */ Delay1us(); PinClear(SCL_PIN); /* Clock low */ } return(readByte); } /* readOneByte() */ /************************************************************************/ /* Function : sendAck */ /* Purpose : Send ACK bit */ /* Inputs : None */ /* Outputs : None */ /************************************************************************/ static void sendAck(void) { PinClear(SDA_PIN); /* ACK is zero on SDA */ PinClear(SCL_PIN); DelayHalfus(); /* Clock low for 1.5 us */ Delay1us(); PinSet(SCL_PIN); /* Clock high for 1.0 us */ Delay1us(); PinClear(SCL_PIN); } /* sendAck() */ /************************************************************************/ /* Function : sendNack */ /* Purpose : Send NACK bit */ /* Inputs : None */ /* Outputs : None */ /************************************************************************/ static void sendNack(void) { PinSet(SDA_PIN); /* NACK is one on SDA */ PinClear(SCL_PIN); DelayHalfus(); /* Clock low for 1.5 us */ Delay1us(); PinSet(SCL_PIN); /* Clock high for 1.0 us */ Delay1us(); PinClear(SCL_PIN); } /* sendNack() */ /************************************************************************/ /* Function : earlyAbort */ /* Purpose : Return func when got NACK */ /* Inputs : Message about where error came from */ /* Outputs : None */ /************************************************************************/ static inv_error_t earlyAbort(char *msg) { sendStopBit(); _SerialError("%s: Error, no ACK\n"); return(INV_ERROR_SERIAL_WRITE); } /* earlyAbort() */ /************************************************************************/ /* Functions ported from mlsl_at32.c */ /************************************************************************/ inv_error_t inv_serial_open(char const * port, void **sl_handle) { #pragma unused(port, sl_handle) PIOSet(SCL_PIN); PIOSet(SDA_PIN); return INV_SUCCESS; } inv_error_t inv_serial_reset(void *sl_handle) { #pragma unused(sl_handle) PIOSet(SCL_PIN); PIOSet(SDA_PIN); return INV_SUCCESS; } inv_error_t inv_serial_close(void *sl_handle) { #pragma unused(sl_handle) PIOSet(SCL_PIN); PIOSet(SDA_PIN); return INV_SUCCESS; } static char *serialWriteMsg = "inv_serial_write"; static char *singleWriteMsg = "inv_serial_single_write"; static char *writeMemMsg = "inv_serial_write_mem"; /************************************************************************/ /* Function : inv_serial_write */ /* Purpose : Write data to I2C */ /* Inputs, Outputs: See below */ /************************************************************************/ /** * @brief used as generic serial write. Does not accept a * register parameter like the rest of the MLSLSerial * functions - if used to write to a register, the register * address should be the first member of data * This should be sent by I2C. * * @param sl_handle Unused, pass as NULL * @param slaveAddr I2C slave address of device. * @param length Length of burst of data. * @param data Pointer to block of data. * * @return INV_SUCCESS if successful, a non-zero error code otherwise. */ inv_error_t inv_serial_write( void *sl_handle, unsigned char slaveAddr, unsigned short length, unsigned char const *data ) { #pragma unused(sl_handle) uint16_t i; sendStartBit(); if (!writeOneByte(slaveAddr & 0xfe)) return(earlyAbort(serialWriteMsg)); for (i = 0; i < length-1; i++) if (!writeOneByte(data[i])) return(earlyAbort(serialWriteMsg)); sendStopBit(); return(INV_SUCCESS); } /* inv_serial_write() */ /************************************************************************/ /* Function : inv_serial_single_write */ /* Purpose : Write single register */ /* Inputs, Outputs: See below */ /************************************************************************/ /** * @brief used to write a single byte to a single register. * * @param sl_handle Unused, pass as NULL * @param slaveAddr I2C slave address of device. * @param registerAddr Register address to write. * @param data Single byte of data to write. * * @return INV_SUCCESS if successful, a non-zero error code otherwise. */ inv_error_t inv_serial_single_write(void *sl_handle, unsigned char slaveAddr, unsigned char registerAddr, unsigned char data) { #pragma unused(sl_handle) inv_error_t result; sendStartBit(); if (!writeOneByte(slaveAddr & 0xfe)) return(earlyAbort(singleWriteMsg)); if (!writeOneByte(registerAddr)) return(earlyAbort(singleWriteMsg)); if (!writeOneByte(data)) return(earlyAbort(singleWriteMsg)); sendStopBit(); return(INV_SUCCESS); } /* inv_serial_single_write() */ /************************************************************************/ /* Function : inv_serial_write_mem */ /* Purpose : Write multiple bytes to DMP memory */ /* Inputs, Outputs: See below */ /************************************************************************/ /** * @brief used to write multiple bytes of data to DMP memory. * * @param sl_handle Unused, pass as NULL * @param slaveAddr I2C slave address of device. * @param memAddr The location in the memory to write to. 16 bits. * @param length Length of burst data. Must not cross 8-bit address boundary. * @param data Pointer to block of data. * * @return Zero if successful; an error code otherwise */ inv_error_t inv_serial_write_mem( void *sl_handle, unsigned char slaveAddr, unsigned short memAddr, unsigned short length, unsigned char const *data ) { inv_error_t result; unsigned char tmpAddr; unsigned char memAddress; unsigned char i2cWrite[SERIAL_MAX_TRANSFER_SIZE]; uint_fast16_t bytesWritten = 0; if ((memAddr & 0xFF) + length > MPU_MEM_BANK_SIZE) { _SerialDebug("inv_serial_write_mem: memory write length (%d B) " "extends beyond its limits (%d)\n " "if started at location %d\n", length, MPU_MEM_BANK_SIZE, memAddr & 0xFF); return INV_ERROR_INVALID_PARAMETER; } /* Write bank - first time only */ if (inv_serial_single_write(sl_handle, slaveAddr, MPUREG_BANK_SEL, (memAddr>>8)) != INV_SUCCESS) return(INV_ERROR_MEMORY_SET); while (bytesWritten #include #include #include "log.h" #undef MPL_LOG_TAG #define MPL_LOG_TAG "mlsl-cf2" #include "mpu.h" #if defined CONFIG_MPU_SENSORS_MPU6050A2 # include "mpu6050a2.h" #elif defined CONFIG_MPU_SENSORS_MPU6050B1 # include "mpu6050b1.h" #elif defined CONFIG_MPU_SENSORS_MPU3050 # include "mpu3050.h" #else #error Invalid or undefined CONFIG_MPU_SENSORS_MPUxxxx #endif #include "mlsl.h" #include "mlinclude.h" #include "stdint_invensense.h" //#include "umplCalClient.h" #ifdef DEBUG #define _SerialError(...) MPL_LOGE(__VA_ARGS__) #define _SerialDebug(...) MPL_LOGD(__VA_ARGS__) #else #define _SerialError(...) do {} while(0) #define _SerialDebug(...) do {} while(0) #endif /** * @brief stub on at32 platform. The Atmel ASF twim library, * which provides twim_read and twim_write, * should be configured before this is called. * * @param port Unused, pass as NULL * @param sl_handle Unused, pass as NULL */ inv_error_t inv_serial_open(char const * port, void **sl_handle) { #pragma unused(port, sl_handle) PIOSet(SCL_PIN); PIOSet(SDA_PIN); return INV_SUCCESS; } inv_error_t inv_serial_reset(void *sl_handle) { #pragma unused(sl_handle) PIOSet(SCL_PIN); PIOSet(SDA_PIN); return INV_SUCCESS; } inv_error_t inv_serial_close(void *sl_handle) { #pragma unused(sl_handle) PIOSet(SCL_PIN); PIOSet(SDA_PIN); return INV_SUCCESS; } /** * @brief used as generic serial write. Does not accept a * register parameter like the rest of the MLSLSerial * functions - if used to write to a register, the register * address should be the first member of data * This should be sent by I2C. * * @param sl_handle Unused, pass as NULL * @param slaveAddr I2C slave address of device. * @param length Length of burst of data. * @param data Pointer to block of data. * * @return INV_SUCCESS if successful, a non-zero error code otherwise. */ inv_error_t inv_serial_write( void *sl_handle, unsigned char slaveAddr, unsigned short length, unsigned char const *data ) { #pragma unused(sl_handle, slaveAddr, length, data) #if 0 inv_error_t result; /********** Persistor bit-banging for I2C goes here ****************/ // twim_write does follow the slaveAddr / slaveRegister convention // so we transform the inputs to follow this convention. uint8_t regAddr = data[0]; data = data + 1; length = length -1; result = twim_write(slaveAddr, regAddr, data, (size_t)length); #endif return INV_SUCCESS; } /** * @brief used to write a single byte to a single register. * * @param sl_handle Unused, pass as NULL * @param slaveAddr I2C slave address of device. * @param registerAddr Register address to write. * @param data Single byte of data to write. * * @return INV_SUCCESS if successful, a non-zero error code otherwise. */ inv_error_t inv_serial_single_write(void *sl_handle, unsigned char slaveAddr, unsigned char registerAddr, unsigned char data) { #pragma unused(sl_handle, slaveAddr, registerAddr, data) #if 0 inv_error_t result; /********** Persistor bit-banging for I2C goes here ****************/ // Copy the argument to this call stack. // In the past there have been compiler issues which prohibit // dereferencing arguments. unsigned char tempData[1]; tempData[0] = data; result = twim_write((uint16_t)slaveAddr, (uint8_t)registerAddr, tempData, 1); if(result != STATUS_OK ){ #ifdef M_HW /* Mantis rev A2 Silicon Errata: setting reset bit will cause i2c nack */ if ( slaveAddr == 104 && registerAddr == 107 && (tempData[0] | 0x80 )) return INV_SUCCESS; /* Ignore that particular error. */ #endif _SerialError("inv_serial_single_write: Error. Could not twim_write.\n"); return INV_ERROR_SERIAL_WRITE; } #endif return INV_SUCCESS; } /** * @brief used to write multiple bytes of data to DMP memory. * * @param sl_handle Unused, pass as NULL * @param slaveAddr I2C slave address of device. * @param memAddr The location in the memory to write to. 16 bits. * @param length Length of burst data. Must not cross 8-bit address boundary. * @param data Pointer to block of data. * * @return Zero if successful; an error code otherwise */ inv_error_t inv_serial_write_mem( void *sl_handle, unsigned char slaveAddr, unsigned short memAddr, unsigned short length, unsigned char const *data ) { #pragma unused(sl_handle, slaveAddr, memAddr, length, data) #if 0 inv_error_t result; unsigned char tmpAddr; unsigned char memAddress[1]; unsigned char i2cWrite[SERIAL_MAX_TRANSFER_SIZE]; uint_fast16_t bytesWritten = 0; if ((memAddr & 0xFF) + length > MPU_MEM_BANK_SIZE) { _SerialDebug("inv_serial_write_mem: memory write length (%d B) " "extends beyond its limits (%d)\n " "if started at location %d\n", length, MPU_MEM_BANK_SIZE, memAddr & 0xFF); return INV_ERROR_INVALID_PARAMETER; } /* Write bank - first time only */ tmpAddr = MPUREG_BANK_SEL; memAddress[0] = memAddr >> 8; /********** Persistor bit-banging for I2C goes here ****************/ result = twim_write((uint16_t)slaveAddr, (uint8_t)tmpAddr, memAddress, 1 ); if (STATUS_OK != result){ _SerialError("inv_serial_write_mem: " "Could not initialize memory read.\n"); return INV_ERROR_MEMORY_SET; } while (bytesWrittenFIFO_HW_SIZE) { _SerialDebug("inv_serial_write_fifo: " "maximum fifo write length is %d\n", FIFO_HW_SIZE); return INV_ERROR_INVALID_PARAMETER; } result = twim_write(slaveAddr, MPUREG_FIFO_R_W, data, length); if(result != STATUS_OK ) { _SerialError("inv_serial_write_fifo: Could not twim_write.\n"); return INV_ERROR_SERIAL_WRITE; } #endif return INV_SUCCESS; } /** * @brief used to read multiple bytes of data from registers. * This should be sent by I2C. * * @param sl_handle Unused, pass as NULL * @param slaveAddr I2C slave address of device. * @param registerAddr Register address to read. * @param length Length of burst of data. * @param data Pointer to block of data. * * @return Zero if successful; an error code otherwise */ inv_error_t inv_serial_read( void *sl_handle, unsigned char slaveAddr, unsigned char registerAddr, unsigned short length, unsigned char *data ) { #pragma unused(sl_handle, slaveAddr, registerAddr, length, data) #if 0 inv_error_t result; if (slaveAddr == 0x68 && (registerAddr==MPUREG_FIFO_R_W || registerAddr==MPUREG_MEM_R_W)) { _SerialDebug("inv_serial_read: Invalid register." "Use proper MLSLSerial api to read FIFO and DMP memory\n"); return INV_ERROR_INVALID_PARAMETER; } result = twim_read(slaveAddr, registerAddr, data, length); if(result != STATUS_OK ) { _SerialError("\rinv_serial_read: Error. Could not twim_read.\n"); return INV_ERROR_SERIAL_READ; } #endif return INV_SUCCESS; } /** * @brief used to read multiple bytes of data from the memory. * This should be sent by I2C. * * @param sl_handle Unused, pass as NULL * @param slaveAddr I2C slave address of device. * @param memAddr The location in the memory to read from. * @param length Length of burst data. * @param data Pointer to block of data. * * @return Zero if successful; an error code otherwise */ inv_error_t inv_serial_read_mem( void *sl_handle, unsigned char slaveAddr, unsigned short memAddr, unsigned short length, unsigned char *data ) { #pragma unused(sl_handle, slaveAddr, memAddr, length, data) #if 0 inv_error_t result; unsigned char tmpAddr; unsigned char memAddress[1]; uint_fast16_t bytesRead = 0; if (memAddr & 0xFF + length > MPU_MEM_BANK_SIZE) { _SerialDebug("inv_serial_read_mem: memory read length (%d B) " "extends beyond its limits (%d) " "if started at location %d\n", length, MPU_MEM_BANK_SIZE, memAddr & 0xFF); return INV_ERROR_INVALID_PARAMETER; } /* Write bank - first time only */ tmpAddr = MPUREG_BANK_SEL; memAddress[0] = memAddr >> 8; result = twim_write((uint16_t)slaveAddr, (uint8_t)tmpAddr, memAddress, 1 ); if (STATUS_OK != result){ _SerialError("inv_serial_read_mem: " "Could not initialize memory read.\n"); return INV_ERROR_SERIAL_WRITE; } while (bytesRead < length) { unsigned short thisLen = min(SERIAL_MAX_TRANSFER_SIZE, length-bytesRead); /* Write address */ tmpAddr = MPUREG_MEM_START_ADDR; memAddress[0] = memAddr+bytesRead; // valid because we don't allow going // beyond the MPU_MEM_BANK_SIZE boundary result = twim_write((uint16_t)slaveAddr,(uint8_t)tmpAddr, memAddress, 1); if (STATUS_OK != result){ _SerialError("inv_serial_read_mem: " "Could not set memory location for read.\n"); return INV_ERROR_SERIAL_WRITE; } /* Read actual data */ result = twim_read(slaveAddr, MPUREG_MEM_R_W, &data[bytesRead], thisLen); if (STATUS_OK != result){ _SerialError("inv_serial_read_mem: " "Could not read DMP memory.\n"); return INV_ERROR_SERIAL_READ; } bytesRead += thisLen; } return result; #else return INV_SUCCESS; #endif } /** * @brief used to read multiple bytes of data from the fifo. * This should be sent by I2C. * * @param sl_handle Unused, pass as NULL * @param slaveAddr I2C slave address of device. * @param length Number of bytes to read from fifo. * @param data Pointer to write fifo data. * * @return Zero if successful; an error code otherwise */ inv_error_t inv_serial_read_fifo( void *sl_handle, unsigned char slaveAddr, unsigned short length, unsigned char *data ) { #pragma unused(sl_handle, slaveAddr, length, data) #if 0 uint16_t bytesRead = 0; inv_error_t result; if (length>FIFO_HW_SIZE) { _SerialDebug("inv_serial_read_fifo: " "maximum fifo read length is %d\n", FIFO_HW_SIZE); return INV_ERROR_INVALID_PARAMETER; } while (bytesRead < length) { unsigned short thisLen = min(SERIAL_MAX_TRANSFER_SIZE, length-bytesRead); result = twim_read(slaveAddr, MPUREG_FIFO_R_W, data, thisLen); if(result != STATUS_OK ){ _SerialError("inv_serial_read_fifo: Could not twim_read.\n"); return INV_ERROR_SERIAL_READ; } bytesRead += thisLen; } #endif return INV_SUCCESS; } /** * @brief used to get the calibration data. * It is called by the MPL to get the calibration data used by the * motion library. * This data would typically be saved in non-volatile memory. * * @param cfg Pointer to the calibration data. * @param len Length of the calibration data. * * @return INV_SUCCESS if successful, a non-zero error code otherwise. */ inv_error_t inv_serial_read_cal( unsigned char *cal, unsigned int len ) { #pragma unused(cal, len) /* UMPL does not implement the inv_serial_read_cal function.*/ return INV_ERROR_FEATURE_NOT_IMPLEMENTED; } /** * @brief Get the calibration length. * @param len * lenght to be returned * @return INV_SUCCESS if successful, a non-zero error code otherwise. */ inv_error_t inv_serial_get_cal_length(unsigned int *len) { #pragma unused(len) /* UMPL does not implement the inv_serial_get_cal_length function.*/ return INV_ERROR_FEATURE_NOT_IMPLEMENTED; } /** * @brief used to save the calibration data. * It is called by the MPL to save the calibration data used by the * motion library. * This data would typically be saved in non-volatile memory. * * @param cfg Pointer to the calibration data. * @param len Length of the calibration data. * * @return INV_SUCCESS if successful, a non-zero error code otherwise. */ inv_error_t inv_serial_write_cal( unsigned char *cal, unsigned int len ) { #pragma unused(cal, len) /* UMPL does not implement the inv_serial_write_cal function.*/ return INV_ERROR_FEATURE_NOT_IMPLEMENTED; }