void infrared_i2c_hor_event( void ) {
#if ! (defined SITL || defined HITL)
irh_trans.status = I2CTransDone;
switch (ir_i2c_hor_status) {
case IR_I2C_IDLE :
break;
case IR_I2C_READ_IR1 :
if (bit_is_set(irh_trans.buf[2],7)) {
I2CReceive(i2c0, irh_trans, IR_HOR_I2C_ADDR, 3);
break;
}
// Read IR1 value
int16_t ir1 = (irh_trans.buf[0]<<8) | irh_trans.buf[1];
ir1 = ir1 - (IR_I2C_IR1_NEUTRAL << ir_i2c_conf_word);
ir_i2c.ir1 = FilterIR(ir_i2c.ir1, ir1);
// Select IR2 channel
irh_trans.buf[0] = IR_HOR_I2C_SELECT_IR2 | IR_HOR_OC_BIT | ir_i2c_conf_word | IR_START_CONV;
I2CTransmit(i2c0, irh_trans, IR_HOR_I2C_ADDR, 1);
ir_i2c_hor_status = IR_I2C_IR2_SELECTED;
break;
case IR_I2C_IR2_SELECTED :
// IR2 selected, asking for IR2 value
I2CReceive(i2c0, irh_trans, IR_HOR_I2C_ADDR, 3);
ir_i2c_hor_status = IR_I2C_READ_IR2;
break;
case IR_I2C_READ_IR2 :
// Read IR2 value
if (bit_is_set(irh_trans.buf[2],7)) {
I2CReceive(i2c0, irh_trans, IR_HOR_I2C_ADDR, 3);
break;
}
int16_t ir2 = (irh_trans.buf[0]<<8) | irh_trans.buf[1];
ir2 = ir2 - (IR_I2C_IR2_NEUTRAL << ir_i2c_conf_word);
ir_i2c.ir2 = FilterIR(ir_i2c.ir2, ir2);
// Update estimator
ir_i2c_data_hor_available = TRUE;
#ifndef IR_I2C_READ_ONLY
if (ir_i2c_data_ver_available) {
ir_i2c_data_hor_available = FALSE;
ir_i2c_data_ver_available = FALSE;
UpdateIRValue(ir_i2c);
}
#endif
// Select IR1 channel
irh_trans.buf[0] = IR_HOR_I2C_SELECT_IR1 | IR_HOR_OC_BIT | ir_i2c_conf_word | IR_START_CONV;
I2CTransmit(i2c0, irh_trans, IR_HOR_I2C_ADDR, 1);
ir_i2c_hor_status = IR_I2C_IR1_SELECTED;
break;
case IR_I2C_IR1_SELECTED :
// End reading cycle
ir_i2c_hor_status = IR_I2C_IDLE;
break;
case IR_I2C_CONFIGURE_HOR :
// End conf cycle
ir_i2c_conf_hor_done = TRUE;
ir_i2c_hor_status = IR_I2C_IDLE;
break;
}
#endif /* !SITL && !HITL */
}
/** \brief Update \a ir_roll and ir_pitch from ADCs or from simulator
* message in HITL and SITL modes
*/
void infrared_adc_update(void) {
#if ! (defined SITL || defined HITL)
ir_adc.ir1 = (int32_t)(buf_ir1.sum/buf_ir1.av_nb_sample) - IR_ADC_IR1_NEUTRAL;
ir_adc.ir2 = (int32_t)(buf_ir2.sum/buf_ir2.av_nb_sample) - IR_ADC_IR2_NEUTRAL;
#ifdef ADC_CHANNEL_IR_TOP
ir_adc.ir3 = (int32_t)(buf_ir3.sum/buf_ir3.av_nb_sample) - IR_ADC_TOP_NEUTRAL;
#endif // IR_TOP
#ifndef IR_ADC_READ_ONLY
UpdateIRValue(ir_adc);
#endif
#endif /* !SITL && !HITL */
/** #else ir_roll set by simulator in sim_ir.c */
}
void infrared_i2c_ver_event( void ) {
#if ! (defined SITL || defined HITL)
irv_trans.status = I2CTransDone;
// Read TOP value
if (irv_trans.type == I2CTransRx) {
int16_t ir3 = (irv_trans.buf[0]<<8) | irv_trans.buf[1];
ir3 = ir3 - (IR_I2C_TOP_NEUTRAL << ir_i2c_conf_word);
ir_i2c.ir3 = FilterIR(ir_i2c.ir3, ir3);
ir_i2c_data_ver_available = TRUE;
#ifndef IR_I2C_READ_ONLY
if (ir_i2c_data_hor_available) {
ir_i2c_data_hor_available = FALSE;
ir_i2c_data_ver_available = FALSE;
UpdateIRValue(ir_i2c);
}
#endif
}
if (irv_trans.type == I2CTransTx) {
ir_i2c_conf_ver_done = TRUE;
}
#endif /* !SITL && !HITL */
}
