X-Git-Url: http://git.droids-corp.org/?a=blobdiff_plain;f=imu.c;h=25ea1c3f656327a8e2f040cbc63044d1a5686936;hb=a4e442441be45647f1115a59b7bff452910cf509;hp=72bdd74b451606c1219b7935dca80725e85e8ce7;hpb=84796cd7a01e949167a155ec21f55fd71e788015;p=protos%2Fimu.git diff --git a/imu.c b/imu.c index 72bdd74..25ea1c3 100644 --- a/imu.c +++ b/imu.c @@ -36,6 +36,7 @@ #include "partition.h" #include "sd_raw.h" #include "sd_raw_config.h" +#include "sd_log.h" #include #include @@ -55,17 +56,6 @@ int mag_y; int mag_z; -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) { @@ -96,33 +86,6 @@ 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]; @@ -217,105 +180,6 @@ int SENSOR_SIGN[]= { 1,1,1 // MAGX, MAGY, MAGZ, }; -#if 0 - -double read_adc(uint8_t index) -{ - int16_t value; - - switch(index) { - case 0: - itg3200_read_axe(0, &value); - return (double) (SENSOR_SIGN[index] * value); - case 1: - itg3200_read_axe(1, &value); - return (double) (SENSOR_SIGN[index] * value); - case 2: - itg3200_read_axe(2, &value); - return (double) (SENSOR_SIGN[index] * value); - case 3: - bma150_read_axe(0, &value); - return (double) (SENSOR_SIGN[index] * bma15_axe2g(value)); - case 4: - bma150_read_axe(1, &value); - return (double) (SENSOR_SIGN[index] * bma15_axe2g(value)); - case 5: - bma150_read_axe(2, &value); - return (double) (SENSOR_SIGN[index] * bma15_axe2g(value)); - } - return 0.0; -} - - -uint8_t measure_time = 0; -void read_sensors(void) -{ - uint8_t flags; - uint8_t err; - uint16_t axes[3]; - - /* read mag */ - measure_time ++;//= (measure_time +1)%3; - if (measure_time%2 == 0) { - err = ak8500_start_measure(); - if (err) { - printf("mag start err %X\r\n", err); - measure_time = 0; - } - } - else if (measure_time%2 == 1) { - err = ak8500_read_all_axes(&axes); - if (err == 0) { - /* - 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]; - */ - /* - */ - mag_x = SENSOR_SIGN[6] * axes[0]; - mag_y = SENSOR_SIGN[7] * axes[1]; - mag_z = SENSOR_SIGN[8] * axes[2]; - Magnet_Vector[0] = mag_x; - Magnet_Vector[1] = mag_y; - Magnet_Vector[2] = mag_z; - - } - else { - printf("mag read err %X\r\n", err); - } - } - /* - printf("%d %d %d\r\n", - mag_x, - mag_y, - mag_z - ); - */ - /* - printf("%f %f %f\r\n", - Magnet_Vector[0], - Magnet_Vector[1], - Magnet_Vector[2]); - */ - /* - Gyro_Vector[0]=Gyro_Scaled_X(read_adc(0)); //gyro x roll - Gyro_Vector[1]=Gyro_Scaled_Y(read_adc(1)); //gyro y pitch - Gyro_Vector[2]=Gyro_Scaled_Z(read_adc(2)); //gyro Z yaw - */ - Gyro_Vector[0]=ToRad(read_adc(0)); //gyro x roll - Gyro_Vector[1]=ToRad(read_adc(1)); //gyro y pitch - Gyro_Vector[2]=ToRad(read_adc(2)); //gyro Z yaw - - Accel_Vector[0]=9.81 * read_adc(3); // acc x - Accel_Vector[1]=9.81 * read_adc(4); // acc y - Accel_Vector[2]=9.81 * read_adc(5); // acc z - -} - -#endif - - void quaternion2euler(void) { /* @@ -328,117 +192,10 @@ void quaternion2euler(void) yaw = atan2f(2.0f * (q1 * q2 + q0 * q3), q0*q0 + q1*q1 - q2*q2 - q3*q3); } -static struct fat_file_struct *open_log_file(void) -{ - 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; - } - - /* 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 - 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; - } - - /* 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; - } - - 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; - } - - 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; - - - struct fat_file_struct *fd = NULL; - int16_t mpu6050_axes[10]; - char buf[128]; - int16_t len; - uint32_t ms; - uint8_t flags; +void imu_init(void) +{ /* bma150_init(); itg3200_init(); @@ -450,12 +207,16 @@ int imu_loop(void) GYRO_PRIO); */ mpu6050_init(); +} - if (1) { - fd = open_log_file(); - if (fd == NULL) - printf("open log failed\r\n"); - } +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) {