#include "partition.h"
#include "sd_raw.h"
#include "sd_raw_config.h"
+#include "sd_log.h"
#include <uart.h>
#include <stdio.h>
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)
{
}
-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];
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)
{
/*
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();
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) {
git.droids-corp.org / protos / imu.git / blobdiff