#include <stdio.h>
#include <string.h>
-#include <scheduler.h>
-#include <timer.h>
-
+#include <aversive/irq_lock.h>
#include <aversive/wait.h>
+#include <aversive/pgmspace.h>
#include <uart.h>
//#include "itg3200.h"
//#include "ak8500.h"
+#include "main.h"
#include "mpu6050.h"
#include "vector.h"
#include "MadgwickAHRS.h"
-#define LED_PRIO 170
+
+#include <string.h>
+#include <avr/pgmspace.h>
+#include <avr/sleep.h>
+#include "fat.h"
+#include "fat_config.h"
+#include "partition.h"
+#include "sd_raw.h"
+#include "sd_raw_config.h"
+#include "sd_log.h"
+
+#include <uart.h>
+#include <stdio.h>
+#include <aversive/error.h>
+#include "cmdline.h"
+
+
+//#define LED_PRIO 170
#define GYRO_PRIO 100
int internal_mag_x;
int mag_z;
-void i2c_recvevent(uint8_t * buf, int8_t size)
+void i2c_recvevent(uint8_t *buf, int8_t size)
{
+ (void)buf;
+ (void)size;
}
void i2c_sendevent(int8_t size)
{
+ (void)size;
}
-
+#if 0
static void main_timer_interrupt(void)
{
static uint8_t cpt = 0;
if ((cpt & 0x3) == 0)
scheduler_interrupt();
}
-
+#endif
#define LED1_TOGGLE() PORTB ^= 0x20;
void do_led_blink(void *dummy)
{
+ (void)dummy;
+
#if 1 /* simple blink */
LED1_TOGGLE();
#endif
}
-
-
/* for i2c */
//uint8_t command_buf[I2C_SEND_BUFFER_SIZE];
}
#define swap_u16(a) (((a>>8)&0xff) | (((a&0xFF)<<8)))
-int main(void)
-{
- int16_t temp;
- //uint8_t err;
- uint16_t * ptr;
- uint8_t a;
- int i;
-
-
- int16_t mpu6050_axes[10];
-
- /* UART */
- uart_init();
-
- fdevopen(uart0_dev_send, uart0_dev_recv);
-
-#if 0
- i2c_init(I2C_MODE_MASTER, 1/* I2C_MAIN_ADDR */);
- i2c_register_recv_event(i2c_recvevent);
- i2c_register_send_event(i2c_sendevent);
-#else
-
- i2cm_NUM_init();
-#endif
-
-
-
- // LED output
- DDRB |= 0x20;
-
-
- /* TIMER */
- timer_init();
- timer0_register_OV_intr(main_timer_interrupt);
-
-
- /* SCHEDULER */
- scheduler_init();
-
-
- sei();
-
+void imu_init(void)
+{
/*
bma150_init();
itg3200_init();
ak8975_read_sensitivity();
*/
- scheduler_add_periodical_event_priority(do_led_blink, NULL,
- 1000000L / SCHEDULER_UNIT,
- LED_PRIO);
/*
scheduler_add_periodical_event_priority(update_gyro, NULL,
1000000L / SCHEDULER_UNIT,
GYRO_PRIO);
*/
mpu6050_init();
+}
- Mad_f32_init();
+int imu_loop(void)
+{
+ struct fat_file_struct *fd = NULL;
+ int16_t mpu6050_axes[10];
+ char buf[128];
+ int16_t len;
+ uint32_t ms;
+ uint8_t flags;
while (1) {
+
counter ++;
mpu6050_read_all_axes(mpu6050_axes);
/*
mpu6050_axes[8] = swap_u16(mpu6050_axes[8]);
mpu6050_axes[9] = swap_u16(mpu6050_axes[9]);
*/
- printf("%+3.3f\t%+3.3f\t%+3.3f\r\n", roll, pitch, yaw);
+ //printf("%+3.3f\t%+3.3f\t%+3.3f\r\n", roll, pitch, yaw);
+
+ IRQ_LOCK(flags);
+ ms = global_ms;
+ IRQ_UNLOCK(flags);
+
+ if (fd != NULL) {
+ len = snprintf(buf, sizeof(buf),
+ "%"PRIu32"\t"
+ "gyro %+3.3f\t%+3.3f\t%+3.3f\t\t"
+ "accel %+3.3f\t%+3.3f\t%+3.3f\t\t"
+ "magnet %+3.3f\t%+3.3f\t%+3.3f\r\n",
+ ms,
+ mpu6050_gx, mpu6050_gy, mpu6050_gz,
+ mpu6050_ax, mpu6050_ay, mpu6050_az,
+ mpu6050_mx, mpu6050_my, mpu6050_mz);
+ if (fat_write_file(fd, (unsigned char *)buf, len) != len) {
+ printf_P(PSTR("error writing to file\n"));
+ return -1;
+ }
+ }
+
+ printf("%"PRIu32"\t", ms);
+ printf("gyro %+3.3f\t%+3.3f\t%+3.3f\t\t",
+ mpu6050_gx, mpu6050_gy, mpu6050_gz);
+ printf("accel %+3.3f\t%+3.3f\t%+3.3f\t\t",
+ mpu6050_ax, mpu6050_ay, mpu6050_az);
+ printf("magnet %+3.3f\t%+3.3f\t%+3.3f\r\n",
+ mpu6050_mx, mpu6050_my, mpu6050_mz);
+
//printf("%+.4d %+.4d %+.4d\r\n", mpu6050_axes[7], mpu6050_axes[8], mpu6050_axes[9]);
//printf("%+3.3f\r\n", mpu6050_temp);//, mpu6050_axes[9]);
//printf("%+3.3f\t%+3.3f\t%+3.3f\r\n", mpu6050_mx, mpu6050_my, mpu6050_mz );