some cleaning

diff --git a/gps_venus.c b/gps_venus.c
index 20d8639..01551a6 100644 (file)
--- a/gps_venus.c
+++ b/gps_venus.c
@@ -541,15 +541,12 @@ void gps_get_pos(struct gps_pos *pos)
 
 int gps_loop(void)
 {
-       struct fat_file_struct *fd = NULL;
        uint32_t ms;
        uint8_t flags, prio;
        int16_t len;
        char buf[128];
        struct gps_pos pos;
 
-       fd = get_log_file();
-
        while (1) {
 
                IRQ_LOCK(flags);
@@ -573,9 +570,9 @@ int gps_loop(void)
                        (double)gps_pos.sea_altitude/100.);
 
 
-               if (fd != NULL) {
+               if (sd_log_enabled()) {
 
-                       if (fat_write_file(fd, (unsigned char *)buf, len) != len) {
+                       if (sd_log_write(buf, len) != len) {
                                printf_P(PSTR("error writing to file\n"));
                                return -1;
                        }

diff --git a/imu.c b/imu.c
index 25ea1c3..0833a81 100644 (file)
--- a/imu.c
+++ b/imu.c
@@ -1,302 +1,113 @@
 #include <stdio.h>
 #include <string.h>
+#include <math.h>
 
 #include <aversive/irq_lock.h>
 #include <aversive/wait.h>
 #include <aversive/pgmspace.h>
-#include <uart.h>
-
-
-#include <i2c.h>
-//#include "i2cm_sw.h"
 
-
-#include "math.h"
+#include <uart.h>
 
 #include "i2c_helper.h"
-//#include "bma150.h"
-//#include "itg3200.h"
-//#include "ak8500.h"
-
-#include "main.h"
 #include "mpu6050.h"
-
 #include "vector.h"
 #include "matrix.h"
-
 #include "MadgwickAHRS.h"
-
-
-
-#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 "imu.h"
+#include "main.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 internal_mag_y;
-int internal_mag_z;
-
-int mag_x;
-int mag_y;
-int mag_z;
-
-
-#if 0
-static void main_timer_interrupt(void)
-{
-       static uint8_t cpt = 0;
-       cpt++;
-       sei();
-       if ((cpt & 0x3) == 0)
-               scheduler_interrupt();
-}
-#endif
-
-#define LED1_TOGGLE()   PORTB ^= 0x20;
-
-
-uint16_t counter;
-
-void do_led_blink(void *dummy)
-{
-       (void)dummy;
-
-#if 1 /* simple blink */
-       LED1_TOGGLE();
-#endif
-       //counter ++;
-       printf("\r\n%"PRId16"\r\n", counter);
-       counter = 0;
-
-}
-
-
-/* for i2c */
-//uint8_t command_buf[I2C_SEND_BUFFER_SIZE];
-
-
-double Accel_Vector[3]= {0,0,0}; //Store the acceleration in a vector
-double Gyro_Vector[3]= {0,0,0};//Store the gyros rutn rate in a vector
-double Magnet_Vector[3]= {0,0,0}; //Store the acceleration in a vector
-
-double Omega_Vector[3]= {0,0,0}; //Corrected Gyro_Vector data
-double Omega_P[3]= {0,0,0};//Omega Proportional correction
-double Omega_I[3]= {0,0,0};//Omega Integrator
-double Omega[3]= {0,0,0};
-
-double Update_Matrix[3][3]={{0,1,2},{3,4,5},{6,7,8}}; //Gyros here
-
-double DCM_Matrix[3][3]= {
-  {
-    1,0,0  }
-  ,{
-    0,1,0  }
-  ,{
-    0,0,1  }
-};
-
-double Temporary_Matrix[3][3]={
-  {
-    0,0,0  }
-  ,{
-    0,0,0  }
-  ,{
-    0,0,0  }
-};
-
-double errorRollPitch[3]= {0,0,0};
-double errorYaw[3]= {0,0,0};
-double errorCourse=180;
-double COGX=0; //Course overground X axis
-double COGY=1; //Course overground Y axis
-
-
-
-#define OUTPUTMODE 2
-
-#define ToRad(x) (x*0.01745329252)  // *pi/180
-#define ToDeg(x) (x*57.2957795131)  // *180/pi
-
-/*
-#define Gyro_Gain_X 0.92 //X axis Gyro gain
-#define Gyro_Gain_Y 0.92 //Y axis Gyro gain
-#define Gyro_Gain_Z 0.94 //Z axis Gyro gain
-*/
-#define Gyro_Gain_X (1.) //X axis Gyro gain
-#define Gyro_Gain_Y (1.) //Y axis Gyro gain
-#define Gyro_Gain_Z (1.) //Z axis Gyro gain
-
-#define Gyro_Scaled_X(x) ((x)*ToRad(Gyro_Gain_X)) //Return the scaled ADC raw data of the gyro in radians for second
-#define Gyro_Scaled_Y(x) ((x)*ToRad(Gyro_Gain_Y)) //Return the scaled ADC raw data of the gyro in radians for second
-#define Gyro_Scaled_Z(x) ((x)*ToRad(Gyro_Gain_Z)) //Return the scaled ADC raw data of the gyro in radians for second
-
-double G_Dt=0.02;    // Integration time (DCM algorithm)
-
-#define GRAVITY 1.01 //this equivalent to 1G in the raw data coming from the accelerometer
-#define Accel_Scale(x) x*(GRAVITY/9.81)//Scaling the raw data of the accel to actual acceleration in meters for seconds square
-
-
-#define Kp_ROLLPITCH (1.515/GRAVITY)
-#define Ki_ROLLPITCH (0.00101/GRAVITY)
-
-#define Kp_YAW 1.2
-//#define Kp_YAW 2.5      //High yaw drift correction gain - use with caution!
-#define Ki_YAW 0.00005
-
-#define MAGNETIC_DECLINATION 4.0
-
-#define constrain(v, a, b) (((v)<(a))?(a):((v)>(b)?(b):(v)))
-
-
-int mag_offset[3];
-double Heading;
-double Heading_X;
-double Heading_Y;
-
+static struct callout imu_timer;
 
-// Euler angles
 double roll;
 double pitch;
 double yaw;
 
-int SENSOR_SIGN[]= {
-       1,1,1,   // GYROX, GYROY, GYROZ,
-       1,1,1,   // ACCELX, ACCELY, ACCELZ,
-       1,1,1    // MAGX, MAGY, MAGZ,
-};
-
 void quaternion2euler(void)
 {
-       /*
-       roll = atan2f(2. * (q0*q1 + q2*q3), 1. - 2. * (q1*q1 + q2*q2));
-       pitch = asinf(2 * (q0*q2 - q3*q1));
-       yaw =  atan2f(2. * (q0*q3 + q1*q2), 1. - 2. * (q2*q2 + q3*q3));
-       */
        roll = atan2f(2.0f * (q0 * q1 + q2 * q3), q0*q0 - q1*q1 - q2*q2 + q3*q3);
        pitch = -asinf(2.0f * (q1 * q3 - q0 * q2));
        yaw = atan2f(2.0f * (q1 * q2 + q0 * q3), q0*q0 + q1*q1 - q2*q2 - q3*q3);
 }
 
-#define swap_u16(a) (((a>>8)&0xff) | (((a&0xFF)<<8)))
+static void imu_cb(struct callout_mgr *cm, struct callout *tim, void *arg)
+{
+       int16_t mpu6050_axes[10];
+
+       (void)arg;
+
+       mpu6050_read_all_axes(mpu6050_axes);
+
+       MadgwickAHRSupdate(mpu6050_gx,
+               mpu6050_gy,
+               mpu6050_gz,
+               mpu6050_ax,
+               mpu6050_ay,
+               mpu6050_az,
+               mpu6050_mx,
+               mpu6050_my,
+               mpu6050_mz
+       );
+
+       quaternion2euler();
+
+       callout_schedule(cm, tim, 2);
+}
 
 void imu_init(void)
 {
-       /*
-       bma150_init();
-       itg3200_init();
-       ak8975_read_sensitivity();
-       */
-       /*
-       scheduler_add_periodical_event_priority(update_gyro, NULL,
-                                               1000000L / SCHEDULER_UNIT,
-                                               GYRO_PRIO);
-       */
        mpu6050_init();
+
+       callout_init(&imu_timer, imu_cb, NULL, IMU_PRIO);
+       callout_schedule(&imuboard.intr_cm, &imu_timer, 20); /* every 20ms */
+}
+
+void imu_get_info(struct imu_info *imu)
+{
+       imu->gx = mpu6050_gx;
+       imu->gy = mpu6050_gy;
+       imu->gz = mpu6050_gz;
+       imu->ax = mpu6050_ax;
+       imu->ay = mpu6050_ay;
+       imu->az = mpu6050_az;
+       imu->mx = mpu6050_mx;
+       imu->my = mpu6050_my;
+       imu->mz = mpu6050_mz;
+       imu->temp = 0;
 }
 
+
 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;
+       struct imu_info imu;
 
        while (1) {
 
-               counter ++;
-               mpu6050_read_all_axes(mpu6050_axes);
-               /*
-               for (i=0;i<7; i++){
-                       printf("%"PRId16" ", mpu6050_axes[i]);
-               }
-               printf("\r\n");
-               */
-               /*
-               printf("%3.3f %3.3f %3.3f\r\n",
-                      mpu6050_gx,
-                      mpu6050_gy,
-                      mpu6050_gz);
-               continue;
-               */
-
-               /*
-               MadgwickAHRSupdateIMU(mpu6050_gx,
-                                     mpu6050_gy,
-                                     mpu6050_gz,
-                                     mpu6050_ax,
-                                     mpu6050_ay,
-                                     mpu6050_az);
-               */
-
-               MadgwickAHRSupdate(mpu6050_gx,
-                                  mpu6050_gy,
-                                  mpu6050_gz,
-                                  mpu6050_ax,
-                                  mpu6050_ay,
-                                  mpu6050_az,
-                                  mpu6050_mx,
-                                  mpu6050_my,
-                                  mpu6050_mz
-                                  );
-
-               quaternion2euler();
-
-               /*
-               mpu6050_axes[7] = swap_u16(mpu6050_axes[7]);
-               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);
-
                IRQ_LOCK(flags);
                ms = global_ms;
+               imu_get_info(&imu);
                IRQ_UNLOCK(flags);
 
-               if (fd != NULL) {
+               if (sd_log_enabled()) {
                        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) {
+                               imu.gx, imu.gy, imu.gz,
+                               imu.ax, imu.ay, imu.az,
+                               imu.mx, imu.my, imu.mz);
+                       if (sd_log_write(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 );
-
        }
 
        return 0;

diff --git a/main.c b/main.c
index 8502044..a1d6af1 100644 (file)
--- a/main.c
+++ b/main.c
@@ -58,6 +58,7 @@
 #include "sd_log.h"
 #include "../fpv-common/i2c_commands.h"
 #include "i2c_protocol.h"
+#include "imu.h"
 #include "main.h"
 
 struct imuboard imuboard;
@@ -140,7 +141,6 @@ static void main_timer_interrupt(void)
 }
 
 /* XXX */
-int imu_loop(void);
 int sd_main(void);
 
 int main(void)
@@ -177,13 +177,13 @@ int main(void)
 
        eeprom_load_config();
 
-       open_log_file();
+       sd_log_open();
 
        printf_P(PSTR("\r\n"));
        rdline_newline(&imuboard.rdl, imuboard.prompt);
 
        //sd_main();
-       //imu_init();
+       imu_init();
        //imu_loop();
        gps_venus_init();
        gps_loop();

diff --git a/main.h b/main.h
index d0e7b65..ad65316 100644 (file)
--- a/main.h
+++ b/main.h
@@ -59,6 +59,7 @@
 #define LED_PRIO           160
 #define TIME_PRIO          140
 #define GPS_PRIO            80
+#define IMU_PRIO            70
 #define LOW_PRIO            60
 /* lowest priority */
 

diff --git a/mpu6050.h b/mpu6050.h
index b04c67c..327cd4a 100644 (file)
--- a/mpu6050.h
+++ b/mpu6050.h
@@ -118,7 +118,6 @@
 uint8_t mpu6050_init(void);
 uint8_t mpu6050_read_all_axes(int16_t *values);
 
-
 extern double mpu6050_gx;
 extern double mpu6050_gy;
 extern double mpu6050_gz;

diff --git a/sd_log.c b/sd_log.c
index 4659936..97496f4 100644 (file)
--- a/sd_log.c
+++ b/sd_log.c
@@ -12,7 +12,7 @@
 
 static struct fat_file_struct *log_fd = NULL;
 
-uint8_t find_file_in_dir(struct fat_fs_struct* fs,
+static uint8_t find_file_in_dir(struct fat_fs_struct* fs,
        struct fat_dir_struct* dd, const char* name,
        struct fat_dir_entry_struct* dir_entry)
 {
@@ -28,7 +28,7 @@ uint8_t find_file_in_dir(struct fat_fs_struct* fs,
        return 0;
 }
 
-struct fat_file_struct *open_file_in_dir(struct fat_fs_struct* fs,
+static struct fat_file_struct *open_file_in_dir(struct fat_fs_struct* fs,
        struct fat_dir_struct* dd, const char* name)
 {
        struct fat_dir_entry_struct file_entry;
@@ -39,7 +39,8 @@ struct fat_file_struct *open_file_in_dir(struct fat_fs_struct* fs,
        return fat_open_file(fs, &file_entry);
 }
 
-struct fat_file_struct *open_log_file(void)
+/* open the log file on the SD card */
+int8_t sd_log_open(void)
 {
        struct fat_file_struct *fd;
        struct fat_fs_struct *fs;
@@ -55,7 +56,7 @@ struct fat_file_struct *open_log_file(void)
 #if SD_DEBUG
                printf_P(PSTR("MMC/SD initialization failed\n"));
 #endif
-               return NULL;
+               return -1;
        }
 
        /* open first partition */
@@ -86,7 +87,7 @@ struct fat_file_struct *open_log_file(void)
 #if SD_DEBUG
                        printf_P(PSTR("opening partition failed\n"));
 #endif
-                       return NULL;
+                       return -1;
                }
        }
 
@@ -96,7 +97,7 @@ struct fat_file_struct *open_log_file(void)
 #if SD_DEBUG
                printf_P(PSTR("opening filesystem failed\n"));
 #endif
-               return NULL;
+               return -1;
        }
 
        /* open root directory */
@@ -106,12 +107,9 @@ struct fat_file_struct *open_log_file(void)
 #if SD_DEBUG
                printf_P(PSTR("opening root directory failed\n"));
 #endif
-               return NULL;
+               return -1;
        }
 
-       /* print some card information as a boot message */
-       //print_disk_info(fs);
-
        printf("choose log file name\n");
        while (1) {
                snprintf(name, sizeof(name), "log%.4d", i++);
@@ -127,13 +125,22 @@ struct fat_file_struct *open_log_file(void)
        fd = open_file_in_dir(fs, dd, name);
        if (!fd) {
                printf_P(PSTR("error opening "));
-               return NULL;
+               return -1;
        }
 
-       return fd;
+       return 0;
+}
+
+/* log output */
+intptr_t sd_log_write(const void *buffer, uintptr_t buffer_len)
+{
+       if (log_fd == NULL)
+               return -1;
+
+       return fat_write_file(log_fd, buffer, buffer_len);
 }
 
-struct fat_file_struct *get_log_file(void)
+uint8_t sd_log_enabled(void)
 {
-       return log_fd;
+       return log_fd != NULL;
 }

diff --git a/sd_log.h b/sd_log.h
index d2333e2..d386126 100644 (file)
--- a/sd_log.h
+++ b/sd_log.h
@@ -1,7 +1,30 @@
 #ifndef SD_LOG_H_
 #define SD_LOG_H_
 
-struct fat_file_struct *open_log_file(void);
-struct fat_file_struct *get_log_file(void);
+/**
+ * open a log file on sd card.
+ *
+ * The format of the filename is "log%.4d". The choosen integer is the
+ * first file that doesn't exist on the sd card, starting from 0.
+ *
+ * @return
+ *   0 on success, negative value on error
+ */
+int8_t sd_log_open(void);
+
+/**
+ * Write a buffer in the log file
+ *
+ * This function fails (return -1)if the log file was not opened.
+ *
+ * @return
+ *   number of written bytes on success, negative value en error
+ */
+intptr_t sd_log_write(const void *buffer, uintptr_t buffer_len);
+
+/**
+ * return true if log file was properly opened
+ */
+uint8_t sd_log_enabled(void);
 
 #endif