X-Git-Url: http://git.droids-corp.org/?p=protos%2Fimu.git;a=blobdiff_plain;f=imu.c;fp=imu.c;h=72bdd74b451606c1219b7935dca80725e85e8ce7;hp=efaba62a906d061f7daa12b72f0ce6299149e2a0;hb=84796cd7a01e949167a155ec21f55fd71e788015;hpb=883f5aae494ab066938b0cebb554a1ee13766713 diff --git a/imu.c b/imu.c index efaba62..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,7 +55,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 +66,7 @@ void i2c_sendevent(int8_t size) (void)size; } - +#if 0 static void main_timer_interrupt(void) { static uint8_t cpt = 0; @@ -60,7 +75,7 @@ static void main_timer_interrupt(void) if ((cpt & 0x3) == 0) scheduler_interrupt(); } - +#endif #define LED1_TOGGLE() PORTB ^= 0x20; @@ -81,6 +96,31 @@ 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 */ @@ -288,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, @@ -347,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); /* @@ -393,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 );