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

Lots of development of new features. Radio input, motor output, PID control

Browse Source 
loops, boot-time initialisation, option to run without attached UART.

And.. it flies!
This commit is contained in:
Gavan Fantom
2012-08-30 12:42:38 +00:00
parent 2890007195
commit 3f12132231
18 changed files with 727 additions and 23 deletions
Download Patch File Download Diff File Expand all files Collapse all files

16
Makefile
View File

@@ -4,14 +4,23 @@ NAME=quad
SSRCS=crt0.s
CSRCS=main.c i2c.c wmp.c timer.c interrupt.c uart.c event.c matrix.c dcm.c
CSRCS+=fisqrt.c
CSRCS+=fisqrt.c stick.c trig.c motor.c
#PROJOPTS=-DUSE_UART -DSEND_DCM
COPTIM?=-O1
CFLAGS=-march=armv4t -msoft-float $(COPTIM) -Wall -Werror -Wextra
CFLAGS=-march=armv4t -msoft-float $(COPTIM) -Wall -Werror -Wextra $(PROJOPTS)
LDSCRIPT=lpc2103_flash.ld
# To build with the Clang Static Analyzer, use
# scan-build --use-cc=arm-elf-gcc make
# And uncomment the following line:
CC=arm-elf-gcc
OBJCOPY=arm-elf-objcopy
LINT=splint
LINTFLAGS=-booltype bool -nolib +charint
CLEANOBJS=$(OBJS) $(NAME).hex $(NAME).elf $(NAME).bin $(NAME).map .depend
@@ -45,4 +54,7 @@ clean:
depend:
$(CC) -MM $(CFLAGS) -nostdlib -nostartfiles $(CSRCS) >.depend
lint:
$(LINT) $(LINTFLAGS) $(CSRCS)
.sinclude ".depend"

28
dcm.c
View File

@@ -8,6 +8,7 @@
#include "matrix.h"
#include "dcm.h"
#include "uart.h"
#include "motor.h"
#define GRAVITY 9.80665f
@@ -27,13 +28,15 @@ float dcm[3*3] = {1, 0, 0,
float omega_p[3] = {0.0, 0.0, 0.0};
float omega_i[3] = {0.0, 0.0, 0.0};
float omega_x, omega_y, omega_z;
float delta_t = 0.01;
void dcm_update(float x, float y, float z)
{
float omega_x = x + omega_i[0] + omega_p[0];
float omega_y = y + omega_i[1] + omega_p[1];
float omega_z = z + omega_i[2] + omega_p[2];
omega_x = x + omega_i[0] + omega_p[0];
omega_y = y + omega_i[1] + omega_p[1];
omega_z = z + omega_i[2] + omega_p[2];
float tx = delta_t * omega_x;
float ty = delta_t * omega_y;
@@ -182,9 +185,12 @@ void dcm_drift_correction(float x, float y, float z)
omega_i[i] += error[i] * (KI_ROLLPITCH * weight);
}
#if 0
putstr("w: ");
putint_s((int)(weight * 100000.0f));
putstr("\r\n");
#endif
#if 0
putstr("p: ");
putint_s((int)(omega_p[0] * 100000.0f));
@@ -202,6 +208,22 @@ void dcm_drift_correction(float x, float y, float z)
#endif
}
void dcm_attitude_error(float roll, float pitch, float yaw)
{
/* dcm[6] = sine of pitch */
/* dcm[7] = sine of roll */
/* pitch error = pitch - dcm[6] */
/* roll error = roll - dcm[7] */
/* That was the theory. In practice, there appears to be some
confusion over axes. Pitch and roll seem.. reversed. */
/* TODO: What if we are upside down? */
motor_pid_update(roll, dcm[6], pitch, -dcm[7], yaw, -omega_z);
}
void dcm_dump(void)
{
putstr("dcm: ");

4
dcm.h
View File

@@ -2,6 +2,8 @@
#include "types.h"
extern float delta_t;
void dcm_update(float omega_x, float omega_y, float omega_z);
void dcm_normalise(void);
bool dcm_renormalise(float *v);
@@ -9,4 +11,4 @@ void dcm_dump(void);
void dcm_send_packet(void);
void dcm_setvector(float x, float y, float z);
void dcm_drift_correction(float x, float y, float z);
void dcm_attitude_error(float x, float y, float z);

1
interrupt.h
View File

@@ -35,6 +35,7 @@
#define I_PRIORITY_I2C0 0
#define I_PRIORITY_UART0 1
#define I_PRIORITY_TIMER0 2
#define I_PRIORITY_TIMER1 3
#define interrupt_clear() do { VICVectAddr = 0xff; } while (0)

87
main.c
View File

@@ -1,3 +1,4 @@
/* main.c */
#include "wmp.h"
#include "i2c.h"
@@ -5,8 +6,10 @@
#include "uart.h"
#include "interrupt.h"
#include "event.h"
#include "stick.h"
#define PINSEL0 (*((volatile unsigned char *) 0xE002C000))
#define PINSEL0 (*((volatile unsigned int *) 0xE002C000))
#define PINSEL1 (*((volatile unsigned int *) 0xE002C004))
#define FP0XDIR (*((volatile unsigned int *) 0x3FFFC000))
#define FP0XVAL (*((volatile unsigned int *) 0x3FFFC014))
@@ -15,18 +18,26 @@
void init_pins(void)
{
PINSEL0 = 0x00000055; /* P0.0 and P0.1 assigned to UART */
PINSEL0 = 0x00a88055; /* P0.0 and P0.1 assigned to UART */
/* P0.2 and P0.3 assigned to I2C */
/* P0.7 and P0.9 assigned to MAT2.[02] */
/* P0.10 and P0.11 assigned to CAP1.[01] */
PINSEL1 = 0x20000828; /* P0.21 and P0.30 assigned to MAT3.[03] */
/* P0.17 and P0.18 assigned to CAP1.[23] */
SCS = 1;
FP0XDIR = 0x04000000; /* P0.26 is an output */
FP0XVAL = 0x0;
}
#ifdef USE_UART
void reply(char *str)
{
putstr(str);
putstr("\r\n");
}
#else
#define reply(x) ((void)0)
#endif
unsigned int count = 0;
@@ -167,6 +178,8 @@ void timer_event_handler(void)
void menu_handler(void);
int main(void) {
armed = FALSE;
init_interrupt();
init_uart();
init_i2c();
@@ -183,6 +196,21 @@ int main(void) {
putstr("prompt> ");
FP0XVAL &= ~0x04000000;
timer_delay_ms(1000);
FP0XVAL |= 0x04000000;
timer_delay_ms(1000);
FP0XVAL &= ~0x04000000;
if (!wmp_init())
putstr("WMP initialisation failed\r\n");
/* Flight is potentially live after this. */
timer_set_period(5*TIMER_MS);
wmp_start_zero();
FP0XVAL |= 0x04000000;
/* Good luck! */
while (1) {
FP0XVAL ^= 0x04000000;
event_dispatch();
@@ -197,6 +225,9 @@ void menu_handler(void)
char c;
while (getch(&c)) {
#if 1
continue; /* Yes, let's just ignore UART input now. */
#endif
if (c == 0x0a)
continue;
putch(c);
@@ -283,6 +314,58 @@ void menu_handler(void)
reply("done");
wmp_start_zero();
break;
case 'W':
for (i = 0; i < 4; i++) {
putstr("Width ");
putint(i);
putstr(": ");
putint(timer_input(i));
if (!timer_valid(i))
putstr(" (invalid)");
putstr("\r\n");
}
if (!timer_allvalid()) {
putstr("ALL INVALID!\r\n");
}
break;
case '0' & 0xdf:
timer_set_pwm_value(0, 0);
timer_set_pwm_value(1, 0);
timer_set_pwm_value(2, 0);
timer_set_pwm_value(3, 0);
break;
#if 0
case '1' & 0xdf:
timer_set_pwm_value(0, PWM_MAX/2);
break;
case '2' & 0xdf:
timer_set_pwm_value(1, PWM_MAX/2);
break;
case '3' & 0xdf:
timer_set_pwm_value(2, PWM_MAX/2);
break;
case '4' & 0xdf:
timer_set_pwm_value(3, PWM_MAX/2);
break;
case '5' & 0xdf:
timer_set_pwm_value(0, PWM_MAX);
break;
case '6' & 0xdf:
timer_set_pwm_value(1, PWM_MAX);
break;
case '7' & 0xdf:
timer_set_pwm_value(2, PWM_MAX);
break;
case '8' & 0xdf:
timer_set_pwm_value(3, PWM_MAX);
break;
#endif
case '9' & 0xdf:
timer_set_pwm_invalid(0);
timer_set_pwm_invalid(1);
timer_set_pwm_invalid(2);
timer_set_pwm_invalid(3);
break;
default:
reply("Unrecognised command.");
break;

153
motor.c Normal file
View File

@@ -0,0 +1,153 @@
/* motor.c */
#include "stick.h"
#include "timer.h"
#include "dcm.h"
#include "uart.h"
float integral[3] = {0.0f, 0.0f, 0.0f};
float last[3];
float throttle = 0.0f;
#define Kp 0.2
#define Ki 0.04
#define Kd 0.08
#define Ka 0.0
#define Kp_y 0.2
#define Ki_y 0.00
#define Kd_y 0.00
#define Ka_y 0.0
/*
* Perform a PID loop iteration.
* roll and pitch are absolute values
* yaw is, currently, a rate.
*/
void motor_pid_update(float troll, float mroll,
float tpitch, float mpitch,
float tyaw, float myaw)
{
float derivative[3];
float out[3];
float motor[3];
float roll, pitch, yaw;
float error, max_error;
float min_motor;
int i;
roll = troll - mroll;
pitch = tpitch - mpitch;
yaw = tyaw - myaw;
#if 0
if ((stick_counter % 100) == 0) {
putstr("{");
putint_s((int)(tyaw * 10000));
putstr(", ");
putint_s((int)(myaw * 10000));
putstr("}\r\n");
}
#endif
integral[0] += roll * delta_t;
integral[1] += pitch * delta_t;
integral[2] += yaw * delta_t;
/* The measurements are the opposite sign to the error */
derivative[0] = (-mroll - last[0]) / delta_t;
derivative[1] = (-mpitch - last[1]) / delta_t;
derivative[2] = (-myaw - last[2]) / delta_t;
last[0] = -mroll;
last[1] = -mpitch;
last[2] = -myaw;
out[0] = roll * Kp + integral[0] * Ki + derivative[0] * Kd;
out[1] = pitch * Kp + integral[1] * Ki + derivative[1] * Kd;
out[2] = yaw * Kp_y + integral[2] * Ki_y + derivative[2] * Kd_y;
if (armed) {
/* Front right */
motor[0] = throttle + out[0] + out[1] + out[2];
/* Front left */
motor[1] = throttle - out[0] + out[1] - out[2];
/* Rear left */
motor[2] = throttle - out[0] - out[1] + out[2];
/* Rear right */
motor[3] = throttle + out[0] - out[1] - out[2];
} else {
motor[0] = 0.0;
motor[1] = 0.0;
motor[2] = 0.0;
motor[3] = 0.0;
}
max_error = 0.0;
min_motor = 1.0;
for (i = 0; i < 3; i++) {
if (motor[i] < 0.0)
motor[i] = 0.0;
if (motor[i] > 1.0f) {
error = motor[i] - 1.0f;
if (error > max_error)
max_error = error;
}
if (motor[i] < min_motor)
min_motor = motor[i];
}
if (max_error > 0.0) {
for (i = 0; i < 3; i++) {
motor[i] -= max_error;
if (motor[i] < 0.0)
motor[i] = 0.0;
}
}
if (throttle <= 0.0) {
motor[0] = 0.0;
motor[1] = 0.0;
motor[2] = 0.0;
motor[3] = 0.0;
integral[0] = 0.0;
integral[1] = 0.0;
integral[2] = 0.0;
}
if (max_error < min_motor) {
float new_throttle2, new_out[3];
new_throttle2 = (motor[0] + motor[1] + motor[2] + motor[3])/2.0;
new_out[0] = (motor[0] + motor[3] - new_throttle2)/2.0;
new_out[1] = (motor[0] + motor[1] - new_throttle2)/2.0;
new_out[2] = (motor[0] + motor[2] - new_throttle2)/2.0;
/* Anti-windup */
for (i = 0; i < 3; i++) {
if (new_out[i] > 1.0)
integral[i] -= (new_out[i]-1.0) * Ka;
if (new_out[i] < 0.0)
integral[i] -= (new_out[i]) * Ka;
}
}
timer_set_pwm_value(0, (int)(motor[0] * PWM_MAX));
timer_set_pwm_value(1, (int)(motor[1] * PWM_MAX));
timer_set_pwm_value(2, (int)(motor[2] * PWM_MAX));
timer_set_pwm_value(3, (int)(motor[3] * PWM_MAX));
}
void motor_kill(void) {
throttle = 0.0;
timer_set_pwm_value(0, 0);
timer_set_pwm_value(1, 0);
timer_set_pwm_value(2, 0);
timer_set_pwm_value(3, 0);
}
void motor_set_throttle(float t) {
if (armed)
throttle = t;
}

9
motor.h Normal file
View File

@@ -0,0 +1,9 @@
/* motor.h */
extern float temp_kp;
void motor_pid_update(float troll, float mroll,
float tpitch, float mpitch,
float tyaw, float myaw);
void motor_kill(void);
void motor_set_throttle(float t);

147
stick.c Normal file
View File

@@ -0,0 +1,147 @@
/* stick.c */
#ifdef WE_HAVE_SQRT
#include <math.h>
#else
#include "fisqrt.h"
#endif
#include "matrix.h"
#include "stick.h"
#include "dcm.h"
#include "uart.h"
#include "timer.h"
#include "trig.h"
#include "motor.h"
#include "wmp.h"
#define TWO_PI 6.28318531f
#define PI 3.14159265f
#define MIN_X 15830
#define MAX_X 28300
#define CENTRE_X 22100
#define MIN_Y 18530
#define MAX_Y 28200
#define CENTRE_Y 22100
#define MIN_Z 15800
#define MAX_Z 28304
#define CENTRE_Z 22100
#define MIN_THR 16500
#define MAX_THR 28275
#define MIN_REAL_THR 15830
#define CENTRE_ZONE 100
/* Full scale is a roll/pitch angle of 30 degrees from the vertical */
#define SCALE_X (TWO_PI*30.0/360.0 / (MAX_X-CENTRE_X))
#define SCALE_Y (TWO_PI*30.0/360.0 / (MAX_Y-CENTRE_Y))
/* Full scale is a complete rotation in one second */
#define SCALE_Z (TWO_PI / (MAX_Z-CENTRE_Z))
/* 0 is min throttle, 1 is max throttle */
#define SCALE_THROTTLE (1.0f/(MAX_THR - MIN_THR))
float stick_yaw = 0.0f;
unsigned int stick_counter;
bool armed = FALSE;
void stick_update(float x, float y, float z)
{
float tz = delta_t * z;
stick_yaw += tz;
if (stick_yaw <= -PI)
stick_yaw += TWO_PI;
if (stick_yaw > PI)
stick_yaw -= TWO_PI;
#if 0
z = stick_yaw;
#endif
x = sine(x);
y = sine(y);
#if 0
z = 1.0/fisqrt(1-x*x-y*y);
#endif
dcm_attitude_error(x, y, z);
}
void stick_input(void) {
float x, y, z, throttle;
if (timer_allvalid()) {
x = timer_input(0);
y = timer_input(1);
throttle = timer_input(2);
z = timer_input(3);
if (!armed) {
if ((throttle < MIN_THR) &&
(x > (CENTRE_X - CENTRE_ZONE)) &&
(x < (CENTRE_X + CENTRE_ZONE)) &&
(y > (CENTRE_Y - CENTRE_ZONE)) &&
(y < (CENTRE_Y + CENTRE_ZONE)) &&
(z > (CENTRE_Z - CENTRE_ZONE)) &&
(z < (CENTRE_Z + CENTRE_ZONE)) &&
(wmp_zero == FALSE)) {
putstr("ARMED!!!\r\n");
armed = TRUE;
}
}
x -= CENTRE_X;
y -= CENTRE_Y;
z -= CENTRE_Z;
x = x * SCALE_X;
y = y * SCALE_Y;
z = z * SCALE_Z;
throttle = (throttle - MIN_THR) * SCALE_THROTTLE;
if (throttle < 0.0)
throttle = 0.0;
} else {
x = 0.0f;
y = 0.0f;
z = 0.0f;
throttle = 0.0f;
}
motor_set_throttle(throttle);
/* So the controls are back to front. Let's fix that. */
x = -x;
y = -y;
z = -z;
#if 0
if ((stick_counter % 100) == 0) {
putstr("[");
putint_s((int)(z * 10000));
putstr("] ");
}
#endif
#if 1
stick_update(x, y, z);
#else
if ((stick_counter % 100) == 0)
stick_update(x, y, z);
#endif
/*
if ((stick_counter & 3) == 0)
stick_send_packet();
*/
stick_counter++;
}

10
stick.h Normal file
View File

@@ -0,0 +1,10 @@
/* stick.h */
#include "types.h"
extern bool armed;
extern unsigned int stick_counter;
void stick_input(void);
void stick_update(float x, float y, float omega_z);

172
timer.c
View File

@@ -3,8 +3,12 @@
#include "uart.h"
#include "event.h"
#define FP0XVAL (*((volatile unsigned int *) 0x3FFFC014))
#define TIMER0BASE 0xE0004000
#define TIMER1BASE 0xE0008000
#define TIMER2BASE 0xE0070000
#define TIMER3BASE 0xE0074000
#define IR 0x00
#define TCR 0x04
@@ -28,6 +32,15 @@
#define TREG(x) (((volatile unsigned char *)TIMER0BASE)[x])
#define TWREG(x) (((volatile unsigned int *)TIMER0BASE)[(x)/sizeof(unsigned int)])
#define T1REG(x) (((volatile unsigned char *)TIMER1BASE)[x])
#define T1WREG(x) (((volatile unsigned int *)TIMER1BASE)[(x)/sizeof(unsigned int)])
#define T2REG(x) (((volatile unsigned char *)TIMER2BASE)[x])
#define T2WREG(x) (((volatile unsigned int *)TIMER2BASE)[(x)/sizeof(unsigned int)])
#define T3REG(x) (((volatile unsigned char *)TIMER3BASE)[x])
#define T3WREG(x) (((volatile unsigned int *)TIMER3BASE)[(x)/sizeof(unsigned int)])
#define TCR_ENABLE (1<<0)
#define TCR_RESET (1<<1)
@@ -44,7 +57,14 @@
#define MR3R (1<<10)
#define MR3S (1<<11)
volatile unsigned int timer1_rising[4];
volatile unsigned int timer1_width[4];
unsigned int timer_map[] = {0, 3, 2, 1};
void __attribute__((interrupt("IRQ"))) timer_interrupt_handler(void);
void __attribute__((interrupt("IRQ"))) timer1_interrupt_handler(void);
void timer_event_handler(void);
@@ -58,6 +78,52 @@ void init_timer(void)
TWREG(PC) = 0;
TREG(TCR) = TCR_ENABLE;
interrupt_register(TIMER1, timer1_interrupt_handler);
T1REG(TCR) = TCR_ENABLE | TCR_RESET;
T1REG(CTCR) = 0; /* Use PCLK */
T1WREG(TC) = 0;
T1WREG(PR) = TIMER_PRESCALE ;
T1WREG(PC) = 0;
T1WREG(CCR) = 0x00000fff;
T1REG(TCR) = TCR_ENABLE;
T2REG(TCR) = TCR_ENABLE | TCR_RESET;
T2REG(CTCR) = 0; /* Use PCLK */
T2WREG(PR) = 0; // Prescaling
T2WREG(PC) = 0; // Reset the prescale counter
T2WREG(TC) = 0; // Reset the counter
T2WREG(MCR) = 0x0400; // Reset on MR3 match
T2WREG(PWM) = 0x0000000d; // Enable PWMs
T2WREG(MR3) = PWM_PERIOD; // Period duration
/* This is chosen to be an invalid output. */
T2WREG(MR1) = 1; // Pulse width
T2WREG(MR0) = 1; // Pulse width
T3REG(TCR) = TCR_ENABLE | TCR_RESET;
T3REG(CTCR) = 0; /* Use PCLK */
T3WREG(PR) = 0; // Prescaling
T3WREG(PC) = 0; // Reset the prescale counter
T3WREG(TC) = 0; // Reset the counter
T3WREG(MCR) = 0x0010; // Reset on MR1 match
T3WREG(PWM) = 0x0000000b; // Enable PWMs
T3WREG(MR1) = PWM_PERIOD; // Period duration
/* This is chosen to be an invalid output. */
T3WREG(MR3) = 1; // Pulse width
T3WREG(MR0) = 1; // Pulse width
T2REG(TCR) = TCR_ENABLE;
T3REG(TCR) = TCR_ENABLE;
}
unsigned int timer_read(void)
@@ -92,3 +158,109 @@ void __attribute__((interrupt("IRQ"))) timer_interrupt_handler(void)
interrupt_clear();
}
void __attribute__((interrupt("IRQ"))) timer1_interrupt_handler(void)
{
unsigned int ir;
unsigned int gpio;
ir = T1REG(IR);
T1REG(IR) = ir;
gpio = FP0XVAL;
if (ir & (1<<4)) {
/* Capture channel 0 */
if (gpio & (1<<10)) {
timer1_rising[0] = T1WREG(CR0);
} else {
timer1_width[0] = T1WREG(CR0) - timer1_rising[0];
}
}
if (ir & (1<<5)) {
/* Capture channel 1 */
if (gpio & (1<<11)) {
timer1_rising[1] = T1WREG(CR1);
} else {
timer1_width[1] = T1WREG(CR1) - timer1_rising[1];
}
}
if (ir & (1<<6)) {
/* Capture channel 2 */
if (gpio & (1<<17)) {
timer1_rising[2] = T1WREG(CR2);
} else {
timer1_width[2] = T1WREG(CR2) - timer1_rising[2];
}
}
if (ir & (1<<7)) {
/* Capture channel 3 */
if (gpio & (1<<18)) {
timer1_rising[3] = T1WREG(CR3);
} else {
timer1_width[3] = T1WREG(CR3) - timer1_rising[3];
}
}
interrupt_clear();
}
bool timer_valid(int channel) {
channel = TIMER_CH(channel);
/* Be careful here to ensure that this can't be in the past */
unsigned int chtime = timer1_rising[channel]; /* Atomic */
unsigned int time = T1WREG(TC); /* Atomic */
return (time - chtime) < TIMER_INPUT_TIMEOUT;
}
bool timer_allvalid(void) {
unsigned int time;
unsigned int chtime[4];
int i;
/* Be careful here to ensure that this can't be in the past */
for (i = 0; i < 4; i++)
chtime[i] = timer1_rising[i];
time = T1WREG(TC);
for (i = 0; i < 4; i++)
if ((time - chtime[i]) >= TIMER_INPUT_TIMEOUT)
return FALSE;
return TRUE;
}
void timer_set_pwm_value(int channel, int value)
{
value = PWM_PERIOD - (PWM_MAX + value);
switch (channel) {
case 0:
T2WREG(MR2) = value;
break;
case 1:
T2WREG(MR0) = value;
break;
case 2:
T3WREG(MR3) = value;
break;
case 3:
T3WREG(MR0) = value;
break;
}
}
void timer_set_pwm_invalid(int channel)
{
int value = 1;
switch (channel) {
case 0:
T2WREG(MR2) = value;
break;
case 1:
T2WREG(MR0) = value;
break;
case 2:
T3WREG(MR3) = value;
break;
case 3:
T3WREG(MR0) = value;
break;
}
}

20
timer.h
View File

@@ -1,6 +1,8 @@
#ifndef __TIMER_H
#define __TIMER_H
#include "types.h"
#define TIMER_PCLK 14745600
#define TIMER_PRESCALE 0
@@ -8,12 +10,30 @@
#define TIMER_MS (TIMER_SECOND/1000)
#define TIMER_US (TIMER_SECOND/1000000)
#define PWM_MAX 14745
#if 0
#define PWM_PERIOD 58980
#endif
#define PWM_PERIOD ((4*PWM_MAX)+1)
#define TIMER_INPUT_TIMEOUT (TIMER_PCLK/10)
#define TIMER_CH(x) (timer_map[x])
extern volatile unsigned int timer1_width[];
extern unsigned int timer_map[];
void init_timer(void);
unsigned int timer_read(void);
void timer_delay_clocks(unsigned int clocks);
void timer_set_period(unsigned int period);
void timer_set_pwm_value(int channel, int value);
void timer_set_pwm_invalid(int channel);
bool timer_valid(int channel);
bool timer_allvalid(void);
#define timer_delay_us(x) timer_delay_clocks((x)*TIMER_US)
#define timer_delay_ms(x) timer_delay_clocks((x)*TIMER_MS)
#define timer_input(ch) (timer1_width[TIMER_CH(ch)])
#endif /* __TIMER_H */

44
trig.c Normal file
View File

@@ -0,0 +1,44 @@
/* trig.c */
/* Cosine implementation from:
* http://www.ganssle.com/articles/atrig.htm
*/
double cosine(double x)
{
double p0,p1,p2,p3,p4,p5,y,t,absx,frac,pi2;
int quad;
p0= 0.999999999781;
p1=-0.499999993585;
p2= 0.041666636258;
p3=-0.0013888361399;
p4= 0.00002476016134;
p5=-0.00000026051495;
pi2=1.570796326794896; /* pi/2 */
absx=x;
if (x<0) absx=-absx; /* absolute value of input */
quad=(int) (absx/pi2); /* quadrant (0 to 3) */
if (quad > 3) {
int q = quad & ~3; /* round down to the next multiple of 4 */
absx = absx / (pi2 * quad);
quad -= q;
t = 0.0; /* hello, compiler warnings */
}
frac= (absx/pi2) - quad; /* fractional part of input */
if(quad==0) t=frac * pi2;
if(quad==1) t=(1-frac) * pi2;
if(quad==2) t=frac * pi2;
if(quad==3) t=(frac-1) * pi2;
t=t * t;
y=p0 + (p1*t) + (p2*t*t) + (p3*t*t*t) + (p4*t*t*t*t) + (p5*t*t*t*t*t);
if(quad==2 || quad==1) y=-y; /* correct sign */
return(y);
};
double sine(double x)
{
double pi2=1.570796326794896; /* pi/2 */
x = x - pi2;
if (x < -pi2) x += pi2*4;
return cosine(x);
}

5
trig.h Normal file
View File

@@ -0,0 +1,5 @@
/* trig.h */
double cosine(double x);
double sine(double x);

2
types.h
View File

@@ -7,6 +7,6 @@ typedef int bool;
#define NULL 0
typedef int size_t;
typedef unsigned int size_t;
#endif /* __TYPES_H */

3
uart.c
View File

@@ -38,6 +38,8 @@ volatile bool tx_running;
void __attribute__((interrupt("IRQ"))) uart_interrupt_handler(void);
#ifdef USE_UART
void init_uart(void)
{
UREG(FDR) = 0x10; /* DivAddVal = 0, MulVal = 1 */
@@ -205,3 +207,4 @@ bool getch(char *c) {
uart_rxread = (uart_rxread + 1) % UART_RXBUFSIZE;
return TRUE;
}
#endif

10
uart.h
View File

@@ -3,6 +3,7 @@
#include "types.h"
#ifdef USE_UART
void init_uart(void);
void putch(char c);
void putstr(char *s);
@@ -10,5 +11,14 @@ void putint(unsigned int n);
void putint_s(int n);
void puthex(unsigned int n);
bool getch(char *c);
#else
#define init_uart() ((void)0)
#define putch(x) ((void)0)
#define putstr(x) ((void)0)
#define putint(x) ((void)0)
#define putint_s(x) ((void)0)
#define puthex(x) ((void)0)
#define getch(x) (FALSE)
#endif
#endif /* __UART_H */

37
wmp.c
View File

@@ -1,9 +1,11 @@
/* wmp.c */
#include "wmp.h"
#include "i2c.h"
#include "uart.h"
#include "dcm.h"
#include "fisqrt.h"
#include "stick.h"
#define WMP_ZERO_COUNT 100
@@ -115,6 +117,7 @@ int accel_z;
bool wmp_update;
bool wmp_zero;
unsigned int wmp_discard;
#define TWO_PI 6.28318531f
#define DEG_TO_RAD (TWO_PI/360.0f)
@@ -201,24 +204,28 @@ void wmp_process_gyro_sample(void)
wmp_generation++;
#if 1
if ((wmp_generation % 2) == 0)
#if SEND_DCM
if ((wmp_generation % 20) == 0)
dcm_send_packet();
#endif
} 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");
if (wmp_discard) {
wmp_discard--;
} else {
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");
}
}
}
}
@@ -268,6 +275,7 @@ void wmp_process_accel_sample(void)
* on that so we'll just fudge it here.
*/
dcm_drift_correction(x, -y, -z);
stick_input();
}
void wmp_event_handler(void)
@@ -287,6 +295,7 @@ void wmp_start_zero(void)
{
wmp_zero = TRUE;
wmp_update = FALSE;
wmp_discard = 100;
wmp_generation = 0;
putstr("Starting zero\r\n");
}

2
wmp.h
View File

@@ -13,6 +13,8 @@ extern bool wmp_yaw_fast;
extern bool wmp_pitch_fast;
extern bool wmp_roll_fast;
extern bool wmp_zero;
bool wmp_init(void);
bool wmp_sample(void);
bool wmp_read_calibration_data(void);