- 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)
-{
- /*
- 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);
-}
-
-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];