#include <stdio.h>
#include <string.h>
-#include <scheduler.h>
-#include <timer.h>
-
+#include <aversive/irq_lock.h>
#include <aversive/wait.h>
+#include <aversive/pgmspace.h>
#include <uart.h>
//#include "itg3200.h"
//#include "ak8500.h"
+#include "main.h"
#include "mpu6050.h"
#include "vector.h"
#include "MadgwickAHRS.h"
-#define LED_PRIO 170
+
+#include <string.h>
+#include <avr/pgmspace.h>
+#include <avr/sleep.h>
+#include "fat.h"
+#include "fat_config.h"
+#include "partition.h"
+#include "sd_raw.h"
+#include "sd_raw_config.h"
+
+#include <uart.h>
+#include <stdio.h>
+#include <aversive/error.h>
+#include "cmdline.h"
+
+
+//#define LED_PRIO 170
#define GYRO_PRIO 100
int internal_mag_x;
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;
if ((cpt & 0x3) == 0)
scheduler_interrupt();
}
-
+#endif
#define LED1_TOGGLE() PORTB ^= 0x20;
void do_led_blink(void *dummy)
{
+ (void)dummy;
+
#if 1 /* simple blink */
LED1_TOGGLE();
#endif
}
+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 }
,{
0,0,1 }
};
-float Temporary_Matrix[3][3]={
+double Temporary_Matrix[3][3]={
{
0,0,0 }
,{
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
#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
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,
#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;
}
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];
*/
/*
*/
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,
*/
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);
/*
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 );
git.droids-corp.org / protos / imu.git / blobdiff