void mb_twi_controller_set( float throttle ) {
if (mb_twi_i2c_done) {
if (mb_twi_controller_asctech_command) {
mb_twi_controller_asctech_command = FALSE;
switch (mb_twi_controller_asctech_command_type) {
case MB_TWI_CONTROLLER_COMMAND_TEST :
i2c0_buf[0] = 251;
i2c0_buf[1] = mb_twi_controller_asctech_addr;
i2c0_buf[2] = 0;
i2c0_buf[3] = 231 + mb_twi_controller_asctech_addr;
// mb_twi_i2c_done = FALSE;
i2c0_transmit(MB_TWI_CONTROLLER_ADDR, 4, &mb_twi_i2c_done);
break;
case MB_TWI_CONTROLLER_COMMAND_REVERSE :
i2c0_buf[0] = 254;
i2c0_buf[1] = mb_twi_controller_asctech_addr;
i2c0_buf[2] = 0;
i2c0_buf[3] = 234 + mb_twi_controller_asctech_addr;
// mb_twi_i2c_done = FALSE;
i2c0_transmit(MB_TWI_CONTROLLER_ADDR, 4, &mb_twi_i2c_done);
break;
case MB_TWI_CONTROLLER_COMMAND_SET_ADDR :
i2c0_buf[0] = 250;
i2c0_buf[1] = mb_twi_controller_asctech_addr;
i2c0_buf[2] = mb_twi_controller_asctech_new_addr;
i2c0_buf[3] = 230 + mb_twi_controller_asctech_addr +
mb_twi_controller_asctech_new_addr;
mb_twi_controller_asctech_addr = mb_twi_controller_asctech_new_addr;
// mb_twi_i2c_done = FALSE;
i2c0_transmit(MB_TWI_CONTROLLER_ADDR, 4, &mb_twi_i2c_done);
break;
}
}
else {
uint8_t pitch = 100;
uint8_t roll = 100;
uint8_t yaw = 100;
uint8_t power = throttle * MB_TWI_CONTROLLER_MAX_CMD;
i2c0_buf[0] = pitch;
i2c0_buf[1] = roll;
i2c0_buf[2] = yaw;
i2c0_buf[3] = power;
// mb_twi_i2c_done = FALSE;
i2c0_transmit(MB_TWI_CONTROLLER_ADDR, 4, &mb_twi_i2c_done);
}
}
else
mb_twi_nb_overun++;
}
void enose_periodic(void)
{
enose_PID_val = buf_PID.sum / buf_PID.av_nb_sample;
if (enose_i2c_done) {
if (enose_conf_requested) {
const uint8_t msg[] = { ENOSE_PWM_ADDR, enose_heat[0], enose_heat[1], enose_heat[2] };
memcpy((void *)i2c0_buf, msg, sizeof(msg));
i2c0_transmit(ENOSE_SLAVE_ADDR, sizeof(msg), &enose_i2c_done);
enose_i2c_done = FALSE;
enose_conf_requested = FALSE;
} else if (enose_status == ENOSE_IDLE) {
enose_status = ENOSE_MEASURING_WR;
const uint8_t msg[] = { ENOSE_DATA_ADDR };
memcpy((void *)i2c0_buf, msg, sizeof(msg));
i2c0_transmit(ENOSE_SLAVE_ADDR, sizeof(msg), &enose_i2c_done);
enose_i2c_done = FALSE;
} else if (enose_status == ENOSE_MEASURING_WR) {
enose_status = ENOSE_MEASURING_RD;
i2c0_receive(ENOSE_SLAVE_ADDR, 6, &enose_i2c_done);
enose_i2c_done = FALSE;
} else if (enose_status == ENOSE_MEASURING_RD) {
uint16_t val = (i2c0_buf[0] << 8) | i2c0_buf[1];
if (val < 5000) {
enose_val[0] = val;
}
val = (i2c0_buf[2] << 8) | i2c0_buf[3];
if (val < 5000) {
enose_val[1] = val;
}
val = (i2c0_buf[4] << 8) | i2c0_buf[5];
if (val < 5000) {
enose_val[2] = val;
}
enose_status = ENOSE_IDLE;
}
}
DOWNLINK_SEND_ENOSE_STATUS(DefaultChannel, DefaultDevice, &enose_val[0], &enose_val[1], &enose_val[2], &enose_PID_val,
3, enose_heat);
}
