void baro_scp_event(void)
{
if (scp_trans.status == I2CTransSuccess) {
if (baro_scp_status == BARO_SCP_RD_TEMP) {
/* read two byte temperature */
baro_scp_temperature = scp_trans.buf[0] << 8;
baro_scp_temperature |= scp_trans.buf[1];
if (baro_scp_temperature & 0x2000) {
baro_scp_temperature |= 0xC000;
}
baro_scp_temperature *= 5;
/* start one byte msb pressure */
scp_trans.buf[0] = SCP1000_DATARD8;
baro_scp_status = BARO_SCP_RD_PRESS_0;
i2c_transceive(&SCP_I2C_DEV, &scp_trans, SCP1000_SLAVE_ADDR, 1, 1);
}
else if (baro_scp_status == BARO_SCP_RD_PRESS_0) {
/* read one byte pressure */
baro_scp_pressure = scp_trans.buf[0] << 16;
/* start two byte lsb pressure */
scp_trans.buf[0] = SCP1000_DATARD16;
baro_scp_status = BARO_SCP_RD_PRESS_1;
i2c_transceive(&SCP_I2C_DEV, &scp_trans, SCP1000_SLAVE_ADDR, 1, 2);
}
else if (baro_scp_status == BARO_SCP_RD_PRESS_1) {
/* read two byte pressure */
baro_scp_pressure |= scp_trans.buf[0] << 8;
baro_scp_pressure |= scp_trans.buf[1];
baro_scp_pressure *= 25;
float pressure = (float) baro_scp_pressure;
AbiSendMsgBARO_ABS(BARO_SCP_SENDER_ID, pressure);
#ifdef SENSOR_SYNC_SEND
DOWNLINK_SEND_SCP_STATUS(DefaultChannel, DefaultDevice, &baro_scp_pressure, &baro_scp_temperature);
#endif
baro_scp_status = BARO_SCP_IDLE;
}
else { baro_scp_status = BARO_SCP_IDLE; }
}
}
// Normal reading
void l3g4200_read(struct L3g4200 *l3g)
{
if (l3g->initialized && l3g->i2c_trans.status == I2CTransDone) {
l3g->i2c_trans.buf[0] = L3G4200_REG_STATUS_REG;
i2c_transceive(l3g->i2c_p, &(l3g->i2c_trans), l3g->i2c_trans.slave_addr, 1, 9);
}
}
void humid_sht_periodic_i2c( void ) {
switch (sht_status) {
case SHT2_UNINIT:
/* do soft reset, then wait at least 15ms */
sht_status = SHT2_RESET;
sht_trans.buf[0] = SHT2_SOFT_RESET;
i2c_transmit(&SHT_I2C_DEV, &sht_trans, SHT_SLAVE_ADDR, 1);
break;
case SHT2_SERIAL:
/* get serial number part 1 */
sht_status = SHT2_SERIAL1;
sht_trans.buf[0] = 0xFA;
sht_trans.buf[1] = 0x0F;
i2c_transceive(&SHT_I2C_DEV, &sht_trans, SHT_SLAVE_ADDR, 2, 8);
break;
case SHT2_SERIAL1:
case SHT2_SERIAL2:
break;
default:
/* trigger temp measurement, no master hold */
sht_trans.buf[0] = SHT2_TRIGGER_TEMP;
sht_status = SHT2_TRIG_TEMP;
i2c_transmit(&SHT_I2C_DEV, &sht_trans, SHT_SLAVE_ADDR, 1);
/* send serial number every 30 seconds */
RunOnceEvery((4*30), DOWNLINK_SEND_SHT_I2C_SERIAL(DefaultChannel, DefaultDevice, &sht_serial1, &sht_serial2));
break;
}
}
void imu_periodic( void )
{
// Start reading the latest gyroscope data
aspirin2_mpu60x0.buf[0] = MPU60X0_REG_INT_STATUS;
i2c_transceive(&PPZUAVIMU_I2C_DEV, &aspirin2_mpu60x0, MPU60X0_ADDR, 1, 21);
/*
// Start reading the latest accelerometer data
ppzuavimu_adxl345.buf[0] = ADXL345_REG_DATA_X0;
i2c_transceive(&PPZUAVIMU_I2C_DEV, &ppzuavimu_adxl345, ADXL345_ADDR, 1, 6);
*/
// Start reading the latest magnetometer data
#if PERIODIC_FREQUENCY > 60
RunOnceEvery(2,{
#endif
/* ppzuavimu_hmc5843.type = I2CTransTxRx;
ppzuavimu_hmc5843.len_r = 6;
ppzuavimu_hmc5843.len_w = 1;
ppzuavimu_hmc5843.buf[0] = HMC5843_REG_DATXM;
i2c_submit(&PPZUAVIMU_I2C_DEV, &ppzuavimu_hmc5843);
*/
#if PERIODIC_FREQUENCY > 60
});
#endif
//RunOnceEvery(10,aspirin2_subsystem_downlink_raw());
}
void actuators_bebop_commit(void)
{
// Receive the status
actuators_bebop.i2c_trans.buf[0] = ACTUATORS_BEBOP_GET_OBS_DATA;
i2c_transceive(&i2c1, &actuators_bebop.i2c_trans, actuators_bebop.i2c_trans.slave_addr, 1, 13);
// Update status
electrical.vsupply = (actuators_bebop.i2c_trans.buf[9] + (actuators_bebop.i2c_trans.buf[8] << 8)) / 100;
// The 15th bit contains saturation information, so it needs to be removed to get the rpm
actuators_bebop.rpm_obs[0] = (actuators_bebop.i2c_trans.buf[1] + (actuators_bebop.i2c_trans.buf[0] << 8)) & ~(1<<15);
actuators_bebop.rpm_obs[1] = (actuators_bebop.i2c_trans.buf[3] + (actuators_bebop.i2c_trans.buf[2] << 8)) & ~(1<<15);
actuators_bebop.rpm_obs[2] = (actuators_bebop.i2c_trans.buf[5] + (actuators_bebop.i2c_trans.buf[4] << 8)) & ~(1<<15);
actuators_bebop.rpm_obs[3] = (actuators_bebop.i2c_trans.buf[7] + (actuators_bebop.i2c_trans.buf[6] << 8)) & ~(1<<15);
// When detected a suicide
actuators_bebop.i2c_trans.buf[10] = actuators_bebop.i2c_trans.buf[10] & 0x7;
if (actuators_bebop.i2c_trans.buf[11] == 2 && actuators_bebop.i2c_trans.buf[10] != 1) {
autopilot_set_motors_on(FALSE);
}
// Start the motors
if (actuators_bebop.i2c_trans.buf[10] != 4 && actuators_bebop.i2c_trans.buf[10] != 2 && autopilot_motors_on) {
// Reset the error
actuators_bebop.i2c_trans.buf[0] = ACTUATORS_BEBOP_CLEAR_ERROR;
i2c_transmit(&i2c1, &actuators_bebop.i2c_trans, actuators_bebop.i2c_trans.slave_addr, 1);
// Start the motors
actuators_bebop.i2c_trans.buf[0] = ACTUATORS_BEBOP_START_PROP;
#if BEBOP_VERSION2
actuators_bebop.i2c_trans.buf[1] = 0b00000110; // For Bebop version 2 some motors are reversed (FIXME: test final version)
#else
actuators_bebop.i2c_trans.buf[1] = 0b00000000;
#endif
i2c_transmit(&i2c1, &actuators_bebop.i2c_trans, actuators_bebop.i2c_trans.slave_addr, 2);
}
// Stop the motors
else if (actuators_bebop.i2c_trans.buf[10] == 4 && !autopilot_motors_on) {
actuators_bebop.i2c_trans.buf[0] = ACTUATORS_BEBOP_STOP_PROP;
i2c_transmit(&i2c1, &actuators_bebop.i2c_trans, actuators_bebop.i2c_trans.slave_addr, 1);
} else if (actuators_bebop.i2c_trans.buf[10] == 4 && autopilot_motors_on) {
// Send the commands
actuators_bebop.i2c_trans.buf[0] = ACTUATORS_BEBOP_SET_REF_SPEED;
actuators_bebop.i2c_trans.buf[1] = actuators_bebop.rpm_ref[0] >> 8;
actuators_bebop.i2c_trans.buf[2] = actuators_bebop.rpm_ref[0] & 0xFF;
actuators_bebop.i2c_trans.buf[3] = actuators_bebop.rpm_ref[1] >> 8;
actuators_bebop.i2c_trans.buf[4] = actuators_bebop.rpm_ref[1] & 0xFF;
actuators_bebop.i2c_trans.buf[5] = actuators_bebop.rpm_ref[2] >> 8;
actuators_bebop.i2c_trans.buf[6] = actuators_bebop.rpm_ref[2] & 0xFF;
actuators_bebop.i2c_trans.buf[7] = actuators_bebop.rpm_ref[3] >> 8;
actuators_bebop.i2c_trans.buf[8] = actuators_bebop.rpm_ref[3] & 0xFF;
actuators_bebop.i2c_trans.buf[9] = 0x00; //UNK?
#pragma GCC diagnostic push
#pragma GCC diagnostic ignored "-Wcast-qual"
actuators_bebop.i2c_trans.buf[10] = actuators_bebop_checksum((uint8_t *)actuators_bebop.i2c_trans.buf, 9);
#pragma GCC diagnostic pop
i2c_transmit(&i2c1, &actuators_bebop.i2c_trans, actuators_bebop.i2c_trans.slave_addr, 11);
}
void ads1114_read(struct ads1114_periph *p)
{
// Config done with success
// start new reading when previous is done (and read if success)
if (p->config_done && p->trans.status == I2CTransDone) {
p->trans.buf[0] = ADS1114_POINTER_CONV_REG;
i2c_transceive(&ADS1114_I2C_DEV, &(p->trans), p->i2c_addr, 1, 2);
}
}
uint8_t readPCAP01_SRAM(uint16_t s_add)
{
while (pcap01_trans.status == I2CTransPending);
pcap01_trans.buf[0] = 0x10 + (unsigned char)(s_add >> 8);
pcap01_trans.buf[1] = (unsigned char)(s_add);
i2c_transceive(&PCAP01_I2C_DEV, &pcap01_trans, PCAP01_ADDR, 2, 1);
while (pcap01_trans.status == I2CTransPending);
return pcap01_trans.buf[0];
}
void actuators_bebop_commit(void)
{
// Receive the status
actuators_bebop.i2c_trans.buf[0] = ACTUATORS_BEBOP_GET_OBS_DATA;
i2c_transceive(&i2c1, &actuators_bebop.i2c_trans, actuators_bebop.i2c_trans.slave_addr, 1, 13);
// Update status
electrical.vsupply = (actuators_bebop.i2c_trans.buf[9] + (actuators_bebop.i2c_trans.buf[8] << 8)) / 100;
actuators_bebop.rpm_obs[0] = (actuators_bebop.i2c_trans.buf[1] + (actuators_bebop.i2c_trans.buf[0] << 8));
actuators_bebop.rpm_obs[1] = (actuators_bebop.i2c_trans.buf[3] + (actuators_bebop.i2c_trans.buf[2] << 8));
actuators_bebop.rpm_obs[2] = (actuators_bebop.i2c_trans.buf[5] + (actuators_bebop.i2c_trans.buf[4] << 8));
actuators_bebop.rpm_obs[3] = (actuators_bebop.i2c_trans.buf[7] + (actuators_bebop.i2c_trans.buf[6] << 8));
// Saturate the bebop motors
//actuators_bebop_saturate();
// When detected a suicide
actuators_bebop.i2c_trans.buf[10] = actuators_bebop.i2c_trans.buf[10] & 0x7;
if (actuators_bebop.i2c_trans.buf[11] == 2 && actuators_bebop.i2c_trans.buf[10] != 1) {
autopilot_set_motors_on(FALSE);
}
// Start the motors
if (actuators_bebop.i2c_trans.buf[10] != 4 && actuators_bebop.i2c_trans.buf[10] != 2 && autopilot_motors_on) {
// Reset the error
actuators_bebop.i2c_trans.buf[0] = ACTUATORS_BEBOP_CLEAR_ERROR;
i2c_transmit(&i2c1, &actuators_bebop.i2c_trans, actuators_bebop.i2c_trans.slave_addr, 1);
// Start the motors
actuators_bebop.i2c_trans.buf[0] = ACTUATORS_BEBOP_START_PROP;
i2c_transmit(&i2c1, &actuators_bebop.i2c_trans, actuators_bebop.i2c_trans.slave_addr, 1);
}
// Stop the motors
else if (actuators_bebop.i2c_trans.buf[10] == 4 && !autopilot_motors_on) {
actuators_bebop.i2c_trans.buf[0] = ACTUATORS_BEBOP_STOP_PROP;
i2c_transmit(&i2c1, &actuators_bebop.i2c_trans, actuators_bebop.i2c_trans.slave_addr, 1);
} else if (actuators_bebop.i2c_trans.buf[10] == 4 && autopilot_motors_on) {
// Send the commands
actuators_bebop.i2c_trans.buf[0] = ACTUATORS_BEBOP_SET_REF_SPEED;
actuators_bebop.i2c_trans.buf[1] = actuators_bebop.rpm_ref[0] >> 8;
actuators_bebop.i2c_trans.buf[2] = actuators_bebop.rpm_ref[0] & 0xFF;
actuators_bebop.i2c_trans.buf[3] = actuators_bebop.rpm_ref[1] >> 8;
actuators_bebop.i2c_trans.buf[4] = actuators_bebop.rpm_ref[1] & 0xFF;
actuators_bebop.i2c_trans.buf[5] = actuators_bebop.rpm_ref[2] >> 8;
actuators_bebop.i2c_trans.buf[6] = actuators_bebop.rpm_ref[2] & 0xFF;
actuators_bebop.i2c_trans.buf[7] = actuators_bebop.rpm_ref[3] >> 8;
actuators_bebop.i2c_trans.buf[8] = actuators_bebop.rpm_ref[3] & 0xFF;
actuators_bebop.i2c_trans.buf[9] = 0x00; //UNK?
#pragma GCC diagnostic push
#pragma GCC diagnostic ignored "-Wcast-qual"
actuators_bebop.i2c_trans.buf[10] = actuators_bebop_checksum((uint8_t *)actuators_bebop.i2c_trans.buf, 9);
#pragma GCC diagnostic pop
i2c_transmit(&i2c1, &actuators_bebop.i2c_trans, actuators_bebop.i2c_trans.slave_addr, 11);
}
/**
* Periodic function to ensure proper delay after triggering reset or conversion.
* Should run at 100Hz max.
* Typical conversion time is 8.22ms at max resolution.
*/
void ms5611_i2c_periodic_check(struct Ms5611_I2c *ms)
{
switch (ms->status) {
case MS5611_STATUS_RESET:
ms->status = MS5611_STATUS_RESET_OK;
break;
case MS5611_STATUS_RESET_OK:
if (ms->i2c_trans.status == I2CTransDone) {
/* start getting prom data */
ms->i2c_trans.buf[0] = MS5611_PROM_READ | (ms->prom_cnt << 1);
i2c_transceive(ms->i2c_p, &(ms->i2c_trans), ms->i2c_trans.slave_addr, 1, 2);
ms->status = MS5611_STATUS_PROM;
}
break;
case MS5611_STATUS_CONV_D1:
ms->status = MS5611_STATUS_CONV_D1_OK;
break;
case MS5611_STATUS_CONV_D1_OK:
if (ms->i2c_trans.status == I2CTransDone) {
/* read D1 adc */
ms->i2c_trans.buf[0] = MS5611_ADC_READ;
i2c_transceive(ms->i2c_p, &(ms->i2c_trans), ms->i2c_trans.slave_addr, 1, 3);
ms->status = MS5611_STATUS_ADC_D1;
}
break;
case MS5611_STATUS_CONV_D2:
ms->status = MS5611_STATUS_CONV_D2_OK;
break;
case MS5611_STATUS_CONV_D2_OK:
if (ms->i2c_trans.status == I2CTransDone) {
/* read D2 adc */
ms->i2c_trans.buf[0] = MS5611_ADC_READ;
i2c_transceive(ms->i2c_p, &(ms->i2c_trans), ms->i2c_trans.slave_addr, 1, 3);
ms->status = MS5611_STATUS_ADC_D2;
}
break;
default:
break;
}
}
void pbn_periodic(void)
{
if (startup_delay > 0) {
--startup_delay;
return;
}
// Initiate next read
pbn_trans.buf[0] = 0;
i2c_transceive(&PBN_I2C_DEV, &pbn_trans, PBN_I2C_ADDR, 1, 4);
}
void atmega_i2c_cam_ctrl_send(uint8_t cmd)
{
atmega_i2c_cam_ctrl_just_sent_command = 1;
// Send Command
atmega_i2c_cam_ctrl_trans.buf[0] = cmd;
i2c_transceive(&ATMEGA_I2C_DEV, &atmega_i2c_cam_ctrl_trans, ATMEGA_SLAVE_ADDR, 1, 1);
if (cmd == DC_SHOOT)
{
dc_send_shot_position();
}
}
// Normal reading
void mpl3115_read(struct Mpl3115 *mpl)
{
// ask for a reading and then prepare next conversion
if (mpl->initialized && mpl->trans.status == I2CTransDone) {
mpl->trans.buf[0] = MPL3115_REG_STATUS;
i2c_transceive(mpl->i2c_p, &mpl->trans, mpl->trans.slave_addr, 1, 6);
if (mpl->req_trans.status == I2CTransDone) {
mpl->req_trans.buf[0] = MPL3115_REG_CTRL_REG1;
mpl->req_trans.buf[1] = ((MPL3115_OVERSAMPLING << 3) | (mpl->raw_mode << 6) |
(mpl->alt_mode << 7) | MPL3115_OST_BIT);
i2c_transmit(mpl->i2c_p, &mpl->req_trans, mpl->trans.slave_addr, 2);
}
}
}
void baro_scp_periodic( void ) {
if (baro_scp_status == BARO_SCP_UNINIT && sys_time.nb_sec > 1) {
baro_scp_start_high_res_measurement();
baro_scp_status = BARO_SCP_IDLE;
} else if (baro_scp_status == BARO_SCP_IDLE) {
/* init: start two byte temperature */
scp_trans.buf[0] = SCP1000_TEMPOUT;
baro_scp_status = BARO_SCP_RD_TEMP;
i2c_transceive(&SCP_I2C_DEV, &scp_trans, SCP1000_SLAVE_ADDR, 1, 2);
}
}
void mpu9250_i2c_read(struct Mpu9250_I2c *mpu)
{
if (mpu->config.initialized && mpu->i2c_trans.status == I2CTransDone) {
/* set read bit and multiple byte bit, then address */
mpu->i2c_trans.buf[0] = MPU9250_REG_INT_STATUS;
i2c_transceive(mpu->i2c_p, &(mpu->i2c_trans), mpu->i2c_trans.slave_addr, 1, mpu->config.nb_bytes);
/* read mag */
#ifdef MPU9250_MAG_PRESCALER
RunOnceEvery(MPU9250_MAG_PRESCALER, ak8963_read(&mpu->akm));
#else
ak8963_read(&mpu->akm);
#endif
}
}
/**
* 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;
}
}
/**
* Poll lidar for data
*/
void lidar_sf11_periodic(void)
{
switch (lidar_sf11.status) {
case LIDAR_SF11_REQ_READ:
// read two bytes
lidar_sf11.trans.buf[0] = 0; // set tx to zero
i2c_transceive(&LIDAR_SF11_I2C_DEV, &lidar_sf11.trans, lidar_sf11.addr, 1, 2);
break;
case LIDAR_SF11_READ_OK:
// process results
// filter data
lidar_sf11.distance_raw = update_median_filter(
&lidar_sf11_filter,
(uint32_t)((lidar_sf11.trans.buf[0] << 8) | lidar_sf11.trans.buf[1]));
lidar_sf11.distance = ((float)lidar_sf11.distance_raw)/100.0;
// compensate AGL measurement for body rotation
if (lidar_sf11.compensate_rotation) {
float phi = stateGetNedToBodyEulers_f()->phi;
float theta = stateGetNedToBodyEulers_f()->theta;
float gain = (float)fabs( (double) (cosf(phi) * cosf(theta)));
lidar_sf11.distance = lidar_sf11.distance / gain;
}
// send message (if requested)
if (lidar_sf11.update_agl) {
AbiSendMsgAGL(AGL_LIDAR_SF11_ID, lidar_sf11.distance);
}
// reset status
lidar_sf11.status = LIDAR_SF11_REQ_READ;
break;
default:
break;
}
}
/**
* 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;
}
}
void humid_sht_event_i2c( void ) {
if (sht_trans.status == I2CTransSuccess) {
switch (sht_status) {
case SHT2_TRIG_TEMP:
sht_status = SHT2_GET_TEMP;
sht_trans.status = I2CTransDone;
break;
case SHT2_READ_TEMP:
/* read temperature */
tempsht_i2c = (sht_trans.buf[0] << 8) | sht_trans.buf[1];
tempsht_i2c &= 0xFFFC;
if (humid_sht_crc(sht_trans.buf) == 0) {
/* trigger humid measurement, no master hold */
sht_trans.buf[0] = SHT2_TRIGGER_HUMID;
sht_status = SHT2_TRIG_HUMID;
i2c_transmit(&SHT_I2C_DEV, &sht_trans, SHT_SLAVE_ADDR, 1);
}
else {
/* checksum error, restart */
sht_status = SHT2_IDLE;
sht_trans.status == I2CTransDone;
}
break;
case SHT2_TRIG_HUMID:
sht_status = SHT2_GET_HUMID;
sht_trans.status = I2CTransDone;
break;
case SHT2_READ_HUMID:
/* read humidity */
humidsht_i2c = (sht_trans.buf[0] << 8) | sht_trans.buf[1];
humidsht_i2c &= 0xFFFC;
fhumidsht_i2c = -6. + 125. / 65536. * humidsht_i2c;
ftempsht_i2c = -46.85 + 175.72 / 65536. * tempsht_i2c;
sht_status = SHT2_IDLE;
sht_trans.status = I2CTransDone;
if (humid_sht_crc(sht_trans.buf) == 0) {
DOWNLINK_SEND_SHT_I2C_STATUS(DefaultChannel, DefaultDevice, &humidsht_i2c, &tempsht_i2c, &fhumidsht_i2c, &ftempsht_i2c);
}
break;
case SHT2_RESET:
sht_status = SHT2_SERIAL;
sht_trans.status = I2CTransDone;
break;
case SHT2_SERIAL1:
/* read serial number part 1 */
sht_serial[5] = sht_trans.buf[0];
sht_serial[4] = sht_trans.buf[2];
sht_serial[3] = sht_trans.buf[4];
sht_serial[2] = sht_trans.buf[6];
/* get serial number part 2 */
sht_status = SHT2_SERIAL2;
sht_trans.buf[0] = 0xFC;
sht_trans.buf[1] = 0xC9;
i2c_transceive(&SHT_I2C_DEV, &sht_trans, SHT_SLAVE_ADDR, 2, 6);
break;
case SHT2_SERIAL2:
/* read serial number part 2 */
sht_serial[1] = sht_trans.buf[0];
sht_serial[0] = sht_trans.buf[1];
sht_serial[7] = sht_trans.buf[3];
sht_serial[6] = sht_trans.buf[4];
sht_serial1=sht_serial[7]<<24|sht_serial[6]<<16|sht_serial[5]<<8|sht_serial[4];
sht_serial2=sht_serial[3]<<24|sht_serial[2]<<16|sht_serial[1]<<8|sht_serial[0];
DOWNLINK_SEND_SHT_I2C_SERIAL(DefaultChannel, DefaultDevice, &sht_serial1, &sht_serial2);
sht_status = SHT2_IDLE;
sht_trans.status = I2CTransDone;
break;
default:
sht_trans.status = I2CTransDone;
break;
}
}
}
void tmp102_periodic( void ) {
tmp_trans.buf[0] = TMP102_TEMP_REG;
i2c_transceive(&TMP_I2C_DEV, &tmp_trans, TMP102_SLAVE_ADDR, 1, 2);
tmp_meas_started = TRUE;
}
void lm75_periodic( void ) {
lm75_trans.buf[0] = LM75_TEMP_REG;
i2c_transceive(&LM75_I2C_DEV, &lm75_trans, LM75_SLAVE_ADDR, 1, 2);
}
void ms5611_i2c_event(struct Ms5611_I2c *ms) {
if (ms->initialized) {
if (ms->i2c_trans.status == I2CTransFailed) {
ms->status = MS5611_STATUS_IDLE;
ms->i2c_trans.status = I2CTransDone;
}
else if (ms->i2c_trans.status == I2CTransSuccess) {
// Successfull reading
switch (ms->status) {
case MS5611_STATUS_ADC_D1:
/* read D1 (pressure) */
ms->data.d1 = (ms->i2c_trans.buf[0] << 16) |
(ms->i2c_trans.buf[1] << 8) |
ms->i2c_trans.buf[2];
ms->i2c_trans.status = I2CTransDone;
if (ms->data.d1 == 0) {
/* if value is zero, it was read to soon and is invalid, back to idle */
ms->status = MS5611_STATUS_IDLE;
}
else {
/* start D2 conversion */
ms->i2c_trans.buf[0] = MS5611_START_CONV_D2;
i2c_transmit(ms->i2c_p, &(ms->i2c_trans), ms->i2c_trans.slave_addr, 1);
ms->status = MS5611_STATUS_CONV_D2;
}
break;
case MS5611_STATUS_ADC_D2:
/* read D2 (temperature) */
ms->data.d2 = (ms->i2c_trans.buf[0] << 16) |
(ms->i2c_trans.buf[1] << 8) |
ms->i2c_trans.buf[2];
ms->i2c_trans.status = I2CTransDone;
if (ms->data.d2 == 0) {
/* if value is zero, it was read to soon and is invalid, back to idle */
ms->status = MS5611_STATUS_IDLE;
}
else {
/* calculate temp and pressure from measurements and set available if valid */
if (ms5611_calc(&(ms->data)))
ms->data_available = TRUE;
ms->status = MS5611_STATUS_IDLE;
}
break;
default:
ms->i2c_trans.status = I2CTransDone;
break;
}
}
}
else if (ms->status != MS5611_STATUS_UNINIT) { // Configuring but not yet initialized
switch (ms->i2c_trans.status) {
case I2CTransFailed:
/* try again */
ms->status = MS5611_STATUS_UNINIT;
ms->i2c_trans.status = I2CTransDone;
break;
case I2CTransSuccess:
if (ms->status == MS5611_STATUS_PROM) {
/* read prom data */
ms->data.c[ms->prom_cnt++] = (ms->i2c_trans.buf[0] << 8) |
ms->i2c_trans.buf[1];
ms->i2c_trans.status = I2CTransDone;
if (ms->prom_cnt < PROM_NB) {
/* get next prom data */
ms->i2c_trans.buf[0] = MS5611_PROM_READ | (ms->prom_cnt << 1);
i2c_transceive(ms->i2c_p, &(ms->i2c_trans), ms->i2c_trans.slave_addr, 1, 2);
}
else {
/* done reading prom, check prom crc */
if (ms5611_prom_crc_ok(ms->data.c)) {
ms->initialized = TRUE;
ms->status = MS5611_STATUS_IDLE;
}
else {
/* checksum error, try again */
ms->status = MS5611_STATUS_UNINIT;
}
}
}
break;
default:
ms->i2c_trans.status = I2CTransDone;
break;
}
}
}
void pcap01readRegister(uint8_t reg)
{
uint16_t byte1 = 0x40 | reg;
pcap01_trans.buf[0] = byte1;
i2c_transceive(&PCAP01_I2C_DEV, &pcap01_trans, PCAP01_ADDR, 1, 3);
}