X-Git-Url: http://git.droids-corp.org/?p=protos%2Fimu.git;a=blobdiff_plain;f=imu.c;h=86b34b46b1e7eb22789676ca96d778e1567904d1;hp=efaba62a906d061f7daa12b72f0ce6299149e2a0;hb=004f99ba874339e50629141ac39e888070b5378e;hpb=9c35c09227041d2d23477b19443b3f707fddc664

diff --git a/imu.c b/imu.c
index efaba62..86b34b4 100644
--- a/imu.c
+++ b/imu.c
@@ -1,10 +1,9 @@
 #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>
 
 
@@ -19,6 +18,7 @@
 //#include "itg3200.h"
 //#include "ak8500.h"
 
+#include "main.h"
 #include "mpu6050.h"
 
 #include "vector.h"
@@ -27,7 +27,24 @@
 #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;
@@ -39,7 +56,6 @@ int mag_y;
 int mag_z;
 
 
-void i2c_recvevent(uint8_t * buf, int8_t size)
 void i2c_recvevent(uint8_t *buf, int8_t size)
 {
 	(void)buf;
@@ -51,7 +67,7 @@ void i2c_sendevent(int8_t size)
 	(void)size;
 }
 
-
+#if 0
 static void main_timer_interrupt(void)
 {
 	static uint8_t cpt = 0;
@@ -60,7 +76,7 @@ static void main_timer_interrupt(void)
 	if ((cpt & 0x3) == 0)
 		scheduler_interrupt();
 }
-
+#endif
 
 #define LED1_TOGGLE()   PORTB ^= 0x20;
 
@@ -81,8 +97,6 @@ void do_led_blink(void *dummy)
 }
 
 
-
-
 /* for i2c */
 //uint8_t command_buf[I2C_SEND_BUFFER_SIZE];
 
@@ -289,67 +303,33 @@ void quaternion2euler(void)
 }
 
 #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);
 		/*
@@ -393,7 +373,36 @@ int main(void)
 		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 );