coolfactor/quadrotor
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity

Take interleaved accelerometer and gyro readings, and integrate them into

Browse Source 
a single attitude estimation.

A bunch of debug stuff, too.
This commit is contained in:
Gavan Fantom
2011-11-04 12:47:16 +00:00
parent 62732758a8
commit 2890007195
7 changed files with 223 additions and 11 deletions
Download Patch File Download Diff File Expand all files Collapse all files

92
wmp.c
View File

@@ -3,10 +3,27 @@
#include "i2c.h"
#include "uart.h"
#include "dcm.h"
#include "fisqrt.h"
#define WMP_ZERO_COUNT 100
unsigned char wmp_init_command[2] = {0xfe, 0x04};
#define ACCEL_ZERO_X 520
#define ACCEL_ZERO_Y 516
#define ACCEL_ZERO_Z 514
/*
516, 510, 710
-4, -6, 196
16, 36, 38416 = 38468
sqrt(38468) = 196.1326...
... somehow once we scale by gravity we get almost exactly 0.05.
*/
#define ACCEL_SCALE 0.05
/* Nunchuck pass-through mode */
unsigned char wmp_init_command[2] = {0xfe, 0x05};
i2c_result wmp_result;
unsigned int wmp_generation;
@@ -92,6 +109,10 @@ bool wmp_yaw_fast;
bool wmp_pitch_fast;
bool wmp_roll_fast;
int accel_x;
int accel_y;
int accel_z;
bool wmp_update;
bool wmp_zero;
@@ -139,15 +160,10 @@ bool wmp_start_sample(void)
return i2c_start_transaction(&wmp_sample_transaction);
}
void wmp_event_handler(void)
void wmp_process_gyro_sample(void)
{
float yaw, pitch, roll;
if (wmp_result != I2C_SUCCESS)
return;
wmp_result = I2C_IN_PROGRESS;
wmp_yaw = ((wmp_sample_data[3]>>2)<<8) + wmp_sample_data[0];
wmp_pitch = ((wmp_sample_data[4]>>2)<<8) + wmp_sample_data[1];
wmp_roll = ((wmp_sample_data[5]>>2)<<8) + wmp_sample_data[2];
@@ -185,8 +201,10 @@ void wmp_event_handler(void)
wmp_generation++;
#if 1
if ((wmp_generation % 2) == 0)
dcm_send_packet();
#endif
} else if (wmp_zero) {
wmp_yaw_zero += wmp_yaw;
@@ -205,6 +223,66 @@ void wmp_event_handler(void)
}
}
void wmp_process_accel_sample(void)
{
float x, y, z;
#if 0
float invmag;
#endif
accel_x = (wmp_sample_data[2]<<2) + ((wmp_sample_data[5]>>3) & 0x02);
accel_y = (wmp_sample_data[3]<<2) + ((wmp_sample_data[5]>>4) & 0x02);
accel_z = ((wmp_sample_data[4]<<2) & 0x3f8) +
((wmp_sample_data[5]>>5) & 0x06);
x = (accel_x - ACCEL_ZERO_X) * ACCEL_SCALE;
y = (accel_y - ACCEL_ZERO_Y) * ACCEL_SCALE;
z = (accel_z - ACCEL_ZERO_Z) * ACCEL_SCALE;
#if 0
invmag = fisqrt(x*x + y*y + z*z);
x = x * invmag;
y = y * invmag;
z = z * invmag;
#endif
#if 0
accel_x = (x * 512.0 + 1000.0);
accel_y = (y * 512.0 + 1000.0);
accel_z = (z * 512.0 + 1000.0);
#endif
#if 0
putstr("(");
putint(accel_x);
putstr(", ");
putint(accel_y);
putstr(", ");
putint(accel_z);
putstr(")\r\n");
#endif
/* The minus signs are needed because something is upside down.
* It might actually be the WMP, but we're defining coordinates based
* on that so we'll just fudge it here.
*/
dcm_drift_correction(x, -y, -z);
}
void wmp_event_handler(void)
{
if (wmp_result != I2C_SUCCESS)
return;
wmp_result = I2C_IN_PROGRESS;
if (wmp_sample_data[5] & 0x02)
wmp_process_gyro_sample();
else
wmp_process_accel_sample();
}
void wmp_start_zero(void)
{
wmp_zero = TRUE;