void hmc5883l_measure(FILE *uart) {
static char reinitialize = 1;
uint8_t b3[1] = { 0x3 };
uint8_t data[6];
int16_t axis[3];
int8_t rv;
if (reinitialize) {
hmc5883l_init();
reinitialize = 0;
}
// Read measurement
rv = i2c_write(HMC5883L_ADDRESS, b3, sizeof(b3));
if (rv < 0) {
reinitialize = 1;
return;
}
rv = i2c_read(HMC5883L_ADDRESS, data, sizeof(data));
if (rv < 0) {
reinitialize = 1;
return;
}
// Post-process measurement
axis[0 /* X */] = (data[0] << 8) | data[1];
axis[2 /* Z */] = (data[2] << 8) | data[3];
axis[1 /* Y */] = (data[4] << 8) | data[5];
fprintf(uart, "X: %5d, Y: %5d, Z: %5d\r\n", axis[0], axis[1], axis[2]);
}
void hmc5883l_test()
{
hmc5883l_cmps cmps;
hmc5883l_init();
while(1)
{
cmps = hmc5883l_get_cmps();
xprintf("cmps_X:%d cmp_Y:%d cmps_Z:%d cmps_Angle:%d \r\n", cmps.X, cmps.Y, cmps.Z, cmps.Angle);
nrf_delay_ms(200);
}
}
/** Configures the board hardware and chip peripherals for the demo's functionality. */
void SetupHardware(void)
{
#if (ARCH == ARCH_AVR8)
/* Disable watchdog if enabled by bootloader/fuses */
MCUSR &= ~(1 << WDRF);
wdt_disable();
/* Disable clock division */
clock_prescale_set(clock_div_1);
#elif (ARCH == ARCH_XMEGA)
/* Start the PLL to multiply the 2MHz RC oscillator to 32MHz and switch the CPU core to run from it */
XMEGACLK_StartPLL(CLOCK_SRC_INT_RC2MHZ, 2000000, F_CPU);
XMEGACLK_SetCPUClockSource(CLOCK_SRC_PLL);
/* Start the 32MHz internal RC oscillator and start the DFLL to increase it to 48MHz using the USB SOF as a reference */
XMEGACLK_StartInternalOscillator(CLOCK_SRC_INT_RC32MHZ);
XMEGACLK_StartDFLL(CLOCK_SRC_INT_RC32MHZ, DFLL_REF_INT_USBSOF, F_USB);
PMIC.CTRL = PMIC_LOLVLEN_bm | PMIC_MEDLVLEN_bm | PMIC_HILVLEN_bm;
#endif
/* Hardware Initialization */
//i2c
TWI_Init(TWI_BIT_PRESCALE_1, TWI_BITLENGTH_FROM_FREQ(1, 400000));
//button ports init
buttons_init();
//timer init
TCCR1B |=(1<<CS10)|(1<<CS12);//prescaler 1024
TCNT1 = 0;//init counter
//screen setup - software spi, sh1106 driver (sh1306 analog)
/* Atmega32U4 mapping:
SCK (D0) - PORTB, PIN1 == PN(1, 1)
MOSI (D1) - PORTB, PIN2
RES (RST) - PORTB, PIN0
A0 (DC) - PORTB, PIN5 (OC1A/#A9)
*/
display_init();
display_draw_logo();
//sensors initialization
adxl345_init(MEASURE_ON, RANGE_2G, ODR_800);
l3g4200d_init(FIFO_HI_LO_PASS, NORM_ODR_800);
hmc5883l_init(MODE_CONT, GAIN_4_7, NORM_75HZ_8AV);
USB_Init();
}
msg_t hmc5883l_thread(void *arg)
{
(void)arg;
uint8_t buf_data[6];
float field[3];
chRegSetThreadName("HMC5883L");
i2cStart(&I2CD2, &i2cconfig);
while (!hmc58831_ID_check()) {
tweeter_set_error(ERROR_MAGNO, true);
chThdSleepMilliseconds(500);
}
tweeter_set_error(ERROR_MAGNO, false);
/* Initialise the settings. */
while (!hmc5883l_init()) {
tweeter_set_error(ERROR_MAGNO, true);
chThdSleepMilliseconds(500);
}
tweeter_set_error(ERROR_MAGNO, false);
while (TRUE)
{
/* Sleep until DRDY */
chSysLock();
tpHMC5883L = chThdSelf();
chSchGoSleepTimeoutS(THD_STATE_SUSPENDED, 100);
tpHMC5883L = NULL;
chSysUnlock();
/* Pull data from magno into buf_data. */
if (hmc5883l_receive(buf_data)) {
tweeter_set_error(ERROR_MAGNO, false);
hmc5883l_field_convert(buf_data, field);
log_s16(CHAN_IMU_MAGNO, field[0], field[1], field[2], 0);
/*define this state estimation function
state_estimation_new_magno(field[0],
field[1], field[2]); */
} else {
chThdSleepMilliseconds(20);
tweeter_set_error(ERROR_MAGNO, true);
}
}
}
void mwii_init(void){
//soc_init();
time_init();
uart_init(0, 38400, uart_buffer[0], 64, uart_buffer[1], 64);
// setup printf stuff (specific to avr-libc)
fdev_set_udata(&uart_fd, uart_get_serial_interface(0));
stdout = &uart_fd;
stderr = &uart_fd;
gpio_init();
//spi_init();
avr_i2c_init(0);
pwm_init();
//adc0_init_default();
sei();
/*
// setup stdout and stderr (avr-libc specific)
fdev_setup_stream(stdout, _serial_fd_putc, _serial_fd_getc, _FDEV_SETUP_RW);
fdev_set_udata(stdout, uart_get_serial_interface(0));
fdev_setup_stream(stderr, _serial_fd_putc, _serial_fd_getc, _FDEV_SETUP_RW);
fdev_set_udata(stderr, uart_get_serial_interface(0));
*/
// first thing must enable interrupts
//kprintf("BOOT\n");
gpio_configure(GPIO_MWII_LED, GP_OUTPUT);
//gpio_set(GPIO_MWII_LED);
gpio_configure(GPIO_MWII_MOTOR0, GP_OUTPUT);
gpio_configure(GPIO_MWII_MOTOR1, GP_OUTPUT);
gpio_configure(GPIO_MWII_MOTOR2, GP_OUTPUT);
gpio_configure(GPIO_MWII_MOTOR3, GP_OUTPUT);
gpio_configure(GPIO_MWII_RX0, GP_INPUT | GP_PULLUP | GP_PCINT);
gpio_configure(GPIO_MWII_RX1, GP_INPUT | GP_PULLUP | GP_PCINT);
gpio_configure(GPIO_MWII_RX2, GP_INPUT | GP_PULLUP | GP_PCINT);
gpio_configure(GPIO_MWII_RX3, GP_INPUT | GP_PULLUP | GP_PCINT);
// calibrate escs
//mwii_calibrate_escs();
// set initial motor speeds
mwii_write_motors(MINCOMMAND, MINCOMMAND, MINCOMMAND, MINCOMMAND);
brd->gpio0 = gpio_get_parallel_interface();
brd->twi0 = avr_i2c_get_interface(0);
/*
// I2C scanner
for(int c = 0; c < 255; c++){
uint8_t buf[2];
i2c_start_read(brd->twi0, c, buf, 1);
if(i2c_stop(brd->twi0) == 1 && c & 1){
kprintf("Device %x@i2c\n", c >> 1);
}
delay_us(10000);
}
*/
gpio_set(GPIO_MWII_LED);
i2cblk_init(&brd->mpublk, brd->twi0, MPU6050_ADDR, 8, I2CBLK_IADDR8);
mpu6050_init(&brd->mpu, i2cblk_get_interface(&brd->mpublk));
//kdebug("MPU6050: %s\n", ((mpu6050_probe(&brd->mpu))?"found":"not found!"));
i2cblk_init(&brd->bmpblk, brd->twi0, BMP085_ADDR, 8, I2CBLK_IADDR8);
bmp085_init(&brd->bmp, i2cblk_get_interface(&brd->bmpblk));
//kdebug("BMP085: found\n");
i2cblk_init(&brd->hmcblk, brd->twi0, HMC5883L_ADDR, 8, I2CBLK_IADDR8);
hmc5883l_init(&brd->hmc, i2cblk_get_interface(&brd->hmcblk));
gpio_clear(GPIO_MWII_LED);
hcsr04_init(&brd->hcsr, brd->gpio0, GPIO_MWII_HCSR_TRIGGER, GPIO_MWII_HCSR_ECHO);
ASYNC_PROCESS_INIT(&brd->process, mwii_task);
ASYNC_QUEUE_WORK(&ASYNC_GLOBAL_QUEUE, &brd->process);
//mwii_calibrate_mpu6050();
// let the escs init as well
delay_us(500000L);
}
void sensor_init(void)
{
mpu6050_init();
hmc5883l_init();
bmp085_init();
}
