introduce sd_log for logging on the sdcard

[protos/imu.git] / imu.c

diff --git a/imu.c b/imu.c
index efaba62..86b34b4 100644 (file)
--- 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 );