/*******************************************************************************
* error_t hmc5883l_reg_write(uint8_t reg, uint8_t value)
*
* Description:
* Write data to HMC5883L register. Only single byte writes to registers are
* supported. Possible errors are reported to the calling function.
*
* Parameters:
* reg address of register to be written
* value value to be written to specified register
*
* Return:
* error ERR_OK - completed without error
* ERR_FAIL - failed to write register
*
* Example:
* error = hmc5883l_reg_write(HMC5883L_CONF_REG_A, 0x18);
*
* Note:
*
* History:
* pka, 03/SEP/2015, initial code
******************************************************************************/
error_t hmc5883l_reg_write(uint8_t reg, uint8_t value)
{
uint8_t tmp[2];
/* Prepare data array to be written to I2C bus. */
tmp[0] = reg;
tmp[1] = value;
/* Transmit START condition and slave address + WRITE. Write register
* address followed by register value and generate STOP condition. */
if (i2c_transmit(HMC5883L_SLAVE_ADDR, tmp, 2) != ERR_OK)
{
return ERR_FAIL;
}
return ERR_OK;
}
/*******************************************************************************
* error_t hmc5883l_reg_read(uint8_t reg, uint8_t* value)
*
* Description:
* Read data from HMC5883L register. Only single byte reads from registers are
* supported. Possible errors are reported to the calling function.
*
* Parameters:
* reg register to be read
* value memory address of register value
*
* Return:
* error ERR_OK - completed without error
* ERR_FAIL - failed to write register
*
* Example:
* error = hmc5883l_reg_read(HMC5883L_CONF_REG_A, &value);
*
* Note:
*
* History:
* pka, 03/SEP/2015, initial code
******************************************************************************/
error_t hmc5883l_reg_read(uint8_t reg, uint8_t* value)
{
/* Transmit START condition and slave address + WRITE. Write register
* address and generate STOP condition. */
if (i2c_transmit(HMC5883L_SLAVE_ADDR, ®, 1) != ERR_OK)
{
return ERR_FAIL;
}
/* Transmit START condition and slave address + READ. Read register value
* and generate STOP condition. */
if (i2c_receive(HMC5883L_SLAVE_ADDR, value, 1) != ERR_OK)
{
return ERR_FAIL;
}
return ERR_OK;
}
void actuators_mkk_set(void) {
const uint8_t actuators_addr[ACTUATORS_MKK_NB] = ACTUATORS_MKK_ADDR;
#if defined ACTUATORS_START_DELAY && ! defined SITL
if (!actuators_delay_done) {
if (SysTimeTimer(actuators_delay_time) < USEC_OF_SEC(ACTUATORS_START_DELAY)) return;
else actuators_delay_done = TRUE;
}
#endif
for (uint8_t i=0; i<ACTUATORS_MKK_NB; i++) {
#ifdef KILL_MOTORS
actuators_mkk.trans[i].buf[0] = 0;
#endif
i2c_transmit(&ACTUATORS_MKK_DEVICE, &actuators_mkk.trans[i], actuators_addr[i], 1);
}
}
char i2c_write(char data)
{
unsigned char twi_status;
char r_val = -1;
// Send the Data to I2C Bus
TWDR = data;
// Transmit I2C Data
twi_status=i2c_transmit(I2C_DATA);
// Check the TWSR status
if (twi_status != TW_MT_DATA_ACK) goto i2c_quit;
r_val=0;
i2c_quit:
return r_val;
}
/**
* Poll lidar for data
* run at max 50Hz
*/
void lidar_lite_periodic(void)
{
switch (lidar.status) {
case LIDAR_INIT_RANGING:
// ask for i2c frame
lidar.trans.buf[0] = LIDAR_LITE_REG_ADDR; // sets register pointer to (0x00)
lidar.trans.buf[1] = LIDAR_LITE_REG_VAL; // take acquisition & correlation processing with DC correction
if (i2c_transmit(&LIDAR_LITE_I2C_DEV, &lidar.trans, lidar.addr, 2)) {
// transaction OK, increment status
lidar.status = LIDAR_REQ_READ;
}
break;
case LIDAR_REQ_READ:
lidar.trans.buf[0] = LIDAR_LITE_READ_ADDR; // sets register pointer to results register
lidar.trans.buf[1] = 0;
if (i2c_transceive(&LIDAR_LITE_I2C_DEV, &lidar.trans, lidar.addr, 1,2)) {
// transaction OK, increment status
lidar.status = LIDAR_READ_DISTANCE;
}
break;
case LIDAR_READ_DISTANCE:
// filter data
/*
lidar.distance_raw = (uint16_t)((lidar.trans.buf[0] << 8) + lidar.trans.buf[1]);
lidar.distance = update_median_filter(&lidar_filter, (int32_t)lidar.distance_raw);
*/
//lidar.distance_raw = (uint32_t)((lidar.trans.buf[0] << 8) | lidar.trans.buf[1]);
lidar.distance_raw = update_median_filter(&lidar_filter, (uint32_t)((lidar.trans.buf[0] << 8) | lidar.trans.buf[1]));
lidar.distance = ((float)lidar.distance_raw)/100.0;
// send message (if requested)
if (lidar.update_agl) {
AbiSendMsgAGL(AGL_LIDAR_LITE_ID, lidar.distance);
}
// increment status
lidar.status = LIDAR_INIT_RANGING;
break;
default:
break;
}
}
I2C_STS GSensor_I2C_Transmit(UINT32 value, BOOL bStart, BOOL bStop)
{
I2C_DATA I2cData;
I2C_STS ret;
I2cData.VersionInfo = DRV_VER_96650;
I2cData.ByteCount = I2C_BYTE_CNT_1;
I2cData.Byte[0].uiValue = value & 0xff;
I2cData.Byte[0].Param = I2C_BYTE_PARAM_NONE;
if ( bStart == TRUE )
I2cData.Byte[0].Param |= I2C_BYTE_PARAM_START;
if ( bStop == TRUE )
I2cData.Byte[0].Param |= I2C_BYTE_PARAM_STOP;
ret = i2c_transmit(&I2cData);
if ( ret != I2C_STS_OK )
{
DBG_ERR("i2c ret = %d!!\r\n", ret);
}
return ret;
}
void generic_com_periodic( void ) {
if (com_trans.status != I2CTransDone) { return; }
com_trans.buf[0] = active_com;
FillBufWith32bit(com_trans.buf, 1, gps.lla_pos.lat);
FillBufWith32bit(com_trans.buf, 5, gps.lla_pos.lon);
FillBufWith16bit(com_trans.buf, 9, (int16_t)(gps.lla_pos.alt/1000)); // altitude (meters)
FillBufWith16bit(com_trans.buf, 11, gps.gspeed); // ground speed (cm/s)
FillBufWith16bit(com_trans.buf, 13, (int16_t)(gps.course/1e4)); // course (1e3rad)
FillBufWith16bit(com_trans.buf, 15, (uint16_t)((*stateGetAirspeed_f())*100)); // TAS (cm/s)
com_trans.buf[17] = electrical.vsupply; // decivolts
com_trans.buf[18] = (uint8_t)(energy/100); // deciAh
com_trans.buf[19] = (uint8_t)(ap_state->commands[COMMAND_THROTTLE]*100/MAX_PPRZ);
com_trans.buf[20] = pprz_mode;
com_trans.buf[21] = nav_block;
FillBufWith16bit(com_trans.buf, 22, autopilot_flight_time);
i2c_transmit(&GENERIC_COM_I2C_DEV, &com_trans, GENERIC_COM_SLAVE_ADDR, NB_DATA);
}
/**
* @brief EEPROM write routine.
* @details Function writes data to EEPROM.
* @pre Data must be fit to single EEPROM page.
*
* @param[in] addr addres of 1-st byte to be write
* @param[in] buf pointer to data
* @param[in] len number of bytes to be written
*/
msg_t eeprom_write(uint32_t addr, const uint8_t *buf, size_t len){
msg_t status = RDY_OK;
chBSemWait(&eeprom_sem);
chDbgCheck(((len <= EEPROM_SIZE) && ((addr+len) <= EEPROM_SIZE)),
"data can not be fitted in device");
chDbgCheck(((addr / EEPROM_PAGE_SIZE) == ((addr + len - 1) / EEPROM_PAGE_SIZE)),
"data can not be fitted in single page");
eeprom_split_addr(localtxbuf, addr); /* write address bytes */
memcpy(&(localtxbuf[2]), buf, len); /* write data bytes */
i2c_transmit(eeprom_i2caddr, localtxbuf, (len + 2), NULL, 0);
/* wait until EEPROM process data */
chThdSleepMilliseconds(EEPROM_WRITE_TIME);
chBSemSignal(&eeprom_sem);
return status;
}
/**
******************************************************************************
* @brief Check if I2C slave device is connected or not
* @param Slave device address
* @retval Connection status (SUCCESS or ERROR)
******************************************************************************
*/
uint8_t i2c_is_device_connected(uint8_t address)
{
i2c_start();
if (i2c_check_status(MT_START) == ERROR)
return ERROR;
// Check device is connected or not
// If connected, device will send ACK after
// master transmit SLA+W or SLA+R
i2c_transmit((address << 1) | I2C_WRITE);
if (i2c_check_status(MT_SLA_WRITE_ACK) == ERROR)
{
i2c_stop();
return ERROR;
}
i2c_stop();
return SUCCESS;
}
/**
******************************************************************************
* @brief Read bytes from slave without specify register address
* @param Slave device address (7-bit right aligned)
* @param Number of data bytes to read from slave
* @param Pointer to data array byte to store data from slave
* @retval Result status (SUCCESS or ERROR)
******************************************************************************
*/
uint8_t i2c_read_multi_no_reg(uint8_t address, uint8_t len, uint8_t* data)
{
i2c_start();
if (i2c_check_status(MT_START) == ERROR)
return ERROR;
i2c_transmit((address << 1) | I2C_READ);
if (i2c_check_status(MT_SLA_READ_ACK) == ERROR)
{
i2c_stop();
return ERROR;
}
for (int i = 0; i < len; i++)
{
if (i == (len - 1))
{
data[i] = i2c_receive_nack();
if (i2c_check_status(MT_DATA_RECEIVED_NACK) == ERROR)
{
i2c_stop();
return ERROR;
}
}
else
{
data[i] = i2c_receive_ack();
if (i2c_check_status(MT_DATA_RECEIVED_ACK) == ERROR)
{
i2c_stop();
return ERROR;
}
}
}
i2c_stop();
return SUCCESS;
}
unsigned int srf08_read_register(unsigned char srf08_register)
{
union i2c_union {
unsigned int rx_word;
unsigned char rx_byte[2];
} i2c;
I2C_START_TX(address);
i2c_transmit(srf08_register);
I2C_START_RX(address);
/* get high byte msb first */
if(srf08_register>=2) { i2c.rx_byte[1]=i2c_receive(I2C_CONTINUE); }
/* get low byte msb first */
i2c.rx_byte[0]=i2c_receive(I2C_QUIT);
i2c_stop();
return(i2c.rx_word);
}
void i2c_read_registers(uint8_t bus_num, uint8_t slave_addr, uint8_t reg_addr, uint8_t numBytes, uint8_t* spaceToWrite) {
uint8_t i2c_packet[1] = {reg_addr};
//=== transmit reg address to gyro
i2c_init_transmit(bus_num,slave_addr);
while( i2c_busy(bus_num) );
i2c_transmit(bus_num,sizeof(i2c_packet),i2c_packet);
while( i2c_busy(bus_num) );
__delay_cycles(I2C_BUS_FREE_TIME);
//=== read what gyro has to say
i2c_init_receive(bus_num,slave_addr);
while( i2c_busy(bus_num) );
i2c_receive(bus_num,numBytes,spaceToWrite);
while( i2c_busy(bus_num) );
__delay_cycles(I2C_BUS_FREE_TIME);
}
void MPPT_periodic( void ) {
if (mppt_trans.status == I2CTransSuccess) {
switch (MPPT_status) {
case MPPT_STATUS_IDLE:
/* If free, change mode if needed */
if (MPPT_mode) {
mppt_trans.buf[0] = MPPT_MODE_ADDR;
mppt_trans.buf[1] = 0;
mppt_trans.buf[2] = MPPT_mode;
i2c_transmit(&i2c0, &mppt_trans, MPPT_SLAVE_ADDR, 3);
MPPT_mode = 0;
MPPT_status = MPPT_STATUS_WRITING;
} else {
MPPT_ask();
}
break;
case MPPT_STATUS_WRITING:
MPPT_status = MPPT_STATUS_IDLE;
break;
case MPPT_STATUS_ASKING:
/* The slave should send 2 bytes */
i2c_receive(&i2c0, &mppt_trans, MPPT_SLAVE_ADDR, 2);
MPPT_status = MPPT_STATUS_READING;
break;
case MPPT_STATUS_READING:
/* We got 2 bytes */
if (data_index < NB_I2C_DATA)
MPPT_data[data_index] = (mppt_trans.buf[0]<<8) | mppt_trans.buf[1];
MPPT_status = MPPT_STATUS_IDLE;
break;
}
}
}
/**
******************************************************************************
* @brief Read byte from slave without specify register address
* @param Slave device address (7-bit right aligned)
* @param Pointer to data byte to store data from slave
* @retval Result status (SUCCESS or ERROR)
******************************************************************************
*/
uint8_t i2c_read_no_reg(uint8_t address, uint8_t* data)
{
i2c_start();
if (i2c_check_status(MT_START) == ERROR)
return ERROR;
i2c_transmit((address << 1) | I2C_READ);
if (i2c_check_status(MT_SLA_READ_ACK) == ERROR)
{
i2c_stop();
return ERROR;
}
*data = i2c_receive_nack();
if (i2c_check_status(MT_DATA_RECEIVED_NACK) == ERROR)
{
i2c_stop();
return ERROR;
}
i2c_stop();
return SUCCESS;
}
/*
*******************************************************************************
* EXPORTED FUNCTIONS
*******************************************************************************
*/
void init_mma8451(BinarySemaphore *mma8451_semp){
search_indexes();
/* Помни о том, что большинство конфигурационных регистров нельзя менять
в активном режиме, надо сначала свалить девайс в STANDBY. */
// TODO: сначала вообще убедиться, что девайс отвечает путем запроса его WHOAMI
// TODO: запустить в нем самодиагностику
#if CH_DBG_ENABLE_ASSERTS
// clear bufers. Just to be safe.
uint32_t i = 0;
for (i = 0; i < ACCEL_TX_DEPTH; i++){txbuf[i] = 0x55;}
for (i = 0; i < ACCEL_RX_DEPTH; i++){rxbuf[i] = 0x55;}
#endif
txbuf[0] = ACCEL_CTRL_REG2;
txbuf[1] = 0b100000; //Reset
while (i2c_transmit(mma8451addr, txbuf, 2, rxbuf, 0) != RDY_OK)
;
chThdSleepMilliseconds(4);
txbuf[0] = ACCEL_CTRL_REG1;
txbuf[1] = 0b0; //set standby to allow configure device
while (i2c_transmit(mma8451addr, txbuf, 2, rxbuf, 0) != RDY_OK)
;
chThdSleepMilliseconds(4);
txbuf[0] = ACCEL_XYZ_DATA_CFG;
txbuf[1] = (uint8_t)(ACCEL_SENS >> 2);
while (i2c_transmit(mma8451addr, txbuf, 2, rxbuf, 0) != RDY_OK)
;
chThdSleepMilliseconds(4);
txbuf[0] = ACCEL_CTRL_REG2;
txbuf[1] = 0b10; //High Resolution
while (i2c_transmit(mma8451addr, txbuf, 2, rxbuf, 0) != RDY_OK)
;
chThdSleepMilliseconds(4);
txbuf[0] = ACCEL_CTRL_REG3;
txbuf[1] = 0b10; //Interrupt active high
while (i2c_transmit(mma8451addr, txbuf, 2, rxbuf, 0) != RDY_OK)
;
chThdSleepMilliseconds(4);
txbuf[0] = ACCEL_CTRL_REG4;
txbuf[1] = 0b01; //Interrupt on data ready
while (i2c_transmit(mma8451addr, txbuf, 2, rxbuf, 0) != RDY_OK)
;
chThdSleepMilliseconds(4);
txbuf[0] = ACCEL_CTRL_REG1;
//txbuf[1] = 0b11101; //100Hz, low noice, active
//txbuf[1] = 0b11001; //100Hz, normal noice, active
txbuf[1] = 0b100101; //50Hz, low noice, active
while (i2c_transmit(mma8451addr, txbuf, 2, rxbuf, 0) != RDY_OK)
;
chThdSleepMilliseconds(2);
chThdCreateStatic(PollAccelThreadWA,
sizeof(PollAccelThreadWA),
I2C_THREADS_PRIO,
PollAccelThread,
mma8451_semp);
chThdSleepMilliseconds(1);
}
void imu_write_reg(uint8_t addr, uint8_t reg, uint8_t value) {
uint8_t data[] = {reg, value};
i2c_transmit(addr, data, 2);
}
void actuators_asctec_set(bool_t motors_on) {
#if defined ACTUATORS_START_DELAY && ! defined SITL
if (!actuators_delay_done) {
if (SysTimeTimer(actuators_delay_time) < USEC_OF_SEC(ACTUATORS_START_DELAY)) return;
else actuators_delay_done = TRUE;
}
#endif
switch (actuators_asctec.i2c_trans.status) {
case I2CTransFailed:
actuators_asctec.nb_err++;
actuators_asctec.i2c_trans.status = I2CTransDone;
break;
case I2CTransSuccess:
case I2CTransDone:
actuators_asctec.i2c_trans.status = I2CTransDone;
break;
default:
actuators_asctec.nb_err++;
return;
}
#ifdef KILL_MOTORS
actuators_asctec.cmds[PITCH] = 0;
actuators_asctec.cmds[ROLL] = 0;
actuators_asctec.cmds[YAW] = 0;
actuators_asctec.cmds[THRUST] = 0;
#else /* ! KILL_MOTORS */
Bound(actuators_asctec.cmds[PITCH],ASCTEC_MIN_CMD, ASCTEC_MAX_CMD);
Bound(actuators_asctec.cmds[ROLL], ASCTEC_MIN_CMD, ASCTEC_MAX_CMD);
Bound(actuators_asctec.cmds[YAW], ASCTEC_MIN_CMD, ASCTEC_MAX_CMD);
if (motors_on) {
Bound(actuators_asctec.cmds[THRUST], ASCTEC_MIN_THROTTLE + 1, ASCTEC_MAX_THROTTLE);
}
else
actuators_asctec.cmds[THRUST] = 0;
#endif /* KILL_MOTORS */
switch (actuators_asctec.cmd) {
case TEST:
actuators_asctec.i2c_trans.buf[0] = 251;
actuators_asctec.i2c_trans.buf[1] = actuators_asctec.cur_addr;
actuators_asctec.i2c_trans.buf[2] = 0;
actuators_asctec.i2c_trans.buf[3] = 231 + actuators_asctec.cur_addr;
break;
case REVERSE:
actuators_asctec.i2c_trans.buf[0] = 254;
actuators_asctec.i2c_trans.buf[1] = actuators_asctec.cur_addr;
actuators_asctec.i2c_trans.buf[2] = 0;
actuators_asctec.i2c_trans.buf[3] = 234 + actuators_asctec.cur_addr;
break;
case SET_ADDR:
actuators_asctec.i2c_trans.buf[0] = 250;
actuators_asctec.i2c_trans.buf[1] = actuators_asctec.cur_addr;
actuators_asctec.i2c_trans.buf[2] = actuators_asctec.new_addr;
actuators_asctec.i2c_trans.buf[3] = 230 + actuators_asctec.cur_addr +
actuators_asctec.new_addr;
actuators_asctec.cur_addr = actuators_asctec.new_addr;
break;
case NONE:
actuators_asctec.i2c_trans.buf[0] = 100 - actuators_asctec.cmds[PITCH];
actuators_asctec.i2c_trans.buf[1] = 100 + actuators_asctec.cmds[ROLL];
actuators_asctec.i2c_trans.buf[2] = 100 - actuators_asctec.cmds[YAW];
actuators_asctec.i2c_trans.buf[3] = actuators_asctec.cmds[THRUST];
break;
default:
break;
}
actuators_asctec.cmd = NONE;
i2c_transmit(&ACTUATORS_ASCTEC_I2C_DEV, &actuators_asctec.i2c_trans,
ACTUATORS_ASCTEC_SLAVE_ADDR, 4);
}
/*
* Call to ChibiOS I2C function.
*/
void mpu_i2c_write(uint8_t addr, uint8_t value){
uint8_t txbuf[2];
txbuf[0] = addr;
txbuf[1] = value;
i2c_transmit(MPU_ADDR, txbuf, 2, NULL, 0);
}
static void l3g4200_i2c_tx_reg(struct L3g4200 *l3g, uint8_t reg, uint8_t val)
{
l3g->i2c_trans.buf[0] = reg;
l3g->i2c_trans.buf[1] = val;
i2c_transmit(l3g->i2c_p, &(l3g->i2c_trans), l3g->i2c_trans.slave_addr, 2);
}
void send_data(uint8_t data) {
micro_oled_transfer_buffer[0] = I2C_DATA;
micro_oled_transfer_buffer[1] = data;
i2c_transmit(I2C_ADDRESS_SA0_1 << 1, micro_oled_transfer_buffer, 2, 100);
}
static int i2c_set_keyscan_interval(int hand, int delay) {
uint8_t buf[] = {TWI_CMD_KEYSCAN_INTERVAL, delay};
i2c_status_t ret = i2c_transmit(I2C_ADDR(hand), buf, sizeof(buf), I2C_TIMEOUT);
return ret;
}
void send_command(uint8_t command) {
micro_oled_transfer_buffer[0] = I2C_COMMAND;
micro_oled_transfer_buffer[1] = command;
i2c_transmit(I2C_ADDRESS_SA0_1 << 1, micro_oled_transfer_buffer, 2, 100);
}
static void baro_scp_start_high_res_measurement(void) {
/* switch to high resolution */
scp_trans.buf[0] = SCP1000_OPERATION;
scp_trans.buf[1] = SCP1000_HIGH_RES;
i2c_transmit(&SCP_I2C_DEV, &scp_trans, SCP1000_SLAVE_ADDR, 2);
}
uint8_t imu_read_reg(uint8_t addr, uint8_t reg) {
i2c_transmit(addr, ®, 1);
uint8_t value;
i2c_receive(addr, &value, 1);
return value;
}
void imu_a_init(){
i2c_transmit(0x30,0x20,0x3F);
}
void imu_impl_init(void)
{
///////////////////
// Configure power:
// MPU60X0_REG_AUX_VDDIO = 0 (good on startup)
// MPU60X0_REG_USER_CTRL:
// -Enable Aux I2C Master Mode
// -Enable SPI
// MPU60X0_REG_PWR_MGMT_1
// -switch to gyroX clock
aspirin2_mpu60x0.buf[0] = MPU60X0_REG_PWR_MGMT_1;
aspirin2_mpu60x0.buf[1] = 0x01;
i2c_transmit(&PPZUAVIMU_I2C_DEV, &aspirin2_mpu60x0, MPU60X0_ADDR, 2);
while(aspirin2_mpu60x0.status == I2CTransPending);
// MPU60X0_REG_PWR_MGMT_2: Nothing should be in standby: default OK
/////////////////////////
// Measurement Settings
// MPU60X0_REG_CONFIG
// -ext sync on gyro X (bit 3->6)
// -digital low pass filter: 1kHz sampling of gyro/acc with 44Hz bandwidth: since reading is at 100Hz
#if PERIODIC_FREQUENCY == 60
#else
# if PERIODIC_FREQUENCY == 120
# else
# error PERIODIC_FREQUENCY should be either 60Hz or 120Hz. Otherwise manually fix the sensor rates
# endif
#endif
aspirin2_mpu60x0.buf[0] = MPU60X0_REG_CONFIG;
aspirin2_mpu60x0.buf[1] = (2 << 3) | (3 << 0);
i2c_transmit(&PPZUAVIMU_I2C_DEV, &aspirin2_mpu60x0, MPU60X0_ADDR, 2);
while(aspirin2_mpu60x0.status == I2CTransPending);
// MPU60X0_REG_SMPLRT_DIV
// -100Hz output = 1kHz / (9 + 1)
aspirin2_mpu60x0.buf[0] = MPU60X0_REG_SMPLRT_DIV;
aspirin2_mpu60x0.buf[1] = 9;
i2c_transmit(&PPZUAVIMU_I2C_DEV, &aspirin2_mpu60x0, MPU60X0_ADDR, 2);
while(aspirin2_mpu60x0.status == I2CTransPending);
// MPU60X0_REG_GYRO_CONFIG
// -2000deg/sec
aspirin2_mpu60x0.buf[0] = MPU60X0_REG_GYRO_CONFIG;
aspirin2_mpu60x0.buf[1] = (3<<3);
i2c_transmit(&PPZUAVIMU_I2C_DEV, &aspirin2_mpu60x0, MPU60X0_ADDR, 2);
while(aspirin2_mpu60x0.status == I2CTransPending);
// MPU60X0_REG_ACCEL_CONFIG
// 16g, no HPFL
aspirin2_mpu60x0.buf[0] = MPU60X0_REG_ACCEL_CONFIG;
aspirin2_mpu60x0.buf[1] = (3<<3);
i2c_transmit(&PPZUAVIMU_I2C_DEV, &aspirin2_mpu60x0, MPU60X0_ADDR, 2);
while(aspirin2_mpu60x0.status == I2CTransPending);
/*
// no interrupts for now, but set data ready interrupt to enable reading status bits
aspirin2_mpu60x0.buf[0] = ITG3200_REG_INT_CFG;
aspirin2_mpu60x0.buf[1] = 0x01;
i2c_submit(&PPZUAVIMU_I2C_DEV,&aspirin2_mpu60x0);
while(aspirin2_mpu60x0.status == I2CTransPending);
*/
/*
/////////////////////////////////////////////////////////////////////
// HMC5843
ppzuavimu_hmc5843.slave_addr = HMC5843_ADDR;
ppzuavimu_hmc5843.type = I2CTransTx;
ppzuavimu_hmc5843.buf[0] = HMC5843_REG_CFGA; // set to rate to max speed: 50Hz no bias
ppzuavimu_hmc5843.buf[1] = 0x00 | (0x06 << 2);
ppzuavimu_hmc5843.len_w = 2;
i2c_submit(&PPZUAVIMU_I2C_DEV,&ppzuavimu_hmc5843);
while(ppzuavimu_hmc5843.status == I2CTransPending);
ppzuavimu_hmc5843.type = I2CTransTx;
ppzuavimu_hmc5843.buf[0] = HMC5843_REG_CFGB; // set to gain to 1 Gauss
ppzuavimu_hmc5843.buf[1] = 0x01<<5;
ppzuavimu_hmc5843.len_w = 2;
i2c_submit(&PPZUAVIMU_I2C_DEV,&ppzuavimu_hmc5843);
while(ppzuavimu_hmc5843.status == I2CTransPending);
ppzuavimu_hmc5843.type = I2CTransTx;
ppzuavimu_hmc5843.buf[0] = HMC5843_REG_MODE; // set to continuous mode
ppzuavimu_hmc5843.buf[1] = 0x00;
ppzuavimu_hmc5843.len_w = 2;
i2c_submit(&PPZUAVIMU_I2C_DEV,&ppzuavimu_hmc5843);
while(ppzuavimu_hmc5843.status == I2CTransPending);
*/
}
unsigned char i2cWrite(unsigned char data) {
data_reg = data;
i2c_transmit(DATA);
return 0;
}
void stop_com(void)
{
active_com = FALSE;
com_trans.buf[0] = active_com;
i2c_transmit(&GENERIC_COM_I2C_DEV, &com_trans, GENERIC_COM_SLAVE_ADDR, 1);
}
/**
* Poll px4flow for data
* 152 i2c frames are created per second, so the PX4FLOW can be polled
* at up to 150Hz (faster rate won't bring any finer resolution)
*/
void px4flow_i2c_periodic(void)
{
switch (px4flow.status) {
case PX4FLOW_FRAME_REQ:
// ask for i2c frame
px4flow.trans.buf[0] = PX4FLOW_I2C_FRAME;
if (i2c_transceive(&PX4FLOW_I2C_DEV, &px4flow.trans, px4flow.addr, 1, PX4FLOW_I2C_FRAME_LENGTH)) {
// transaction OK, increment status
px4flow.status = PX4FLOW_FRAME_REC;
}
break;
case PX4FLOW_FRAME_REC:
// check if the transaction was successful
if (px4flow.trans.status == I2CTransSuccess) {
// retrieve data
uint8_t idx = 0;
px4flow.i2c_frame.frame_count = (px4flow.trans.buf[idx + 1] << 8 | px4flow.trans.buf[idx]);
idx += sizeof(uint16_t);
px4flow.i2c_frame.pixel_flow_x_sum = (px4flow.trans.buf[idx + 1] << 8 | px4flow.trans.buf[idx]);
idx += sizeof(int16_t);
px4flow.i2c_frame.pixel_flow_y_sum = (px4flow.trans.buf[idx + 1] << 8 | px4flow.trans.buf[idx]);
idx += sizeof(int16_t);
px4flow.i2c_frame.flow_comp_m_x = (px4flow.trans.buf[idx + 1] << 8 | px4flow.trans.buf[idx]);
idx += sizeof(int16_t);
px4flow.i2c_frame.flow_comp_m_y = (px4flow.trans.buf[idx + 1] << 8 | px4flow.trans.buf[idx]);
idx += sizeof(int16_t);
px4flow.i2c_frame.qual = (px4flow.trans.buf[idx + 1] << 8 | px4flow.trans.buf[idx]);
idx += sizeof(int16_t);
px4flow.i2c_frame.gyro_x_rate = (px4flow.trans.buf[idx + 1] << 8 | px4flow.trans.buf[idx]);
idx += sizeof(int16_t);
px4flow.i2c_frame.gyro_y_rate = (px4flow.trans.buf[idx + 1] << 8 | px4flow.trans.buf[idx]);
idx += sizeof(int16_t);
px4flow.i2c_frame.gyro_z_rate = (px4flow.trans.buf[idx + 1] << 8 | px4flow.trans.buf[idx]);
idx += sizeof(int16_t);
px4flow.i2c_frame.gyro_range = px4flow.trans.buf[idx];
idx += sizeof(uint8_t);
px4flow.i2c_frame.sonar_timestamp = px4flow.trans.buf[idx];
idx += sizeof(uint8_t);
px4flow.i2c_frame.ground_distance = (px4flow.trans.buf[idx + 1] << 8 | px4flow.trans.buf[idx]);
// propagate measurements
px4flow_i2c_frame_cb();
}
// increment status
// ask for regular frame again
px4flow.status = PX4FLOW_FRAME_REQ;
break;
case PX4FLOW_INT_FRAME_REQ:
// Send the command to begin a measurement.
px4flow.trans.buf[0] = PX4FLOW_I2C_INTEGRAL_FRAME;
if (i2c_transmit(&PX4FLOW_I2C_DEV, &px4flow.trans, px4flow.addr, 1)) {
// transaction OK, increment status
px4flow.status = PX4FLOW_INT_FRAME_REC;
}
break;
case PX4FLOW_INT_FRAME_REC:
// ask for integral frame
px4flow.trans.buf[0] = PX4FLOW_I2C_INTEGRAL_FRAME;
if (i2c_transceive(&PX4FLOW_I2C_DEV, &px4flow.trans, px4flow.addr, 1, PX4FLOW_I2C_INTEGRAL_FRAME_LENGTH)) {
// transaction OK, increment status
px4flow.status = PX4FLOW_INT_FRAME_REC_OK;
}
break;
case PX4FLOW_INT_FRAME_REC_OK:
// check if the transaction was successful
if (px4flow.trans.status == I2CTransSuccess) {
// retrieve data
uint8_t idx = 0;
px4flow.i2c_int_frame.frame_count_since_last_readout = (px4flow.trans.buf[idx + 1] << 8 | px4flow.trans.buf[idx]);
idx += sizeof(uint16_t);
px4flow.i2c_int_frame.pixel_flow_x_integral = (px4flow.trans.buf[idx + 1] << 8 | px4flow.trans.buf[idx]);
idx += sizeof(int16_t);
px4flow.i2c_int_frame.pixel_flow_y_integral = (px4flow.trans.buf[idx + 1] << 8 | px4flow.trans.buf[idx]);
idx += sizeof(uint16_t);
px4flow.i2c_int_frame.gyro_x_rate_integral = (px4flow.trans.buf[idx + 1] << 8 | px4flow.trans.buf[idx]);
idx += sizeof(int16_t);
px4flow.i2c_int_frame.gyro_y_rate_integral = (px4flow.trans.buf[idx + 1] << 8 | px4flow.trans.buf[idx]);
idx += sizeof(int16_t);
px4flow.i2c_int_frame.gyro_z_rate_integral = (px4flow.trans.buf[idx + 1] << 8 | px4flow.trans.buf[idx]);
idx += sizeof(int16_t);
px4flow.i2c_int_frame.integration_timespan = (px4flow.trans.buf[idx + 3] << 24 | px4flow.trans.buf[idx + 2] << 16
| px4flow.trans.buf[idx + 1] << 8 | px4flow.trans.buf[idx]);
idx += sizeof(uint32_t);
px4flow.i2c_int_frame.sonar_timestamp = (px4flow.trans.buf[idx + 3] << 24 | px4flow.trans.buf[idx + 2] << 16
| px4flow.trans.buf[idx + 1] << 8 | px4flow.trans.buf[idx]);
idx += sizeof(uint32_t);
px4flow.i2c_int_frame.ground_distance = (px4flow.trans.buf[idx + 1] << 8 | px4flow.trans.buf[idx]);
idx += sizeof(int16_t);
px4flow.i2c_int_frame.gyro_temperature = (px4flow.trans.buf[idx + 1] << 8 | px4flow.trans.buf[idx]);
idx += sizeof(int16_t);
px4flow.i2c_int_frame.qual = px4flow.trans.buf[idx];
// propagate measurements
px4flow_i2c_int_frame_cb();
}
// increment status
px4flow.status = PX4FLOW_INT_FRAME_REQ;
break;
default:
break;
}
}
uint8_t i2c_stop (void)
{
return i2c_transmit (I2C_STOP);
}
