From 849b29b998a8cbc10cd1900d3e5dd1a8488b77f6 Mon Sep 17 00:00:00 2001 From: Olivier Matz Date: Thu, 17 Jul 2014 20:29:36 +0200 Subject: [PATCH] some cleaning --- gps_venus.c | 7 +- imu.c | 295 ++++++++++------------------------------------------ main.c | 6 +- main.h | 1 + mpu6050.h | 1 - sd_log.c | 35 ++++--- sd_log.h | 27 ++++- 7 files changed, 105 insertions(+), 267 deletions(-) diff --git a/gps_venus.c b/gps_venus.c index 20d8639..01551a6 100644 --- 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 --- a/imu.c +++ b/imu.c @@ -1,302 +1,113 @@ #include #include +#include #include #include #include -#include - - -#include -//#include "i2cm_sw.h" - -#include "math.h" +#include #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 -#include -#include -#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 -#include -#include -#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 --- 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 --- 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 --- 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 --- 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 --- 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 -- 2.39.5