diff --git a/Makefile b/Makefile index 799abbc..90f1c94 100644 --- a/Makefile +++ b/Makefile @@ -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" diff --git a/dcm.c b/dcm.c index dea6e7c..d830612 100644 --- a/dcm.c +++ b/dcm.c @@ -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: "); diff --git a/dcm.h b/dcm.h index 4aacf6c..c2e068e 100644 --- a/dcm.h +++ b/dcm.h @@ -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); diff --git a/interrupt.h b/interrupt.h index 7dbf8f4..1e1d406 100644 --- a/interrupt.h +++ b/interrupt.h @@ -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) diff --git a/main.c b/main.c index 0aab5ba..5f0d124 100644 --- a/main.c +++ b/main.c @@ -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; diff --git a/motor.c b/motor.c new file mode 100644 index 0000000..61e1601 --- /dev/null +++ b/motor.c @@ -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; +} + diff --git a/motor.h b/motor.h new file mode 100644 index 0000000..c0bdb90 --- /dev/null +++ b/motor.h @@ -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); diff --git a/stick.c b/stick.c new file mode 100644 index 0000000..2e35bed --- /dev/null +++ b/stick.c @@ -0,0 +1,147 @@ +/* stick.c */ + +#ifdef WE_HAVE_SQRT +#include +#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++; +} + diff --git a/stick.h b/stick.h new file mode 100644 index 0000000..10c9051 --- /dev/null +++ b/stick.h @@ -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); diff --git a/timer.c b/timer.c index 1e96e76..e118e00 100644 --- a/timer.c +++ b/timer.c @@ -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; + } +} + diff --git a/timer.h b/timer.h index 4dd8f8f..e54f613 100644 --- a/timer.h +++ b/timer.h @@ -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 */ diff --git a/trig.c b/trig.c new file mode 100644 index 0000000..510ad64 --- /dev/null +++ b/trig.c @@ -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); +} diff --git a/trig.h b/trig.h new file mode 100644 index 0000000..793ff9d --- /dev/null +++ b/trig.h @@ -0,0 +1,5 @@ +/* trig.h */ + +double cosine(double x); +double sine(double x); + diff --git a/types.h b/types.h index 44767e2..5e38bdd 100644 --- a/types.h +++ b/types.h @@ -7,6 +7,6 @@ typedef int bool; #define NULL 0 -typedef int size_t; +typedef unsigned int size_t; #endif /* __TYPES_H */ diff --git a/uart.c b/uart.c index 99b5235..774f043 100644 --- a/uart.c +++ b/uart.c @@ -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 diff --git a/uart.h b/uart.h index cff52fc..94debdb 100644 --- a/uart.h +++ b/uart.h @@ -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 */ diff --git a/wmp.c b/wmp.c index 62681e7..2e9b1dd 100644 --- a/wmp.c +++ b/wmp.c @@ -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"); } diff --git a/wmp.h b/wmp.h index e9a3bc8..f71ed19 100644 --- a/wmp.h +++ b/wmp.h @@ -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);