X-Git-Url: http://git.droids-corp.org/?p=protos%2Fimu.git;a=blobdiff_plain;f=imu.c;h=72bdd74b451606c1219b7935dca80725e85e8ce7;hp=8486960c1bf5b056e83eba3276fc7f3ffe884849;hb=84796cd7a01e949167a155ec21f55fd71e788015;hpb=96d834bdfb8df4e3369ca1b3c7bc7bc8534fda31 diff --git a/imu.c b/imu.c index 8486960..72bdd74 100644 --- a/imu.c +++ b/imu.c @@ -1,10 +1,9 @@ #include #include -#include -#include - +#include #include +#include #include @@ -19,6 +18,7 @@ //#include "itg3200.h" //#include "ak8500.h" +#include "main.h" #include "mpu6050.h" #include "vector.h" @@ -27,7 +27,23 @@ #include "MadgwickAHRS.h" -#define LED_PRIO 170 + +#include +#include +#include +#include "fat.h" +#include "fat_config.h" +#include "partition.h" +#include "sd_raw.h" +#include "sd_raw_config.h" + +#include +#include +#include +#include "cmdline.h" + + +//#define LED_PRIO 170 #define GYRO_PRIO 100 int internal_mag_x; @@ -39,15 +55,18 @@ 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; + (void)size; } void i2c_sendevent(int8_t size) { + (void)size; } - +#if 0 static void main_timer_interrupt(void) { static uint8_t cpt = 0; @@ -56,7 +75,7 @@ static void main_timer_interrupt(void) if ((cpt & 0x3) == 0) scheduler_interrupt(); } - +#endif #define LED1_TOGGLE() PORTB ^= 0x20; @@ -65,6 +84,8 @@ uint16_t counter; void do_led_blink(void *dummy) { + (void)dummy; + #if 1 /* simple blink */ LED1_TOGGLE(); #endif @@ -75,24 +96,49 @@ void do_led_blink(void *dummy) } +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) +{ + (void)fs; + + while(fat_read_dir(dd, dir_entry)) + { + if(strcmp(dir_entry->long_name, name) == 0) + { + fat_reset_dir(dd); + return 1; + } + } + + return 0; +} + +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; + if(!find_file_in_dir(fs, dd, name, &file_entry)) + return 0; + + return fat_open_file(fs, &file_entry); +} + /* for i2c */ //uint8_t command_buf[I2C_SEND_BUFFER_SIZE]; -float Accel_Vector[3]= {0,0,0}; //Store the acceleration in a vector -float Gyro_Vector[3]= {0,0,0};//Store the gyros rutn rate in a vector -float Magnet_Vector[3]= {0,0,0}; //Store the acceleration in a vector +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 -float Omega_Vector[3]= {0,0,0}; //Corrected Gyro_Vector data -float Omega_P[3]= {0,0,0};//Omega Proportional correction -float Omega_I[3]= {0,0,0};//Omega Integrator -float Omega[3]= {0,0,0}; +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}; -float Update_Matrix[3][3]={{0,1,2},{3,4,5},{6,7,8}}; //Gyros here +double Update_Matrix[3][3]={{0,1,2},{3,4,5},{6,7,8}}; //Gyros here -float DCM_Matrix[3][3]= { +double DCM_Matrix[3][3]= { { 1,0,0 } ,{ @@ -101,7 +147,7 @@ float DCM_Matrix[3][3]= { 0,0,1 } }; -float Temporary_Matrix[3][3]={ +double Temporary_Matrix[3][3]={ { 0,0,0 } ,{ @@ -110,11 +156,11 @@ float Temporary_Matrix[3][3]={ 0,0,0 } }; -float errorRollPitch[3]= {0,0,0}; -float errorYaw[3]= {0,0,0}; -float errorCourse=180; -float COGX=0; //Course overground X axis -float COGY=1; //Course overground Y axis +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 @@ -136,7 +182,7 @@ float COGY=1; //Course overground Y axis #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 -float G_Dt=0.02; // Integration time (DCM algorithm) +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 @@ -155,15 +201,15 @@ float G_Dt=0.02; // Integration time (DCM algorithm) int mag_offset[3]; -float Heading; -float Heading_X; -float Heading_Y; +double Heading; +double Heading_X; +double Heading_Y; // Euler angles -float roll; -float pitch; -float yaw; +double roll; +double pitch; +double yaw; int SENSOR_SIGN[]= { 1,1,1, // GYROX, GYROY, GYROZ, @@ -173,29 +219,29 @@ int SENSOR_SIGN[]= { #if 0 -float read_adc(uint8_t index) +double read_adc(uint8_t index) { int16_t value; switch(index) { case 0: itg3200_read_axe(0, &value); - return (float) (SENSOR_SIGN[index] * value); + return (double) (SENSOR_SIGN[index] * value); case 1: itg3200_read_axe(1, &value); - return (float) (SENSOR_SIGN[index] * value); + return (double) (SENSOR_SIGN[index] * value); case 2: itg3200_read_axe(2, &value); - return (float) (SENSOR_SIGN[index] * value); + return (double) (SENSOR_SIGN[index] * value); case 3: bma150_read_axe(0, &value); - return (float) (SENSOR_SIGN[index] * bma15_axe2g(value)); + return (double) (SENSOR_SIGN[index] * bma15_axe2g(value)); case 4: bma150_read_axe(1, &value); - return (float) (SENSOR_SIGN[index] * bma15_axe2g(value)); + return (double) (SENSOR_SIGN[index] * bma15_axe2g(value)); case 5: bma150_read_axe(2, &value); - return (float) (SENSOR_SIGN[index] * bma15_axe2g(value)); + return (double) (SENSOR_SIGN[index] * bma15_axe2g(value)); } return 0.0; } @@ -221,9 +267,9 @@ void read_sensors(void) err = ak8500_read_all_axes(&axes); if (err == 0) { /* - Magnet_Vector[0] = (float)SENSOR_SIGN[6] * (float)axes[0]; - Magnet_Vector[1] = (float)SENSOR_SIGN[7] * (float)axes[1]; - Magnet_Vector[2] = (float)SENSOR_SIGN[8] * (float)axes[2]; + Magnet_Vector[0] = (double)SENSOR_SIGN[6] * (double)axes[0]; + Magnet_Vector[1] = (double)SENSOR_SIGN[7] * (double)axes[1]; + Magnet_Vector[2] = (double)SENSOR_SIGN[8] * (double)axes[2]; */ /* */ @@ -282,58 +328,122 @@ void quaternion2euler(void) 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))) -int main(void) +static struct fat_file_struct *open_log_file(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); + struct fat_file_struct *fd; + struct fat_fs_struct *fs; + struct partition_struct *partition ; + struct fat_dir_struct *dd; + struct fat_dir_entry_struct directory; + struct fat_dir_entry_struct file_entry; + int16_t i = 0; + char name[16]; + + /* setup sd card slot */ + if (!sd_raw_init()) { +#if SD_DEBUG + printf_P(PSTR("MMC/SD initialization failed\n")); +#endif + return NULL; + } -#if 0 - i2c_init(I2C_MODE_MASTER, 1/* I2C_MAIN_ADDR */); - i2c_register_recv_event(i2c_recvevent); - i2c_register_send_event(i2c_sendevent); + /* open first partition */ + partition = partition_open(sd_raw_read, + sd_raw_read_interval, +#if SD_RAW_WRITE_SUPPORT + sd_raw_write, sd_raw_write_interval, #else - - i2cm_NUM_init(); + 0, 0, #endif + 0); + + if (!partition) { + /* If the partition did not open, assume the storage device + * is a "superfloppy", i.e. has no MBR. + */ + partition = partition_open(sd_raw_read, + sd_raw_read_interval, +#if SD_RAW_WRITE_SUPPORT + sd_raw_write, + sd_raw_write_interval, +#else + 0, + 0, +#endif + -1); + if (!partition) { +#if SD_DEBUG + printf_P(PSTR("opening partition failed\n")); +#endif + return NULL; + } + } + /* open file system */ + fs = fat_open(partition); + if (!fs) { +#if SD_DEBUG + printf_P(PSTR("opening filesystem failed\n")); +#endif + return NULL; + } + /* open root directory */ + fat_get_dir_entry_of_path(fs, "/", &directory); + dd = fat_open_dir(fs, &directory); + if (!dd) { +#if SD_DEBUG + printf_P(PSTR("opening root directory failed\n")); +#endif + return NULL; + } - // LED output - DDRB |= 0x20; + /* 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++); + if (!find_file_in_dir(fs, dd, name, &file_entry)) + break; + } - /* TIMER */ - timer_init(); - timer0_register_OV_intr(main_timer_interrupt); + printf("create log file %s\n", name); + if (!fat_create_file(dd, name, &file_entry)) { + printf_P(PSTR("error creating file: ")); + } + fd = open_file_in_dir(fs, dd, name); + if (!fd) { + printf_P(PSTR("error opening ")); + return NULL; + } - /* SCHEDULER */ - scheduler_init(); + return fd; +} +#define swap_u16(a) (((a>>8)&0xff) | (((a&0xFF)<<8))) +int imu_loop(void) +{ + //int16_t temp; + //uint8_t err; + //uint16_t * ptr; + //uint8_t a; + //int i; - sei(); + struct fat_file_struct *fd = NULL; + int16_t mpu6050_axes[10]; + char buf[128]; + int16_t len; + uint32_t ms; + uint8_t flags; /* 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, @@ -341,9 +451,14 @@ int main(void) */ mpu6050_init(); - Mad_f32_init(); + if (1) { + fd = open_log_file(); + if (fd == NULL) + printf("open log failed\r\n"); + } while (1) { + counter ++; mpu6050_read_all_axes(mpu6050_axes); /* @@ -387,7 +502,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 );