void imu_periodic( void )
{
// Start reading the latest gyroscope data
ppzuavimu_itg3200.type = I2CTransTxRx;
ppzuavimu_itg3200.len_r = 9;
ppzuavimu_itg3200.len_w = 1;
ppzuavimu_itg3200.buf[0] = ITG3200_REG_INT_STATUS;
i2c_submit(&PPZUAVIMU_I2C_DEVICE, &ppzuavimu_itg3200);
// Start reading the latest accelerometer data
ppzuavimu_adxl345.type = I2CTransTxRx;
ppzuavimu_adxl345.len_r = 6;
ppzuavimu_adxl345.len_w = 1;
ppzuavimu_adxl345.buf[0] = ADXL345_REG_DATA_X0;
i2c_submit(&PPZUAVIMU_I2C_DEVICE, &ppzuavimu_adxl345);
// 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_DEVICE, &ppzuavimu_hmc5843);
#if PERIODIC_FREQUENCY > 60
});
/* sends a serie of I2C commands to configure the ITG3200 gyro */
static void configure_gyro(void) {
struct i2c_transaction t;
t.type = I2CTransTx;
t.slave_addr = ITG3200_ADDR;
/* set gyro range to 2000deg/s and low pass at 256Hz */
t.buf[0] = ITG3200_REG_DLPF_FS;
t.buf[1] = (0x03<<3);
t.len_w = 2;
//send_i2c_msg_with_retry(&t);
i2c_submit(&(ITG3200_I2C_DEV), &t);
/* set sample rate to 533Hz */
t.buf[0] = ITG3200_REG_SMPLRT_DIV;
t.buf[1] = 0x0E;
//send_i2c_msg_with_retry(&t);
i2c_submit(&(ITG3200_I2C_DEV), &t);
/* switch to gyroX clock */
t.buf[0] = ITG3200_REG_PWR_MGM;
t.buf[1] = 0x01;
//send_i2c_msg_with_retry(&t);
i2c_submit(&(ITG3200_I2C_DEV), &t);
/* enable interrupt on data ready, idle high, latch until read any register */
t.buf[0] = ITG3200_REG_INT_CFG;
t.buf[1] = (0x01 | (0x1<<4) | (0x1<<5) | 0x01<<7);
//send_i2c_msg_with_retry(&t);
i2c_submit(&(ITG3200_I2C_DEV), &t);
}
/* sends a serie of I2C commands to configure the ITG3200 gyro */
static void configure_gyro(void) {
struct i2c_transaction t;
t.type = I2CTransTx;
t.slave_addr = ITG3200_ADDR;
/* set gyro range to 2000deg/s and low pass at 256Hz */
t.buf[0] = ITG3200_REG_DLPF_FS;
t.buf[1] = (0x03<<3);
t.len_w = 2;
i2c_submit(&i2c2,&t);
while (t.status != I2CTransSuccess);
/* set sample rate to 533Hz */
t.buf[0] = ITG3200_REG_SMPLRT_DIV;
t.buf[1] = 0x0E;
i2c_submit(&i2c2,&t);
while (t.status != I2CTransSuccess);
/* switch to gyroX clock */
t.buf[0] = ITG3200_REG_PWR_MGM;
t.buf[1] = 0x01;
i2c_submit(&i2c2,&t);
while (t.status != I2CTransSuccess);
/* enable interrupt on data ready, idle hight */
t.buf[0] = ITG3200_REG_INT_CFG;
t.buf[1] = (0x01 | 0x01<<7);
i2c_submit(&i2c2,&t);
while (t.status != I2CTransSuccess);
}
void hmc5843_periodic(void)
{
if (!hmc5843.initialized) {
send_config();
hmc5843.initialized = TRUE;
} else if (hmc5843.timeout++ > HMC5843_TIMEOUT && HMC5843_I2C_DEV.status == I2CIdle && i2c_idle(&HMC5843_I2C_DEV)) {
#ifdef USE_HMC59843_ARCH_RESET
hmc5843_arch_reset();
#endif
hmc5843.i2c_trans.type = I2CTransTx;
hmc5843.i2c_trans.len_w = 1;
hmc5843.i2c_trans.buf[0] = 0x3;
i2c_submit(&HMC5843_I2C_DEV, &hmc5843.i2c_trans);
while (hmc5843.i2c_trans.status == I2CTransPending || hmc5843.i2c_trans.status == I2CTransRunning);
hmc5843.i2c_trans.type = I2CTransRx;
hmc5843.i2c_trans.len_r = 6;
i2c_submit(&HMC5843_I2C_DEV, &hmc5843.i2c_trans);
while (hmc5843.i2c_trans.status == I2CTransPending || hmc5843.i2c_trans.status == I2CTransRunning);
hmc5843.timeout = 0;
}
#ifdef HMC5843_NO_IRQ
// < 50Hz
fake_mag_eoc = 1;
#endif
}
void baro_board_send_reset(void) {
baro_trans.type = I2CTransTx;
baro_trans.slave_addr = 0x00;
baro_trans.len_w = 1;
baro_trans.buf[0] = 0x06;
i2c_submit(&i2c2,&baro_trans);
}
static inline void baro_board_read_from_current_register(uint8_t baro_addr) {
baro_trans.type = I2CTransRx;
baro_trans.slave_addr = baro_addr;
baro_trans.len_r = 2;
i2c_submit(&i2c2,&baro_trans);
// i2c2_receive(baro_addr, 2, &baro_board.i2c_done);
}
void config_mkk_v2_send_eeprom(void)
{
// Do not upload while running
RETURN_IF_NOT_KILLMODE();
// Do not upload bad data:
if (config_mkk_v2_eeprom.revision != config_mkk_v2_EEPROM_REVISION)
return;
// New I2C Write Transaction
config_mkk_v2.trans.type = I2CTransTx;
config_mkk_v2.trans.slave_addr = 0x52 + config_mkk_v2.addr * 2;
config_mkk_v2.trans.len_w = 10;
config_mkk_v2.trans.buf[0] = 0;
config_mkk_v2.trans.buf[1] = (BL_READMODE_CONFIG<<3);
config_mkk_v2.trans.buf[2] = config_mkk_v2_eeprom.revision;
config_mkk_v2.trans.buf[3] = config_mkk_v2_eeprom.SetMask;
config_mkk_v2.trans.buf[4] = config_mkk_v2_eeprom.PwmScaling;
config_mkk_v2.trans.buf[5] = config_mkk_v2_eeprom.CurrentLimit;
config_mkk_v2.trans.buf[6] = config_mkk_v2_eeprom.TempLimit;
config_mkk_v2.trans.buf[7] = config_mkk_v2_eeprom.CurrentScaling;
config_mkk_v2.trans.buf[8] = config_mkk_v2_eeprom.BitConfig;
config_mkk_v2.trans.buf[9] = config_mkk_v2_crc(2);
i2c_submit(&ACTUATORS_MKK_V2_DEVICE, &config_mkk_v2.trans);
}
/** @file actuators_asctec.c
* Actuators driver for Asctec motor controllers.
*/
#include "firmwares/rotorcraft/actuators.h"
#include "firmwares/rotorcraft/actuators/actuators_asctec.h"
#ifdef ACTUATORS_ASCTEC_V2_PROTOCOL
#include "firmwares/rotorcraft/actuators/supervision.h"
#endif
#include "firmwares/rotorcraft/commands.h"
#include "mcu_periph/i2c.h"
#include "mcu_periph/sys_time.h"
struct ActuatorsAsctec actuators_asctec;
uint32_t actuators_delay_time;
bool_t actuators_delay_done;
void actuators_init(void) {
actuators_asctec.cmd = NONE;
actuators_asctec.cur_addr = FRONT;
actuators_asctec.new_addr = FRONT;
actuators_asctec.i2c_trans.status = I2CTransSuccess;
actuators_asctec.i2c_trans.type = I2CTransTx;
actuators_asctec.i2c_trans.slave_addr = 0x02;
#ifdef ACTUATORS_ASCTEC_V2_PROTOCOL
actuators_asctec.i2c_trans.len_w = 5;
#else
actuators_asctec.i2c_trans.len_w = 4;
#endif
actuators_asctec.nb_err = 0;
#if defined ACTUATORS_START_DELAY && ! defined SITL
actuators_delay_done = FALSE;
SysTimeTimerStart(actuators_delay_time);
#else
actuators_delay_done = TRUE;
actuators_delay_time = 0;
#endif
#ifdef ACTUATORS_ASCTEC_V2_PROTOCOL
supervision_init();
#endif
}
#ifndef ACTUATORS_ASCTEC_V2_PROTOCOL
void actuators_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
if (!actuators_asctec.i2c_trans.status == I2CTransSuccess)
actuators_asctec.nb_err++;
#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 */
actuators_asctec.cmds[PITCH] = commands[COMMAND_PITCH] + SUPERVISION_TRIM_E;
actuators_asctec.cmds[ROLL] = commands[COMMAND_ROLL] + SUPERVISION_TRIM_A;
actuators_asctec.cmds[YAW] = commands[COMMAND_YAW] + SUPERVISION_TRIM_R;
actuators_asctec.cmds[THRUST] = commands[COMMAND_THRUST];
Bound(actuators_asctec.cmds[PITCH],-100, 100);
Bound(actuators_asctec.cmds[ROLL], -100, 100);
Bound(actuators_asctec.cmds[YAW], -100, 100);
if (motors_on) {
Bound(actuators_asctec.cmds[THRUST], 1, 200);
}
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_submit(&ACTUATORS_ASCTEC_DEVICE, &actuators_asctec.i2c_trans);
}
#else /* ! ACTUATORS_ASCTEC_V2_PROTOCOL */
void actuators_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
supervision_run(motors_on, FALSE, commands);
#ifdef KILL_MOTORS
actuators_asctec.i2c_trans.buf[0] = 0;
actuators_asctec.i2c_trans.buf[1] = 0;
actuators_asctec.i2c_trans.buf[2] = 0;
actuators_asctec.i2c_trans.buf[3] = 0;
actuators_asctec.i2c_trans.buf[4] = 0xAA;
#else
actuators_asctec.i2c_trans.buf[0] = supervision.commands[SERVO_FRONT];
actuators_asctec.i2c_trans.buf[1] = supervision.commands[SERVO_BACK];
actuators_asctec.i2c_trans.buf[2] = supervision.commands[SERVO_LEFT];
actuators_asctec.i2c_trans.buf[3] = supervision.commands[SERVO_RIGHT];
actuators_asctec.i2c_trans.buf[4] = 0xAA + actuators_asctec.i2c_trans.buf[0] + actuators_asctec.i2c_trans.buf[1] +
actuators_asctec.i2c_trans.buf[2] + actuators_asctec.i2c_trans.buf[3];
#endif
i2c_submit(&ACTUATORS_ASCTEC_DEVICE, &actuators_asctec.i2c_trans);
}
void actuators_mkk_v2_set(void)
{
#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
// Read result
for (uint8_t i = 0; i < ACTUATORS_MKK_V2_NB; i++) {
if (actuators_mkk_v2.trans[i].type != I2CTransTx) {
actuators_mkk_v2.trans[i].type = I2CTransTx;
actuators_mkk_v2.data[i].Current = actuators_mkk_v2.trans[i].buf[0];
actuators_mkk_v2.data[i].MaxPWM = actuators_mkk_v2.trans[i].buf[1];
actuators_mkk_v2.data[i].Temperature = actuators_mkk_v2.trans[i].buf[2];
}
}
RunOnceEvery(10, actuators_mkk_v2_read());
for (uint8_t i = 0; i < ACTUATORS_MKK_V2_NB; i++) {
#ifdef KILL_MOTORS
actuators_mkk_v2.trans[i].buf[0] = 0;
actuators_mkk_v2.trans[i].buf[1] = 0;
#else
actuators_mkk_v2.trans[i].buf[0] = (actuators_mkk_v2.setpoint[i] >> 3);
actuators_mkk_v2.trans[i].buf[1] = actuators_mkk_v2.setpoint[i] & 0x07;
#endif
i2c_submit(&ACTUATORS_MKK_V2_DEVICE, &actuators_mkk_v2.trans[i]);
}
}
static inline void baro_board_write_to_register(uint8_t baro_addr, uint8_t reg_addr, uint8_t val_msb, uint8_t val_lsb) {
baro_trans.type = I2CTransTx;
baro_trans.slave_addr = baro_addr;
baro_trans.len_w = 3;
baro_trans.buf[0] = reg_addr;
baro_trans.buf[1] = val_msb;
baro_trans.buf[2] = val_lsb;
i2c_submit(&i2c2,&baro_trans);
}
bool_t i2c_transmit(struct i2c_periph* p, struct i2c_transaction* t,
uint8_t s_addr, uint8_t len)
{
t->type = I2CTransTx;
t->slave_addr = s_addr;
t->len_w = len;
t->len_r = 0;
return i2c_submit(p, t);
}
static void adxl345_i2c_tx_reg(struct Adxl345_I2c *adxl, uint8_t reg, uint8_t val)
{
adxl->i2c_trans.type = I2CTransTx;
adxl->i2c_trans.buf[0] = reg;
adxl->i2c_trans.buf[1] = val;
adxl->i2c_trans.len_r = 0;
adxl->i2c_trans.len_w = 2;
i2c_submit(adxl->i2c_p, &(adxl->i2c_trans));
}
void event_i2c_abuse_test(void)
{
if (i2c_idle(&I2C_ABUSE_PORT))
{
LED_ON(5); // green = idle
LED_OFF(4);
}
else
{
LED_ON(4); // red = busy
LED_OFF(5);
}
// Wait for I2C transaction object to be released by the I2C driver before changing anything
if ((i2c_abuse_test_counter < 12) && (i2c_abuse_test_counter > 3))
{
if ((i2c_test2.status == I2CTransFailed) || (i2c_test2.status == I2CTransSuccess))
{
//i2c_test2.slave_addr = 0x90;
i2c_test2.type = I2CTransRx;
i2c_test2.slave_addr = 0x92;
i2c_test2.len_r = 2;
i2c_submit(&I2C_ABUSE_PORT,&i2c_test2);
}
}
if ((i2c_test1.status == I2CTransFailed) || (i2c_test1.status == I2CTransSuccess))
{
if (i2c_abuse_test_counter < 16)
{
i2c_abuse_test_counter++;
}
else
{
// wait until ready:
if (i2c_idle(&I2C_ABUSE_PORT))
{
i2c_abuse_test_counter = 1;
i2c_setbitrate(&I2C_ABUSE_PORT, i2c_abuse_test_bitrate);
i2c_abuse_test_bitrate += 17000;
if (i2c_abuse_test_bitrate > 410000)
{
i2c_abuse_test_bitrate -= 410000;
}
}
}
if (i2c_abuse_test_counter < 16)
{
RunOnceEvery(100,LED_TOGGLE(I2C_ABUSE_LED));
i2c_abuse_send_transaction( i2c_abuse_test_counter );
}
}
}
static void lsm303dlhc_i2c_tx_reg(struct Lsm303dlhc_I2c *lsm, uint8_t reg, uint8_t val)
{
lsm->i2c_trans.type = I2CTransTx;
lsm->i2c_trans.buf[0] = reg;
lsm->i2c_trans.buf[1] = val;
lsm->i2c_trans.len_r = 0;
lsm->i2c_trans.len_w = 2;
i2c_submit(lsm->i2c_p, &(lsm->i2c_trans));
}
static inline void baro_board_set_current_register(uint8_t baro_addr, uint8_t reg_addr) {
baro_trans.type = I2CTransTx;
baro_trans.slave_addr = baro_addr;
baro_trans.len_w = 1;
baro_trans.buf[0] = reg_addr;
i2c_submit(&i2c2,&baro_trans);
// i2c2.buf[0] = reg_addr;
// i2c2_transmit(baro_addr, 1, &baro_board.i2c_done);
}
static inline void bmp085_read_reg24(uint8_t addr)
{
baro_trans.type = I2CTransTxRx;
baro_trans.slave_addr = BMP085_ADDR;
baro_trans.len_w = 1;
baro_trans.len_r = 3;
baro_trans.buf[0] = addr;
i2c_submit(&i2c2, &baro_trans);
}
static void itg3200_i2c_tx_reg(struct Itg3200 *itg, uint8_t reg, uint8_t val)
{
itg->i2c_trans.type = I2CTransTx;
itg->i2c_trans.buf[0] = reg;
itg->i2c_trans.buf[1] = val;
itg->i2c_trans.len_r = 0;
itg->i2c_trans.len_w = 2;
i2c_submit(itg->i2c_p, &(itg->i2c_trans));
}
bool_t i2c_transceive(struct i2c_periph* p, struct i2c_transaction* t,
uint8_t s_addr, uint8_t len_w, uint16_t len_r)
{
t->type = I2CTransTxRx;
t->slave_addr = s_addr;
t->len_w = len_w;
t->len_r = len_r;
return i2c_submit(p, t);
}
static inline void baro_board_read_from_register(uint8_t baro_addr, uint8_t reg_addr) {
baro_trans.type = I2CTransTxRx;
baro_trans.slave_addr = baro_addr;
baro_trans.len_w = 1;
baro_trans.len_r = 2;
baro_trans.buf[0] = reg_addr;
i2c_submit(&i2c2,&baro_trans);
// i2c2.buf[0] = reg_addr;
// i2c2_transceive(baro_addr, 1, 2, &baro_board.i2c_done);
}
static inline void bmp085_write_reg(uint8_t addr, uint8_t value)
{
baro_trans.type = I2CTransTx;
baro_trans.slave_addr = BMP085_ADDR;
baro_trans.len_w = 2;
baro_trans.buf[0] = addr;
baro_trans.buf[1] = value;
i2c_submit(&i2c2, &baro_trans);
while (baro_trans.status == I2CTransPending || baro_trans.status == I2CTransRunning);
}
// Normal reading
void itg3200_read(struct Itg3200 *itg)
{
if (itg->initialized && itg->i2c_trans.status == I2CTransDone) {
itg->i2c_trans.buf[0] = ITG3200_REG_INT_STATUS;
itg->i2c_trans.type = I2CTransTxRx;
itg->i2c_trans.len_r = 9;
itg->i2c_trans.len_w = 1;
i2c_submit(itg->i2c_p, &(itg->i2c_trans));
}
}
// Normal reading
void adxl345_i2c_read(struct Adxl345_I2c *adxl)
{
if (adxl->initialized && adxl->i2c_trans.status == I2CTransDone) {
adxl->i2c_trans.buf[0] = ADXL345_REG_DATA_X0;
adxl->i2c_trans.type = I2CTransTxRx;
adxl->i2c_trans.len_r = 6;
adxl->i2c_trans.len_w = 1;
i2c_submit(adxl->i2c_p, &(adxl->i2c_trans));
}
}
void hmc5843_idle_task(void)
{
if (hmc5843.i2c_trans.status == I2CTransFailed) {
hmc5843.sent_tx = 0;
hmc5843.sent_rx = 0;
}
if (hmc5843.i2c_trans.status == I2CTransRunning || hmc5843.i2c_trans.status == I2CTransPending) { return; }
if (hmc5843.initialized && mag_eoc() && !hmc5843.sent_tx && !hmc5843.sent_rx) {
if (HMC5843_I2C_DEV.status == I2CIdle && i2c_idle(&HMC5843_I2C_DEV)) {
hmc5843.i2c_trans.type = I2CTransTx;
hmc5843.i2c_trans.len_w = 1;
hmc5843.i2c_trans.buf[0] = 0x3;
i2c_submit(&HMC5843_I2C_DEV, &hmc5843.i2c_trans);
hmc5843.sent_tx = 1;
return;
}
}
if (hmc5843.sent_tx) {
hmc5843.i2c_trans.type = I2CTransRx;
hmc5843.i2c_trans.len_r = 6;
hmc5843.i2c_trans.len_w = 1;
hmc5843.i2c_trans.buf[0] = 0x3;
i2c_submit(&HMC5843_I2C_DEV, &hmc5843.i2c_trans);
hmc5843.sent_rx = 1;
hmc5843.sent_tx = 0;
return;
}
if (hmc5843.sent_rx && hmc5843.i2c_trans.status == I2CTransSuccess) {
hmc5843.sent_rx = 0;
hmc5843.sent_tx = 0;
hmc5843.timeout = 0;
hmc5843.data_available = TRUE;
memcpy(hmc5843.data.buf, (const void *) hmc5843.i2c_trans.buf, 6);
for (int i = 0; i < 3; i++) {
hmc5843.data.value[i] = bswap_16(hmc5843.data.value[i]);
}
}
}
static void send_i2c_msg_with_retry(struct i2c_transaction* t) {
uint8_t max_retry = 8;
uint8_t nb_retry = 0;
do {
i2c_submit(&i2c2, t);
while (t->status == I2CTransPending || t->status == I2CTransRunning);
if (t->status == I2CTransFailed)
nb_retry++;
}
while (t->status != I2CTransSuccess || nb_retry < max_retry);
}
static inline void main_periodic_task( void ) {
trans.type = I2CTransTx;
trans.buf[0] = 0x04;
trans.len_w = 1;
trans.slave_addr = 0x58;
i2c_submit(&ACTUATORS_MKK_DEV,&trans);
LED_PERIODIC();
}
// Normal reading
void lsm303dlhc_i2c_read(struct Lsm303dlhc *lsm)
{
if (lsm->i2c_trans.slave_addr == LSM303DLHC_ACC_ADDR) {
//if ((lsm->init_status.acc == LSM_CONF_ACC_CLR_INT_READ) && (lsm->i2c_trans.status == I2CTransDone)){
if (!(lsm->initialized) || (lsm->initialized && lsm->i2c_trans.status == I2CTransDone)) {
lsm->i2c_trans.buf[0] = LSM303DLHC_REG_OUT_X_L_A | 0x80;
lsm->i2c_trans.type = I2CTransTxRx;
lsm->i2c_trans.len_r = 6;
lsm->i2c_trans.len_w = 1;
i2c_submit(lsm->i2c_p, &(lsm->i2c_trans));
}
} else {
if (lsm->initialized && lsm->i2c_trans.status == I2CTransDone) {
lsm->i2c_trans.buf[0] = LSM303DLHC_REG_OUT_X_H_M;
lsm->i2c_trans.type = I2CTransTxRx;
lsm->i2c_trans.len_r = 6;
lsm->i2c_trans.len_w = 1;
i2c_submit(lsm->i2c_p, &(lsm->i2c_trans));
}
}
}
// FIXME: there should be no arch dependent code here!
static void send_i2c_msg_with_retry(struct i2c_transaction* t) {
uint8_t max_retry = 8;
uint8_t nb_retry = 0;
do {
i2c_submit(&i2c2, t);
while((I2C2_SR2 & I2C_SR2_BUSY));
//while(I2C_GetFlagStatus(I2C2, I2C_FLAG_BUSY));
while (t->status == I2CTransPending || t->status == I2CTransRunning);
if (t->status == I2CTransFailed)
nb_retry++;
}
while (t->status != I2CTransSuccess && nb_retry < max_retry);
}
static inline void main_periodic_task( void ) {
trans.type = I2CTransTx;
trans.slave_addr = 0x02;
trans.len_w = 4;
trans.buf[0] = 100;
trans.buf[1] = 100;
trans.buf[2] = 100;
trans.buf[3] = 1;
i2c_submit(&i2c1,&trans);
LED_PERIODIC();
}
// blocking, only intended to be called for initialization
static void send_config(void)
{
hmc5843.i2c_trans.type = I2CTransTx;
hmc5843.i2c_trans.buf[0] = HMC5843_REG_CFGA; // set to rate to 50Hz
hmc5843.i2c_trans.buf[1] = 0x00 | (0x06 << 2);
hmc5843.i2c_trans.len_w = 2;
i2c_submit(&HMC5843_I2C_DEV, &hmc5843.i2c_trans);
while (hmc5843.i2c_trans.status == I2CTransPending);
hmc5843.i2c_trans.type = I2CTransTx;
hmc5843.i2c_trans.buf[0] = HMC5843_REG_CFGB; // set to gain to 1 Gauss
hmc5843.i2c_trans.buf[1] = 0x01 << 5;
hmc5843.i2c_trans.len_w = 2;
i2c_submit(&HMC5843_I2C_DEV, &hmc5843.i2c_trans);
while (hmc5843.i2c_trans.status == I2CTransPending);
hmc5843.i2c_trans.type = I2CTransTx;
hmc5843.i2c_trans.buf[0] = HMC5843_REG_MODE; // set to continuous mode
hmc5843.i2c_trans.buf[1] = 0x00;
hmc5843.i2c_trans.len_w = 2;
i2c_submit(&HMC5843_I2C_DEV, &hmc5843.i2c_trans);
while (hmc5843.i2c_trans.status == I2CTransPending);
}
void hmc5843_periodic(void)
{
if (!hmc5843.initialized) {
send_config();
hmc5843.initialized = TRUE;
} else if (hmc5843.timeout++ > HMC5843_TIMEOUT) {
hmc5843_arch_reset();
hmc5843.i2c_trans.type = I2CTransRx;
hmc5843.i2c_trans.len_r = 7;
i2c_submit(&i2c2, &hmc5843.i2c_trans);
hmc5843.reading = TRUE;
hmc5843.ready_for_read = FALSE;
hmc5843.timeout = 0;
}
}
