#include <stdio.h>
#include <string.h>
+#include <math.h>
#include <aversive/irq_lock.h>
#include <aversive/wait.h>
#include <aversive/pgmspace.h>
-#include <uart.h>
-
-
-#include <i2c.h>
-//#include "i2cm_sw.h"
-
-
-#include "math.h"
-#include "i2c_helper.h"
-//#include "bma150.h"
-//#include "itg3200.h"
-//#include "ak8500.h"
+#include <uart.h>
-#include "main.h"
#include "mpu6050.h"
-
-#include "vector.h"
#include "matrix.h"
-
#include "MadgwickAHRS.h"
-
-
-
-#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 "sd_log.h"
+#include "imu.h"
+#include "main.h"
-#include <uart.h>
-#include <stdio.h>
-#include <aversive/error.h>
-#include "cmdline.h"
-
-
-//#define LED_PRIO 170
-#define GYRO_PRIO 100
+/* structure storing the latest imu infos returned by the sensor */
+static struct imu_info g_imu;
-int internal_mag_x;
-int internal_mag_y;
-int internal_mag_z;
+/* structure storing the latest quaternion */
+static struct quaternion g_quat;
-int mag_x;
-int mag_y;
-int mag_z;
+/* structure storing the latest euler position */
+static struct euler g_euler;
+/* periodical timer structure */
+static struct callout imu_timer;
-#if 0
-static void main_timer_interrupt(void)
+static void quaternion2euler(const struct quaternion *quat, struct euler *euler)
{
- static uint8_t cpt = 0;
- cpt++;
- sei();
- if ((cpt & 0x3) == 0)
- scheduler_interrupt();
+ float q0 = quat->q0;
+ float q1 = quat->q1;
+ float q2 = quat->q2;
+ float q3 = quat->q3;
+
+ euler->roll = atan2f(2.0f * (q0 * q1 + q2 * q3),
+ q0*q0 - q1*q1 - q2*q2 + q3*q3);
+ euler->pitch = -asinf(2.0f * (q1 * q3 - q0 * q2));
+ euler->yaw = atan2f(2.0f * (q1 * q2 + q0 * q3),
+ q0*q0 + q1*q1 - q2*q2 - q3*q3);
}
-#endif
-
-#define LED1_TOGGLE() PORTB ^= 0x20;
-
-uint16_t counter;
-
-void do_led_blink(void *dummy)
+/* timer callback that polls IMU and does the calculation */
+static void imu_cb(struct callout_mgr *cm, struct callout *tim, void *arg)
{
- (void)dummy;
-
-#if 1 /* simple blink */
- LED1_TOGGLE();
-#endif
- //counter ++;
- printf("\r\n%"PRId16"\r\n", counter);
- counter = 0;
-
+ struct imu_info imu;
+ struct quaternion quat;
+ struct euler euler;
+ uint8_t irq_flags;
+
+ (void)arg;
+
+ /* copy previous quaternion locally */
+ IRQ_LOCK(irq_flags);
+ memcpy(&quat, &g_quat, sizeof(quat));
+ IRQ_UNLOCK(irq_flags);
+
+ mpu6050_read_all_axes(&g_imu);
+ MadgwickAHRSupdate(&g_imu, &g_quat);
+ quaternion2euler(&g_quat, &g_euler);
+
+ /* update global variables */
+ IRQ_LOCK(irq_flags);
+ memcpy(&g_imu, &imu, sizeof(g_imu));
+ IRQ_UNLOCK(irq_flags);
+ IRQ_LOCK(irq_flags);
+ memcpy(&g_quat, &quat, sizeof(g_quat));
+ IRQ_UNLOCK(irq_flags);
+ IRQ_LOCK(irq_flags);
+ memcpy(&g_euler, &euler, sizeof(g_euler));
+ IRQ_UNLOCK(irq_flags);
+
+ /* reschedule event */
+ callout_schedule(cm, tim, 2);
}
-
-/* for i2c */
-//uint8_t command_buf[I2C_SEND_BUFFER_SIZE];
-
-
-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
-
-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};
-
-double Update_Matrix[3][3]={{0,1,2},{3,4,5},{6,7,8}}; //Gyros here
-
-double DCM_Matrix[3][3]= {
- {
- 1,0,0 }
- ,{
- 0,1,0 }
- ,{
- 0,0,1 }
-};
-
-double Temporary_Matrix[3][3]={
- {
- 0,0,0 }
- ,{
- 0,0,0 }
- ,{
- 0,0,0 }
-};
-
-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 OUTPUTMODE 2
-
-#define ToRad(x) (x*0.01745329252) // *pi/180
-#define ToDeg(x) (x*57.2957795131) // *180/pi
-
-/*
-#define Gyro_Gain_X 0.92 //X axis Gyro gain
-#define Gyro_Gain_Y 0.92 //Y axis Gyro gain
-#define Gyro_Gain_Z 0.94 //Z axis Gyro gain
-*/
-#define Gyro_Gain_X (1.) //X axis Gyro gain
-#define Gyro_Gain_Y (1.) //Y axis Gyro gain
-#define Gyro_Gain_Z (1.) //Z axis Gyro gain
-
-#define Gyro_Scaled_X(x) ((x)*ToRad(Gyro_Gain_X)) //Return the scaled ADC raw data of the gyro in radians for second
-#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
-
-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
-
-
-#define Kp_ROLLPITCH (1.515/GRAVITY)
-#define Ki_ROLLPITCH (0.00101/GRAVITY)
-
-#define Kp_YAW 1.2
-//#define Kp_YAW 2.5 //High yaw drift correction gain - use with caution!
-#define Ki_YAW 0.00005
-
-#define MAGNETIC_DECLINATION 4.0
-
-#define constrain(v, a, b) (((v)<(a))?(a):((v)>(b)?(b):(v)))
-
-
-int mag_offset[3];
-double Heading;
-double Heading_X;
-double Heading_Y;
-
-
-// Euler angles
-double roll;
-double pitch;
-double yaw;
-
-int SENSOR_SIGN[]= {
- 1,1,1, // GYROX, GYROY, GYROZ,
- 1,1,1, // ACCELX, ACCELY, ACCELZ,
- 1,1,1 // MAGX, MAGY, MAGZ,
-};
-
-#if 0
-
-double read_adc(uint8_t index)
+void imu_init(void)
{
- int16_t value;
+ mpu6050_init();
- 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;
+ callout_init(&imu_timer, imu_cb, NULL, IMU_PRIO);
+ callout_schedule(&imuboard.intr_cm, &imu_timer, 20); /* every 20ms */
}
-
-uint8_t measure_time = 0;
-void read_sensors(void)
+void imu_get_info(struct imu_info *imu)
{
- 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
-
+ memcpy(imu, &g_imu, sizeof(*imu));
}
-#endif
-
-
-void quaternion2euler(void)
+void imu_get_pos_quat(struct quaternion *quat)
{
- /*
- 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);
+ memcpy(quat, &g_quat, sizeof(*quat));
}
-#define swap_u16(a) (((a>>8)&0xff) | (((a&0xFF)<<8)))
-
-void imu_init(void)
+void imu_get_pos_euler(struct euler *euler)
{
- /*
- bma150_init();
- itg3200_init();
- ak8975_read_sensitivity();
- */
- /*
- scheduler_add_periodical_event_priority(update_gyro, NULL,
- 1000000L / SCHEDULER_UNIT,
- GYRO_PRIO);
- */
- mpu6050_init();
+ memcpy(euler, &g_euler, sizeof(*euler));
}
-int imu_loop(void)
+
+int imu_log(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) {
-
- counter ++;
- mpu6050_read_all_axes(mpu6050_axes);
- /*
- for (i=0;i<7; i++){
- printf("%"PRId16" ", mpu6050_axes[i]);
- }
- printf("\r\n");
- */
- /*
- printf("%3.3f %3.3f %3.3f\r\n",
- mpu6050_gx,
- mpu6050_gy,
- mpu6050_gz);
- continue;
- */
-
- /*
- MadgwickAHRSupdateIMU(mpu6050_gx,
- mpu6050_gy,
- mpu6050_gz,
- mpu6050_ax,
- mpu6050_ay,
- mpu6050_az);
- */
-
- MadgwickAHRSupdate(mpu6050_gx,
- mpu6050_gy,
- mpu6050_gz,
- mpu6050_ax,
- mpu6050_ay,
- mpu6050_az,
- mpu6050_mx,
- mpu6050_my,
- mpu6050_mz
- );
-
- quaternion2euler();
-
- /*
- mpu6050_axes[7] = swap_u16(mpu6050_axes[7]);
- 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);
-
- 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 );
-
+ struct imu_info imu;
+
+ if (sd_log_enabled() == 0)
+ return 0;
+
+ IRQ_LOCK(flags);
+ ms = global_ms;
+ imu_get_info(&imu);
+ IRQ_UNLOCK(flags);
+
+ 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,
+ imu.gx, imu.gy, imu.gz,
+ imu.ax, imu.ay, imu.az,
+ imu.mx, imu.my, imu.mz);
+ if (sd_log_write(buf, len) != len) {
+ printf_P(PSTR("error writing to file\n"));
+ return -1;
}
return 0;