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

Track gyro using a DCM.

Browse Source 
This brings in an implementation of some general matrix manipulation routines,
not all of which are used at teh moment.

Output the DCM on the UART at 50Hz for the host to display it usefully.
This commit is contained in:
Gavan Fantom
2011-10-07 23:39:28 +00:00
parent 6a700d0dbe
commit 62732758a8
10 changed files with 430 additions and 16 deletions
Download Patch File Download Diff File Expand all files Collapse all files

88
wmp.c
View File

@@ -2,6 +2,9 @@
#include "wmp.h"
#include "i2c.h"
#include "uart.h"
#include "dcm.h"
#define WMP_ZERO_COUNT 100
unsigned char wmp_init_command[2] = {0xfe, 0x04};
@@ -81,21 +84,31 @@ unsigned int wmp_yaw;
unsigned int wmp_pitch;
unsigned int wmp_roll;
unsigned int wmp_yaw_zero;
unsigned int wmp_pitch_zero;
unsigned int wmp_roll_zero;
bool wmp_yaw_fast;
bool wmp_pitch_fast;
bool wmp_roll_fast;
bool wmp_update;
bool wmp_zero;
#define TWO_PI 6.28318531f
#define DEG_TO_RAD (TWO_PI/360.0f)
/* There's considerable debate about these values, and they may vary
* between different models of the Wii Motion Plus. It would be nice
* to be able to use the calibration data stored on the device itself
* but we don't know the format yet.
*/
#define SLOW_YAW_STEP (1000/20)
#define SLOW_PITCH_STEP (1000/20)
#define SLOW_ROLL_STEP (1000/20)
#define FAST_YAW_STEP (1000/4)
#define FAST_PITCH_STEP (1000/4)
#define FAST_ROLL_STEP (1000/4)
#define SLOW_YAW_STEP (20 / DEG_TO_RAD)
#define SLOW_PITCH_STEP (20 / DEG_TO_RAD)
#define SLOW_ROLL_STEP (20 / DEG_TO_RAD)
#define FAST_YAW_STEP (4 / DEG_TO_RAD)
#define FAST_PITCH_STEP (4 / DEG_TO_RAD)
#define FAST_ROLL_STEP (4 / DEG_TO_RAD)
bool wmp_sample(void)
{
@@ -128,6 +141,8 @@ bool wmp_start_sample(void)
void wmp_event_handler(void)
{
float yaw, pitch, roll;
if (wmp_result != I2C_SUCCESS)
return;
@@ -142,15 +157,58 @@ void wmp_event_handler(void)
wmp_pitch_fast = !(wmp_sample_data[3] & 0x1);
wmp_roll_fast = !(wmp_sample_data[4] & 0x2);
wmp_generation++;
if ((wmp_generation % 100) == 0) {
putstr("(");
puthex(wmp_roll);
putstr(", ");
puthex(wmp_pitch);
putstr(", ");
puthex(wmp_yaw);
putstr(")\r\n");
if (wmp_update) {
int tmp_yaw = wmp_yaw;
int tmp_pitch = wmp_pitch;
int tmp_roll = wmp_roll;
tmp_yaw -= wmp_yaw_zero;
tmp_pitch -= wmp_pitch_zero;
tmp_roll -= wmp_roll_zero;
if (wmp_yaw_fast)
yaw = ((float)tmp_yaw) / FAST_YAW_STEP;
else
yaw = ((float)tmp_yaw) / SLOW_YAW_STEP;
if (wmp_pitch_fast)
pitch = ((float)tmp_pitch) / FAST_PITCH_STEP;
else
pitch = ((float)tmp_pitch) / SLOW_PITCH_STEP;
if (wmp_roll_fast)
roll = ((float)tmp_roll) / FAST_ROLL_STEP;
else
roll = ((float)tmp_roll) / SLOW_ROLL_STEP;
dcm_update(roll, pitch, yaw);
wmp_generation++;
if ((wmp_generation % 2) == 0)
dcm_send_packet();
} else if (wmp_zero) {
wmp_yaw_zero += wmp_yaw;
wmp_pitch_zero += wmp_pitch;
wmp_roll_zero += wmp_roll;
wmp_generation++;
if (wmp_generation >= WMP_ZERO_COUNT) {
wmp_zero = FALSE;
wmp_update = TRUE;
wmp_generation = 0;
wmp_yaw_zero /= WMP_ZERO_COUNT;
wmp_pitch_zero /= WMP_ZERO_COUNT;
wmp_roll_zero /= WMP_ZERO_COUNT;
putstr("Zero finished\r\n");
}
}
}
void wmp_start_zero(void)
{
wmp_zero = TRUE;
wmp_update = FALSE;
wmp_generation = 0;
putstr("Starting zero\r\n");
}