working version

at91_init0.s

@@ -1,4 +1,6 @@
 /***************************************************************************
+ *   sam7fc - stack/bss/data init                                            *
+ *                                                                         *
  *   Copyright (C) 01/2008 by Olaf Rempel                                  *
  *   razzor@kopf-tisch.de                                                  *
  *                                                                         *

at91_init1.c

@@ -1,4 +1,6 @@
 /***************************************************************************
+ *   sam7fc - PLL Init, IRQ/FIQ Vectors                                      *
+ *                                                                         *
  *   Copyright (C) 01/2008 by Olaf Rempel                                  *
  *   razzor@kopf-tisch.de                                                  *
  *                                                                         *

include/at91_adc.h

@@ -3,21 +3,30 @@
 
 #include <stdint.h>
 
-#define ADC_GYRO_ROLL	0
+#define ADC_GYRO_NICK	0
-#define ADC_GYRO_NICK	1
+#define ADC_GYRO_ROLL	1
-#define ADC_GYRO_YAW	2
+#define ADC_GYRO_GIER	2
-#define ADC_ACC_ROLL	3
+
-#define ADC_ACC_NICK	4
+#define ADC_ACC_NICK	3
-#define ADC_ACC_YAW	5
+#define ADC_ACC_ROLL	4
+#define ADC_ACC_GIER	5
+
 #define ADC_VOLTAGE	6
 
-#define ADC_CAL_NONE	0
+// TODO: not all flags are public
-#define ADC_CAL_GYRO	1
+#define ADC_COMPLETE		0x0001
-#define ADC_CAL_ACC	2
+#define ADC_CAL_GYRO		0x0100
+#define ADC_CAL_GYRO_COMPLETE	0x0200
+#define ADC_CAL_ACC		0x1000
+#define ADC_CAL_ACC_COMPLETE	0x2000
+#define ADC_CAL_ACC_LOAD	0x4000
 
 void adc_trigger(void);
+void adc_get_results(int16_t *adc_result);
 
 void adc_calibrate(uint32_t mode);
+void adc_drift_adjust(int16_t nick, int16_t roll, int16_t yaw);
+
 void at91_adc_init(void);
 
-#endif /*AT91_ADC_H_*/
+#endif /* AT91_ADC_H_ */

include/at91_dbgu.h

@@ -1,12 +1,10 @@
 #ifndef AT91_DBGU_H_
 #define AT91_DBGU_H_
 
-#include <stddef.h>
-
 void at91_dbgu_init(void);
 
 void at91_dbgu_putc(char c);
 void at91_dbgu_puts(const char *p);
 int at91_dbgu_write(void *base, const char *buf, size_t len);
 
-#endif /*AT91_DBGU_H_*/
+#endif /* AT91_DBGU_H_ */

include/at91_pitc.h

@@ -17,6 +17,7 @@ struct pitc_timer {
 };
 
 void pitc_schedule_timer(struct pitc_timer *timer);
+void pitc_remove_timer(struct pitc_timer *timer);
 
 uint32_t pitc_get_ticks(void);
 void at91_pitc_init(void);

include/board.h

@@ -40,4 +40,6 @@
 #define MIN(a,b) ((a) < (b) ? (a) : (b))
 #define MAX(a,b) ((a) > (b) ? (a) : (b))
 
+#define LIMIT(val, min, max) (((val) < (min)) ? (min) : (((val) > (max)) ? (max) : (val)))
+
 #endif /*BOARD_H_*/

include/fixed.h

@@ -1,34 +0,0 @@
-#ifndef _FIXED_H_
-#define _FIXED_H_
-
-#include <stdint.h>
-
-typedef int32_t fixed;
-
-fixed itofix(int x);
-int fixtoi(fixed x);
-
-fixed ftofix(double x);
-double fixtof(fixed x);
-
-fixed fadd(fixed x, fixed y);
-fixed fsub(fixed x, fixed y);
-fixed fmul(fixed x, fixed y);
-fixed fdiv(fixed x, fixed y);
-
-const fixed fixtorad_r;
-const fixed radtofix_r;
-
-fixed fcos(fixed x);
-fixed fsin(fixed x);
-fixed ftan(fixed x);
-
-fixed facos(fixed x);
-fixed fasin(fixed x);
-fixed fatan(fixed x);
-
-fixed fatan2(fixed y, fixed x);
-
-fixed fsqrt(fixed n);
-
-#endif /* _FIXED_H_ */

include/pidctrl.h

@@ -6,8 +6,6 @@
 struct pid_data {
 	int32_t kp;
 
-	int32_t ta;
-
 	int32_t ki;
 	int32_t err_sum;
 	int32_t err_sum_max;

include/tdc_proto.h

@@ -0,0 +1,103 @@
+#ifndef TDCPROTO_H_
+#define TDCPROTO_H_
+
+#include <stdint.h>
+
+/*
+ * 0: this is a request (host -> board)
+ * 1: this is a reply (board -> host)
+ */
+#define TDC_DIR		0x80
+#define TDC_REPLY	TDC_DIR
+
+/*
+ * TDC_DIR = 0: destination address
+ * TDC_DIR = 1: source address
+ */
+#define TDC_ADDRMASK	0x70
+#define TDC_ADDR0	0x00		// host (dynamic!, sends to interface of last hello)
+#define TDC_ADDR1	0x10		// flightcontrol
+#define TDC_ADDR2	0x20		// missioncontrol
+#define TDC_ADDR3	0x30		// videocontrol
+#define TDC_ADDR4	0x40
+#define TDC_ADDR5	0x50
+#define TDC_ADDR6	0x60
+#define TDC_ADDR7	0x70
+
+#define TDC_OPCODEMASK	0x0F
+#define TDC_HELLO	0x00		// sets the path/interface to the host, reply is a info string
+#define TDC_GETVARS	0x01		// request variable names, many replies
+#define TDC_GETVALUE	0x02		// get one value, one reply
+#define TDC_SETVALUE	0x03		// set one value, no reply
+#define TDC_REQVALUES	0x04		// registers a periodic update, timed replies
+#define TDC_TERMINAL	0x05		// stdout data
+
+#define TDC_USERDATA	0x0F		// user-defined data e.g. between boards
+
+struct tdc_pkt_header {
+	uint8_t cmd;			// TDC_*
+	uint8_t size;			// size including this header
+} __attribute__ ((packed));
+
+struct tdc_hello_reply {
+	uint8_t cmd;
+	uint8_t size;
+	char name[32];			// name of device, version string
+} __attribute__ ((packed));
+
+struct tdc_getvars_reply {
+	uint8_t cmd;
+	uint8_t size;
+	uint8_t id;			// variable ID (max 256 vars / board)
+	uint32_t flags;			// variable parameters (type, size, ro/rw)
+	uint8_t name_len;		// size of variable name
+	char name[0];			// variable name, excluding '\0'
+} __attribute__ ((packed));
+
+struct tdc_getvalue_request {
+	uint8_t cmd;
+	uint8_t size;
+	uint8_t id;			// variable ID (max 256 vars / board)
+} __attribute__ ((packed));
+
+struct tdc_getvalue_reply {
+	uint8_t cmd;
+	uint8_t size;
+	uint8_t id;			// variable ID (max 256 vars / board)
+	uint8_t data[0];		// variable data 1-8 bytes
+} __attribute__ ((packed));
+
+struct tdc_setvalue_request {
+	uint8_t cmd;
+	uint8_t size;
+	uint8_t id;			// variable ID (max 256 vars / board)
+	uint8_t data[0];		// variable data 1-8 bytes
+} __attribute__ ((packed));
+
+struct tdc_reqvalues_request {
+	uint8_t cmd;
+	uint8_t size;
+	uint16_t interval;		// interval in ms
+	uint32_t varmap[8];		// bitmap of variables (32 * 8 = 256)
+} __attribute__ ((packed));
+
+struct tdc_reqvalues_reply {
+	uint8_t cmd;
+	uint8_t size;
+	uint32_t timestamp;		// internal jiffie count
+	uint8_t cnt;			// number of variables
+} __attribute__ ((packed));
+
+#define TDC_SIZEMASK	0x000F
+
+#define TDC_TYPEMASK	0x00F0
+#define TDC_UNSIGNED	0x0000
+#define TDC_SIGNED	0x0010
+#define TDC_FP		0x0020
+#define TDC_FIXED	0x0040
+
+#define TDC_READONLY	0x0100
+
+#define TDC_GUI_GRAPH	0x8000
+
+#endif /* TDCPROTO_H_ */

include/telemetrie.h

@@ -1,108 +1,19 @@
 #ifndef TELEMETRIE_H_
 #define TELEMETRIE_H_
 
-/*
+#include "tdc_proto.h"
- * 0: this is a request (host -> board)
- * 1: this is a reply (board -> host)
- */
-#define TDC_DIR		0x80
-#define TDC_REPLY	TDC_DIR
-
-/*
- * TDC_DIR = 0: destination address
- * TDC_DIR = 1: source address
- */
-#define TDC_ADDRMASK	0x70
-#define TDC_ADDR0	0x00		// host (dynamic!, sends to interface of last hello)
-#define TDC_ADDR1	0x10		// flightcontrol
-#define TDC_ADDR2	0x20		// missioncontrol
-#define TDC_ADDR3	0x30		// videocontrol
-#define TDC_ADDR4	0x40
-#define TDC_ADDR5	0x50
-#define TDC_ADDR6	0x60
-#define TDC_ADDR7	0x70
-
-#define TDC_OPCODEMASK	0x0F
-#define TDC_HELLO	0x00		// sets the path/interface to the host, reply is a info string
-#define TDC_GETVARS	0x01		// request variable names, many replies
-#define TDC_GETVALUE	0x02		// get one value, one reply
-#define TDC_SETVALUE	0x03		// set one value, no reply
-#define TDC_REQVALUES	0x04		// registers a periodic update, timed replies
-#define TDC_TERMINAL	0x05		// stdout data
-
-#define TDC_USERDATA	0x0F		// user-defined data e.g. between boards
-
-struct tdc_pkt_header {
-	uint8_t cmd;			// TDC_*
-	uint8_t size;			// bytes after size
-} __attribute__ ((packed));
-
-struct tdc_hello_reply {
-	uint8_t cmd;
-	uint8_t size;
-	char name[32];			// name of device, version string
-} __attribute__ ((packed));
-
-struct tdc_getvars_reply {
-	uint8_t cmd;
-	uint8_t size;
-	uint8_t id;			// variable ID (max 256 vars / board)
-	uint32_t flags;			// variable parameters (type, size, ro/rw)
-	char name[0];		// variable name, excluding '\0'
-} __attribute__ ((packed));
-
-struct tdc_getvalue_request {
-	uint8_t cmd;
-	uint8_t size;
-	uint8_t id;			// variable ID (max 256 vars / board)
-} __attribute__ ((packed));
-
-struct tdc_getvalue_reply {
-	uint8_t cmd;
-	uint8_t size;
-	uint8_t id;			// variable ID (max 256 vars / board)
-	uint8_t data[0];		// variable data 1-8 bytes
-} __attribute__ ((packed));
-
-struct tdc_setvalue_request {
-	uint8_t cmd;
-	uint8_t size;
-	uint8_t id;			// variable ID (max 256 vars / board)
-	uint8_t data[0];		// variable data 1-8 bytes
-} __attribute__ ((packed));
-
-struct tdc_reqvalues_request {
-	uint8_t cmd;
-	uint8_t size;
-	uint16_t interval;		// interval in ms
-	uint32_t varmap[8];		// bitmap of variables (32 * 8 = 256)
-} __attribute__ ((packed));
-
-struct tdc_reqvalues_reply {
-	uint8_t cmd;
-	uint8_t size;
-	uint32_t timestamp;		// internal jiffie count
-	uint8_t cnt;			// number of variables
-} __attribute__ ((packed));
-
-
 
 struct comm_device {
 	struct fifo *rxfifo;
 	struct fifo *txfifo;
+	void (*trigger_tx)(void);
 };
 
 void tdc_register_device(uint32_t addr, struct comm_device *device);
-
 int32_t tdc_transmit(uint32_t addr, struct tdc_pkt_header *head);
 
-void tdc_receive(struct comm_device *device);
+void tdc_check(void);
-
+void tdc_init(void);
-
-
-
-
-
 
 struct tdc_value {
 	void *data;
@@ -110,26 +21,45 @@ struct tdc_value {
 	uint32_t flags;
 };
 
-#define TDC_SIZEMASK	0x0F
+#if 1
-#define TDC_UNSIGNED	0x00
-#define TDC_SIGNED	0x10
-#define TDC_FP		0x20
 
-#define TDC_VALUE(var, desc, type, flags) \
+#define TDC_VALUE(name, var, desc, type, flags) \
-	type * tdc_check_##var(void) { return (&var); } \
+	type * tdc_check_##name(void) { return (&var); } \
 	static struct tdc_value __attribute__((used, section(".tdc_value"))) \
-	tdc_value_##var = { &var, desc, sizeof(type) | flags }; \
+	tdc_value_##name = { &var, desc, sizeof(type) | flags }; \
-	tdc_value_##var = tdc_value_##var;
+	tdc_value_##name = tdc_value_##name;
 
-#define TDC_UINT8(var, desc)	TDC_VALUE(var, desc, uint8_t, TDC_UNSIGNED)
+#define TDC_PTR(name, ptr, desc, type, flags) \
-#define TDC_UINT16(var, desc)	TDC_VALUE(var, desc, uint16_t, TDC_UNSIGNED)
+	static struct tdc_value __attribute__((used, section(".tdc_value"))) \
-#define TDC_UINT32(var, desc)	TDC_VALUE(var, desc, uint32_t, TDC_UNSIGNED)
+	tdc_value_##name = { ptr, desc, sizeof(type) | flags }; \
-#define TDC_UINT64(var, desc)	TDC_VALUE(var, desc, uint64_t, TDC_UNSIGNED)
+
-#define TDC_INT8(var, desc)	TDC_VALUE(var, desc, int8_t, TDC_SIGNED)
+#else
-#define TDC_INT16(var, desc)	TDC_VALUE(var, desc, int16_t, TDC_SIGNED)
+
-#define TDC_INT32(var, desc)	TDC_VALUE(var, desc, int32_t, TDC_SIGNED)
+#define TDC_VALUE(name, var, desc, type, flags)
-#define TDC_INT64(var, desc)	TDC_VALUE(var, desc, int64_t, TDC_SIGNED)
+#define TDC_PTR(name, ptr, desc, type, flags);
-#define TDC_FLOAT(var, desc)	TDC_VALUE(var, desc, float, TDC_FP)
+
-#define TDC_DOUBLE(var, desc)	TDC_VALUE(var, desc, double, TDC_FP)
+#endif
+
+#define TDC_UINT8(var, desc)	TDC_VALUE(var, var, desc, uint8_t, TDC_UNSIGNED)
+#define TDC_UINT16(var, desc)	TDC_VALUE(var, var, desc, uint16_t, TDC_UNSIGNED)
+#define TDC_UINT32(var, desc)	TDC_VALUE(var, var, desc, uint32_t, TDC_UNSIGNED)
+#define TDC_UINT64(var, desc)	TDC_VALUE(var, var, desc, uint64_t, TDC_UNSIGNED)
+#define TDC_INT8(var, desc)	TDC_VALUE(var, var, desc, int8_t, TDC_SIGNED)
+#define TDC_INT16(var, desc)	TDC_VALUE(var, var, desc, int16_t, TDC_SIGNED)
+#define TDC_INT32(var, desc)	TDC_VALUE(var, var, desc, int32_t, TDC_SIGNED)
+#define TDC_INT64(var, desc)	TDC_VALUE(var, var, desc, int64_t, TDC_SIGNED)
+#define TDC_FLOAT(var, desc)	TDC_VALUE(var, var, desc, float, TDC_FP)
+#define TDC_DOUBLE(var, desc)	TDC_VALUE(var, var, desc, double, TDC_FP)
+
+#define TDC_UINT8_RO(var, desc)		TDC_VALUE(var, var, desc, uint8_t, TDC_UNSIGNED | TDC_READONLY)
+#define TDC_UINT16_RO(var, desc)	TDC_VALUE(var, var, desc, uint16_t, TDC_UNSIGNED | TDC_READONLY)
+#define TDC_UINT32_RO(var, desc)	TDC_VALUE(var, var, desc, uint32_t, TDC_UNSIGNED | TDC_READONLY)
+#define TDC_UINT64_RO(var, desc)	TDC_VALUE(var, var, desc, uint64_t, TDC_UNSIGNED | TDC_READONLY)
+#define TDC_INT8_RO(var, desc)		TDC_VALUE(var, var, desc, int8_t, TDC_SIGNED | TDC_READONLY)
+#define TDC_INT16_RO(var, desc)		TDC_VALUE(var, var, desc, int16_t, TDC_SIGNED | TDC_READONLY)
+#define TDC_INT32_RO(var, desc)		TDC_VALUE(var, var, desc, int32_t, TDC_SIGNED | TDC_READONLY)
+#define TDC_INT64_RO(var, desc)		TDC_VALUE(var, var, desc, int64_t, TDC_SIGNED | TDC_READONLY)
+#define TDC_FLOAT_RO(var, desc)		TDC_VALUE(var, var, desc, float, TDC_FP | TDC_READONLY)
+#define TDC_DOUBLE_RO(var, desc)	TDC_VALUE(var, var, desc, double, TDC_FP | TDC_READONLY)
 
 #endif /*TELEMETRIE_H_*/

main.c

@@ -1,4 +1,6 @@
 /***************************************************************************
+ *   sam7fc - main loop                                                    *
+ *                                                                         *
  *   Copyright (C) 01/2008 by Olaf Rempel                                  *
  *   razzor@kopf-tisch.de                                                  *
  *                                                                         *
@@ -16,7 +18,12 @@
  *   Free Software Foundation, Inc.,                                       *
  *   59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.             *
  ***************************************************************************/
+#include <stdio.h>
+#include <string.h>
+
 #include "AT91SAM7S256.h"
+#include "board.h"
+
 #include "at91_sysc.h"
 #include "at91_dbgu.h"
 #include "at91_pitc.h"
@@ -28,94 +35,25 @@
 
 #include "at91_tc1.h"
 #include "memalloc.h"
+#include "telemetrie.h"
 
-#include "board.h"
+extern void base_ctrl(void);
-#include <stdio.h>
-#include <string.h>
 
-static uint32_t global_state;
+volatile static uint32_t run_ctrl;
 
-#define MOTORS_RUNNING		0x0001
+static uint32_t base_ctrl_trigger(struct pitc_timer *timer)
-#define STICK_CALIBRATION	0x0002
-
-static uint32_t stick_timer_cb(struct pitc_timer *timer)
 {
-	static uint32_t i;
+	/* trigger adc, after ~15us () result should be ready */
-	*AT91C_PIOA_SODR = i;
+	adc_trigger();
-	i = i ^ LED_GREEN;
-	*AT91C_PIOA_CODR = i;
 
-	struct rc_values rc;
+	run_ctrl = 1;
-	uint32_t count = rcontrol_getswitches(&rc);
 
-	static uint32_t stick_cal_count;
-
-	if (global_state & MOTORS_RUNNING) {
-		if ((rc.chan[2] < 0 && rc.chan[3] > 0) || count < 4)
-			global_state &= ~MOTORS_RUNNING;
-
-	} else if (global_state & STICK_CALIBRATION) {
-		stick_cal_count--;
-		if (stick_cal_count == 0) {
-			rcontrol_calibrate(RC_CAL_END);
-			rcontrol_calibrate(RC_CAL_SAVE);
-			rcontrol_print_cal();
-			global_state &= ~STICK_CALIBRATION;
-		}
-
-	} else if (count >= 4) {
-		if (rc.chan[2] < 0 && rc.chan[3] < 0)
-			global_state |= MOTORS_RUNNING;
-
-		if (rc.chan[2] > 0 && rc.chan[3] > 0)
-			adc_calibrate(ADC_CAL_GYRO);
-
-		if (rc.chan[2] > 0 && rc.chan[3] < 0) {
-			adc_calibrate(ADC_CAL_ACC);
-
-			stick_cal_count = 3000;
-			global_state |= STICK_CALIBRATION;
-			rcontrol_calibrate(RC_CAL_START);
-		}
-	}
-
-	rcontrol_getvalues(&rc);
-
-	static uint8_t pwm[4];
-	if (global_state & MOTORS_RUNNING) {
-		int32_t pitch = rc.chan[2] * 256;
-		int32_t gier = rc.chan[3] * 64;
-		int32_t nick = rc.chan[0] * 64;
-		int32_t roll = rc.chan[1] * 64;
-
-		int32_t data[4];
-		data[0] = (pitch - nick + gier) / 1024;
-		data[1] = (pitch + nick + gier) / 1024;
-		data[2] = (pitch + roll - gier) / 1024;
-		data[3] = (pitch - roll - gier) / 1024;
-
-		pwm[0] = (data[0] > 0x0f) ? data[0] : 0x0f;
-		pwm[1] = (data[1] > 0x0f) ? data[1] : 0x0f;
-		pwm[2] = (data[2] > 0x0f) ? data[2] : 0x0f;
-		pwm[3] = (data[3] > 0x0f) ? data[3] : 0x0f;
-
-	} else {
-		pwm[0] = 0x00;
-		pwm[1] = 0x00;
-		pwm[2] = 0x00;
-		pwm[3] = 0x00;
-	}
-
-//	printf("%3d %3d %3d %3d\n\r", pwm[0], pwm[1], pwm[2], pwm[3]);
-	twi_setpwm(pwm);
-
-//	adc_trigger();
 	return PITC_RESTART_TIMER;
 }
 
-static struct pitc_timer stick_timer = {
+static struct pitc_timer base_timer = {
-	.interval = 10,
+	.interval = 2,
-	.func = &stick_timer_cb,
+	.func = &base_ctrl_trigger,
 };
 
 int main(void)
@@ -150,9 +88,19 @@ int main(void)
 	/* adc, need timer, twi */
 	at91_adc_init();
 
-	pitc_schedule_timer(&stick_timer);
+	pitc_schedule_timer(&base_timer);
+
+	tdc_init();
 
 	printf("static alloc: %5ld bytes\n\r", static_alloc_used());
 
-	while (1);
+	while (1) {
+		if (run_ctrl) {
+			// TODO: racy?
+			run_ctrl = 0;
+
+			base_ctrl();
+		}
+		tdc_check();
+	}
 }

src/at91_adc.c

@@ -1,4 +1,6 @@
 /***************************************************************************
+ *   sam7fc - ADC routines / calibration                                     *
+ *                                                                         *
  *   Copyright (C) 01/2008 by Olaf Rempel                                  *
  *   razzor@kopf-tisch.de                                                  *
  *                                                                         *
@@ -18,28 +20,35 @@
  ***************************************************************************/
 #include <stdint.h>
 #include <stdio.h>
-
 #include "AT91SAM7S256.h"
 #include "board.h"
 #include "at91_adc.h"
 #include "at91_pitc.h"
 #include "at91_twi.h"
+#include "telemetrie.h"
 
-#define ADC_CAL_COUNT	1024
+static uint32_t adc_status;
 
 static uint16_t adc_tmp[7];
-static int16_t adc_result[7];
 static uint16_t adc_offset[6];
 
+TDC_PTR(adc_offset0, &adc_offset[ADC_GYRO_NICK], "XADC_GYRO_NICK (offset)", int16_t, TDC_UNSIGNED);
+TDC_PTR(adc_offset1, &adc_offset[ADC_GYRO_ROLL], "XADC_GYRO_ROLL (offset)", int16_t, TDC_UNSIGNED);
+TDC_PTR(adc_offset2, &adc_offset[ADC_GYRO_GIER], "XADC_GYRO_GIER (offset)", int16_t, TDC_UNSIGNED);
+TDC_PTR(adc_offset3, &adc_offset[ADC_ACC_NICK], "XADC_ACC_NICK (offset)", int16_t, TDC_UNSIGNED);
+TDC_PTR(adc_offset4, &adc_offset[ADC_ACC_ROLL], "XADC_ACC_ROLL (offset)", int16_t, TDC_UNSIGNED);
+TDC_PTR(adc_offset5, &adc_offset[ADC_ACC_GIER], "XADC_ACC_GIER (offset)", int16_t, TDC_UNSIGNED);
+
+#define ADC_CAL_COUNT_MAX 1024
+
+static uint32_t adc_cal_count;
+static uint32_t adc_cal_data[3];
+
 /* check eeprom parameter size (3x uint16_t) */
 #if ((3 * 2) != EE_ACC_CAL_DATA_SIZE)
 #error "invalid EE_ACC_CAL_DATA_SIZE"
 #endif
 
-static uint32_t adc_cal_count;
-volatile static uint32_t adc_cal_mode;
-static uint32_t adc_cal_data[3];
-
 static void at91_adc_isr(void)
 {
 	AT91S_PDC *pdc = AT91C_BASE_PDC_ADC;
@@ -48,71 +57,50 @@ static void at91_adc_isr(void)
 	pdc->PDC_PTCR = AT91C_PDC_RXTEN;
 
 	/* clear interrupts */
-	uint32_t status = *AT91C_ADC_SR;
+	uint32_t dummy = *AT91C_ADC_SR;
-	status = status;
+	dummy = dummy;
 
-	uint32_t i;
+	if (!(adc_status & (ADC_CAL_GYRO | ADC_CAL_ACC)))
-	for (i = 0; i < ARRAY_SIZE(adc_offset); i++)
+		return;
-		adc_result[i] = adc_tmp[i] - adc_offset[i];
 
-	/* (adc / 1024) * 3.3V * (11k / 1k) * 100 */
+	if (adc_status & ADC_CAL_GYRO) {
-	adc_result[ADC_VOLTAGE] = ((uint32_t)adc_tmp[ADC_VOLTAGE] * 3630) / 1024;
+		adc_cal_data[0] += adc_tmp[ADC_GYRO_NICK];
+		adc_cal_data[1] += adc_tmp[ADC_GYRO_ROLL];
+		adc_cal_data[2] += adc_tmp[ADC_GYRO_GIER];
+
+	} else {
+		adc_cal_data[0] += adc_tmp[ADC_ACC_NICK];
+		adc_cal_data[1] += adc_tmp[ADC_ACC_ROLL];
+		adc_cal_data[2] += adc_tmp[ADC_ACC_GIER];
+	}
+
+	adc_cal_count++;
 }
 
 static uint32_t adc_calibrate_cb(struct pitc_timer *timer)
 {
+	if (adc_cal_count < ADC_CAL_COUNT_MAX) {
 		/* trigger next cycle */
 		*AT91C_ADC_CR = AT91C_ADC_START;
-
+		return PITC_RESTART_TIMER;
-	if (adc_cal_mode == ADC_CAL_GYRO) {
-		adc_cal_data[0] += adc_tmp[ADC_GYRO_ROLL];
-		adc_cal_data[1] += adc_tmp[ADC_GYRO_NICK];
-		adc_cal_data[2] += adc_tmp[ADC_GYRO_YAW];
 
 	} else {
-		adc_cal_data[0] += adc_tmp[ADC_ACC_ROLL];
+		if (adc_status & ADC_CAL_GYRO) {
-		adc_cal_data[1] += adc_tmp[ADC_ACC_NICK];
+			adc_offset[ADC_GYRO_NICK] = adc_cal_data[0] / ADC_CAL_COUNT_MAX;
-		adc_cal_data[2] += adc_tmp[ADC_ACC_YAW];
+			adc_offset[ADC_GYRO_ROLL] = adc_cal_data[1] / ADC_CAL_COUNT_MAX;
-	}
+			adc_offset[ADC_GYRO_GIER] = adc_cal_data[2] / ADC_CAL_COUNT_MAX;
 
-	adc_cal_count--;
+			adc_calibrate(ADC_CAL_GYRO_COMPLETE);
-	if (adc_cal_count == 0) {
-		adc_cal_data[0] /= ADC_CAL_COUNT;
-		adc_cal_data[1] /= ADC_CAL_COUNT;
-		adc_cal_data[2] /= ADC_CAL_COUNT;
-
-		if (adc_cal_mode == ADC_CAL_GYRO) {
-			adc_offset[ADC_GYRO_ROLL] = adc_cal_data[0];
-			adc_offset[ADC_GYRO_NICK] = adc_cal_data[1];
-			adc_offset[ADC_GYRO_YAW] = adc_cal_data[2];
-
-			// TODO: check for invalid values (not centered)
-
-			printf("GYRO offsets: %4d/%4d/%4d\n\r",
-				adc_offset[ADC_GYRO_ROLL],
-				adc_offset[ADC_GYRO_NICK],
-				adc_offset[ADC_GYRO_YAW]);
 
 		} else {
-			adc_offset[ADC_ACC_ROLL] = adc_cal_data[0];
+			adc_offset[ADC_ACC_NICK] = adc_cal_data[0] / ADC_CAL_COUNT_MAX;
-			adc_offset[ADC_ACC_NICK] = adc_cal_data[1];
+			adc_offset[ADC_ACC_ROLL] = adc_cal_data[1] / ADC_CAL_COUNT_MAX;
+			adc_offset[ADC_ACC_GIER] = adc_cal_data[2] / ADC_CAL_COUNT_MAX;
 
-			// TODO: only 1/2 offset?
+			adc_calibrate(ADC_CAL_ACC_COMPLETE);
-			adc_offset[ADC_ACC_YAW] = adc_cal_data[2];
-
-			printf("ACC  offsets: %4d/%4d/%4d\n\r",
-				adc_offset[ADC_ACC_ROLL],
-				adc_offset[ADC_ACC_NICK],
-				adc_offset[ADC_ACC_YAW]);
-
-			twi_write_eeprom(EE_ACC_CAL_DATA, (uint8_t *)&(adc_offset[ADC_ACC_ROLL]), EE_ACC_CAL_DATA_SIZE);
 		}
-
-		adc_cal_mode = ADC_CAL_NONE;
 		return PITC_REMOVE_TIMER;
 	}
-
-	return PITC_RESTART_TIMER;
 }
 
 static struct pitc_timer adc_cal_timer = {
@@ -122,28 +110,88 @@ static struct pitc_timer adc_cal_timer = {
 
 void adc_trigger(void)
 {
-	printf("R:%4d:%4d N:%4d:%4d Y:%4d:%4d U:%4d\n\r",
+	// TODO: err in retvalue?
-		adc_result[ADC_GYRO_ROLL], adc_result[ADC_ACC_ROLL],
+	if (!(adc_status & (ADC_CAL_GYRO | ADC_CAL_ACC)))
-		adc_result[ADC_GYRO_NICK], adc_result[ADC_ACC_NICK],
-		adc_result[ADC_GYRO_YAW], adc_result[ADC_ACC_YAW],
-		adc_result[ADC_VOLTAGE]);
-
 		*AT91C_ADC_CR = AT91C_ADC_START;
 }
 
+void adc_get_results(int16_t *adc_result)
+{
+	// TODO: err in retvalue?
+	if (!(adc_status & (ADC_CAL_GYRO | ADC_CAL_ACC))) {
+		uint32_t i;
+		for (i = ADC_GYRO_NICK; i <= ADC_GYRO_GIER; i++)
+			adc_result[i] = (int16_t)(adc_offset[i]) - (int16_t)(adc_tmp[i]);
+
+		for (i = ADC_ACC_NICK; i <= ADC_ACC_GIER; i++)
+			adc_result[i] = (int16_t)(adc_tmp[i]) - (int16_t)(adc_offset[i]);
+
+		/* (adc / 1024) * 3.3V * (11k / 1k) * 100 */
+		adc_result[ADC_VOLTAGE] = ((uint32_t)adc_tmp[ADC_VOLTAGE] * 3630) / 1024;
+	}
+}
+
 void adc_calibrate(uint32_t mode)
 {
-	if (adc_cal_mode != ADC_CAL_NONE)
+	/* disable interrupt */
-		return;
+	*AT91C_ADC_IDR = AT91C_ADC_ENDRX;
 
-	if (mode == ADC_CAL_GYRO || mode == ADC_CAL_ACC) {
+	switch (mode) {
-		adc_cal_count = ADC_CAL_COUNT;
+	case ADC_CAL_GYRO_COMPLETE:
-		adc_cal_mode = mode;
+		adc_status &= ~ADC_CAL_GYRO;
+		break;
+
+	case ADC_CAL_ACC_COMPLETE:
+		twi_write_eeprom(EE_ACC_CAL_DATA,
+				(uint8_t *)&(adc_offset[ADC_ACC_NICK]),
+				EE_ACC_CAL_DATA_SIZE);
+
+		adc_status &= ~ADC_CAL_ACC;
+		break;
+
+	case ADC_CAL_ACC_LOAD:
+		twi_read_eeprom(EE_ACC_CAL_DATA,
+				(uint8_t *)&(adc_offset[ADC_ACC_NICK]),
+				EE_ACC_CAL_DATA_SIZE);
+		break;
+
+	case ADC_CAL_GYRO:
+	case ADC_CAL_ACC:
+		/* calibration in progress.. */
+		if (adc_status & (ADC_CAL_GYRO | ADC_CAL_ACC))
+			break;
+
+		adc_status |= mode;
+		adc_cal_count = 0;
 		adc_cal_data[0] = 0;
 		adc_cal_data[1] = 0;
 		adc_cal_data[2] = 0;
 		pitc_schedule_timer(&adc_cal_timer);
+
+		/* trigger next cycle */
+		*AT91C_ADC_CR = AT91C_ADC_START;
+		break;
 	}
+
+	if (!(adc_status & (ADC_CAL_GYRO | ADC_CAL_ACC))) {
+		printf("ADC offsets: %d/%d/%d %d/%d/%d\n\r",
+			adc_offset[ADC_GYRO_NICK],
+			adc_offset[ADC_GYRO_ROLL],
+			adc_offset[ADC_GYRO_GIER],
+			adc_offset[ADC_ACC_NICK],
+			adc_offset[ADC_ACC_ROLL],
+			adc_offset[ADC_ACC_GIER]);
+	}
+
+	/* enable interrupt */
+	*AT91C_ADC_IER = AT91C_ADC_ENDRX;
+}
+
+void adc_drift_adjust(int16_t nick, int16_t roll, int16_t yaw)
+{
+	adc_offset[ADC_GYRO_NICK] += nick;
+	adc_offset[ADC_GYRO_ROLL] += roll;
+	adc_offset[ADC_GYRO_GIER] += yaw;
 }
 
 void at91_adc_init(void)
@@ -185,12 +233,6 @@ void at91_adc_init(void)
 	aic->AIC_SVR[AT91C_ID_ADC] = (uint32_t)at91_adc_isr;
 	aic->AIC_IECR = (1<<AT91C_ID_ADC);
 
+	adc_calibrate(ADC_CAL_ACC_LOAD);
 	adc_calibrate(ADC_CAL_GYRO);
-
-	/* load ACC calibration data */
-	twi_read_eeprom(EE_ACC_CAL_DATA, (uint8_t *)&(adc_offset[ADC_ACC_ROLL]), EE_ACC_CAL_DATA_SIZE);
-	printf("ACC  offsets: %4d/%4d/%4d\n\r",
-		adc_offset[ADC_ACC_ROLL],
-		adc_offset[ADC_ACC_NICK],
-		adc_offset[ADC_ACC_YAW]);
 }

src/at91_dbgu.c

@@ -1,4 +1,6 @@
 /***************************************************************************
+ *   sam7fc - Debug Unit RS232 Port                                        *
+ *                                                                         *
  *   Copyright (C) 01/2008 by Olaf Rempel                                  *
  *   razzor@kopf-tisch.de                                                  *
  *                                                                         *
@@ -24,27 +26,18 @@
 #include "at91_sysc.h"
 #include "fifo.h"
 
+#define DBGU_BAUDRATE	115200
+#define DBGU_FIFO_SIZE	1024
+#define DBGU_TX_CHUNKS	16
+
 static struct fifo *txfifo;
 
 static void dbgu_isr(uint32_t status)
 {
 	/* only enabled interrupts */
 	status &= *AT91C_DBGU_IMR;
-/*
-	if (status & AT91C_US_TXEMPTY) {
-		static char c;
-		if (c == '\n') {
-			*AT91C_DBGU_THR = '\r';
-			c = 0;
-
-		} else if (fifo_getbyte(&txfifo, &c) == 1)
-			*AT91C_DBGU_THR = c;
-		else
-			*AT91C_DBGU_IDR = AT91C_US_TXEMPTY;
-	}
-*/
 	if (status & AT91C_US_TXBUFE)
-		if (fifo_txpdc(txfifo, AT91C_BASE_PDC_DBGU, 16) == 0)
+		if (fifo_txpdc(txfifo, AT91C_BASE_PDC_DBGU, DBGU_TX_CHUNKS) == 0)
 			*AT91C_DBGU_IDR = AT91C_US_TXBUFE;
 }
 
@@ -55,11 +48,11 @@ void at91_dbgu_init(void)
 
 	/* enable Debug Port with 115200 Baud (+0.16%) */
 	AT91S_DBGU *dbgu = AT91C_BASE_DBGU;
-	dbgu->DBGU_BRGR = BAUD_TO_DIV(115200);
+	dbgu->DBGU_BRGR = BAUD_TO_DIV(DBGU_BAUDRATE);
 	dbgu->DBGU_MR = AT91C_US_PAR_NONE | AT91C_US_CHMODE_NORMAL;
 	dbgu->DBGU_CR = AT91C_US_RXEN | AT91C_US_TXEN | AT91C_US_RSTSTA;
 
-	txfifo = fifo_alloc(1024);
+	txfifo = fifo_alloc(DBGU_FIFO_SIZE);
 
 	/* enable TX PDC */
 	dbgu->DBGU_PTCR = AT91C_PDC_TXTEN;
@@ -70,21 +63,18 @@ void at91_dbgu_init(void)
 void at91_dbgu_putc(char c)
 {
 	fifo_putbyte(txfifo, c);
-//	*AT91C_DBGU_IER = AT91C_US_TXEMPTY;
 	*AT91C_DBGU_IER = AT91C_US_TXBUFE;
 }
 
 void at91_dbgu_puts(const char *p)
 {
 	fifo_put(txfifo, (char *)p, strlen(p));
-//	*AT91C_DBGU_IER = AT91C_US_TXEMPTY;
 	*AT91C_DBGU_IER = AT91C_US_TXBUFE;
 }
 
 int at91_dbgu_write(void *base, const char *buf, size_t len)
 {
 	int retval = fifo_put(txfifo, (char *)buf, len);
-//	*AT91C_DBGU_IER = AT91C_US_TXEMPTY;
 	*AT91C_DBGU_IER = AT91C_US_TXBUFE;
 	return retval;
 }

src/at91_exceptions.c

@@ -1,4 +1,6 @@
 /***************************************************************************
+ *   sam7fc - Abort Handler with Register/Stackdump                        *
+ *                                                                         *
  *   Copyright (C) 01/2008 by Olaf Rempel                                  *
  *   razzor@kopf-tisch.de                                                  *
  *                                                                         *
@@ -181,9 +183,8 @@ __attribute__((naked)) void ABT_Handler(void)
 		"mov	r3, lr					\n\t"
 
 		/* enter previous mode and get lr(r14), sp(r13) */
-		/* TODO: interrupts might be enabled? */
+		"orr	r1, r0, #I_BIT | F_BIT			\n\t"
-		/* TODO: thumb mode enabled? */
+		"msr	CPSR_c, r1				\n\t"
-		"msr	CPSR_c, r0				\n\t"
 		"mov	r1, sp					\n\t"
 		"mov	r2, lr					\n\t"
 
@@ -194,7 +195,7 @@ __attribute__((naked)) void ABT_Handler(void)
 		"stmfd	sp!, { r1-r3 }				\n\t"
 		"mov	r1, sp					\n\t"
 
-		/* execute C Handler (cpsr, registers) */
+		/* execute C Handler (cpsr, *registers) */
 		"ldr	r5, =at91_abt_handler			\n\t"
 		"mov	lr, pc					\n\t"
 		"bx	r5					\n\t"

src/at91_pio.c

@@ -1,4 +1,6 @@
 /***************************************************************************
+ *   sam7fc - Pinchange Interrupt Handler                                  *
+ *                                                                         *
  *   Copyright (C) 01/2008 by Olaf Rempel                                  *
  *   razzor@kopf-tisch.de                                                  *
  *                                                                         *

src/at91_pitc.c

@@ -1,4 +1,6 @@
 /***************************************************************************
+ *   sam7fc - Periodic Timer Handling                                      *
+ *                                                                         *
  *   Copyright (C) 01/2008 by Olaf Rempel                                  *
  *   razzor@kopf-tisch.de                                                  *
  *                                                                         *
@@ -21,13 +23,15 @@
 #include "at91_sysc.h"
 #include "board.h"
 
+#define PITC_HZ		1000
+
 /* PIV is 20bit -> min. 3Hz @48MHz MCK */
 #define HZ_TO_PIV(HZ) (MCK / (16 * HZ))
 
 static LIST_HEAD(timer_list);
 volatile static uint32_t pitc_ticks;
 
-void pitc_schedule_timer(struct pitc_timer *timer)
+static void _pitc_schedule_timer(struct pitc_timer *timer)
 {
 	timer->nextrun = timer->interval + pitc_ticks;
 
@@ -39,6 +43,26 @@ void pitc_schedule_timer(struct pitc_timer *timer)
 	list_add_tail(&timer->list, &search->list);
 }
 
+void pitc_schedule_timer(struct pitc_timer *timer)
+{
+	/* disable PITC interrupt */
+	*AT91C_PITC_PIMR &= ~AT91C_PITC_PITIEN;
+
+	/* check it timer is already running */
+	if (timer->nextrun == 0 && timer->interval > 0)
+		_pitc_schedule_timer(timer);
+
+	// TODO: if timer is running, interval changes are delayed
+
+	/* enable PITC interrupt */
+	*AT91C_PITC_PIMR |= AT91C_PITC_PITIEN;
+}
+
+void pitc_remove_timer(struct pitc_timer *timer)
+{
+	timer->interval = 0;
+}
+
 static void pitc_isr(uint32_t status)
 {
 	/* get Ticks and clear interrupt */
@@ -54,13 +78,14 @@ static void pitc_isr(uint32_t status)
 		list_del(&search->list);
 
 		/* exec handler */
-		if (search->func(search) == PITC_REMOVE_TIMER) {
+		if ((search->interval == 0) || search->func(search) == PITC_REMOVE_TIMER) {
 			/* one-shot timer, mark as completed */
-			search->nextrun = 0x00;
+			search->nextrun = 0;
 			continue;
 		}
+
 		/* interval timer, reschedule it */
-		pitc_schedule_timer(search);
+		_pitc_schedule_timer(search);
 	}
 }
 
@@ -73,7 +98,7 @@ void at91_pitc_init(void)
 {
 	sysc_register_isr(AT91_SYSIRQ_PIT, &pitc_isr);
 
-	*AT91C_PITC_PIMR = (AT91C_PITC_PIV & HZ_TO_PIV(1000)) |
+	*AT91C_PITC_PIMR = (AT91C_PITC_PIV & HZ_TO_PIV(PITC_HZ)) |
 			   AT91C_PITC_PITEN |
 			   AT91C_PITC_PITIEN;
 }

src/at91_rttc_test.c

@@ -1,3 +1,23 @@
+/***************************************************************************
+ *   sam7fc - Real Time Clock Calibration                                  *
+ *                                                                         *
+ *   Copyright (C) 02/2008 by Olaf Rempel                                  *
+ *   razzor@kopf-tisch.de                                                  *
+ *                                                                         *
+ *   This program is free software; you can redistribute it and/or modify  *
+ *   it under the terms of the GNU General Public License as published by  *
+ *   the Free Software Foundation; version 2 of the License                *
+ *                                                                         *
+ *   This program is distributed in the hope that it will be useful,       *
+ *   but WITHOUT ANY WARRANTY; without even the implied warranty of        *
+ *   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the         *
+ *   GNU General Public License for more details.                          *
+ *                                                                         *
+ *   You should have received a copy of the GNU General Public License     *
+ *   along with this program; if not, write to the                         *
+ *   Free Software Foundation, Inc.,                                       *
+ *   59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.             *
+ ***************************************************************************/
 #include <stdio.h>
 #include "AT91SAM7S256.h"
 #include "at91_sysc.h"
@@ -6,11 +26,6 @@
 static void rtt_isr(uint32_t status)
 {
 	*AT91C_RTTC_RTAR = *AT91C_RTTC_RTVR +1;
-
-	static uint32_t i;
-	*AT91C_PIOA_SODR = i;
-	i = i ^ LED_ORANGE;
-	*AT91C_PIOA_CODR = i;
 }
 
 void at91_rttc_test_init(void)

src/at91_sysc.c

@@ -1,4 +1,6 @@
 /***************************************************************************
+ *   sam7fc - System Interrupt Dispatcher                                  *
+ *                                                                         *
  *   Copyright (C) 01/2008 by Olaf Rempel                                  *
  *   razzor@kopf-tisch.de                                                  *
  *                                                                         *

src/at91_tc1.c

@@ -1,4 +1,6 @@
 /***************************************************************************
+ *   sam7fc - RC-PPM Signal decoder                                        *
+ *                                                                         *
  *   Copyright (C) 01/2008 by Olaf Rempel                                  *
  *   razzor@kopf-tisch.de                                                  *
  *                                                                         *
@@ -133,15 +135,9 @@ uint32_t rcontrol_getvalues(struct rc_values *rc)
 		int32_t tmp = (uint32_t)(width - ch_data[i].mid) * VALUE_RANGE;
 		tmp = tmp / ((tmp > 0) ? (ch_data[i].max - ch_data[i].mid) : (ch_data[i].mid - ch_data[i].min));
 
-		/* keep result in range */
-		if (tmp > VALUE_RANGE)
-			tmp = VALUE_RANGE;
-
-		if (tmp < -VALUE_RANGE)
-			tmp = -VALUE_RANGE;
-
 		// TODO: stick mapping
-		rc->chan[i] = tmp;
+		/* keep result in range */
+		rc->chan[i] = LIMIT(tmp, -VALUE_RANGE, +VALUE_RANGE);
 	}
 	return cnt;
 }
@@ -189,11 +185,11 @@ void rcontrol_calibrate(uint32_t mode)
 			/* treat current position as center */
 			ch_data[i].mid = ch_data[i].width_slow;
 
-			/* if center is near minimum, clamp output to 0..+1024 */
+			/* if center is near minimum, clamp output to 0..+RANGE */
 			if (ch_data[i].mid - ch_data[i].min < PULSE_MID_DIFF)
 				ch_data[i].mid = ch_data[i].min;
 
-			/* if center is near maximum, clamp output to -1024..0 */
+			/* if center is near maximum, clamp output to -RANGE..0 */
 			if (ch_data[i].max - ch_data[i].mid < PULSE_MID_DIFF)
 				ch_data[i].mid = ch_data[i].max;
 		}
@@ -204,7 +200,7 @@ void rcontrol_calibrate(uint32_t mode)
 		for (i = 0; i < ARRAY_SIZE(ch_data); i++) {
 			ch_data[i].min = *ptr++;
 			ch_data[i].mid = *ptr++;
-			ch_data[i].min = *ptr++;
+			ch_data[i].max = *ptr++;
 		}
 		break;
 
@@ -212,7 +208,7 @@ void rcontrol_calibrate(uint32_t mode)
 		for (i = 0; i < ARRAY_SIZE(ch_data); i++) {
 			*ptr++ = ch_data[i].min;
 			*ptr++ = ch_data[i].mid;
-			*ptr++ = ch_data[i].min;
+			*ptr++ = ch_data[i].max;
 		}
 		twi_write_eeprom(EE_RC_CAL_DATA, buf, EE_RC_CAL_DATA_SIZE);
 		break;

src/at91_twi.c

@@ -1,4 +1,6 @@
 /***************************************************************************
+ *   sam7fc - TWI/I2C Handling                                             *
+ *                                                                         *
  *   Copyright (C) 02/2008 by Olaf Rempel                                  *
  *   razzor@kopf-tisch.de                                                  *
  *                                                                         *

src/at91_udp.c

@@ -1,4 +1,6 @@
 /***************************************************************************
+ *   sam7fc - USB Device Port with logical Serial Port                     *
+ *                                                                         *
  *   Copyright (C) 01/2008 by Olaf Rempel                                  *
  *   razzor@kopf-tisch.de                                                  *
  *                                                                         *
@@ -21,6 +23,7 @@
 #include "at91_pio.h"
 #include "board.h"
 #include "fifo.h"
+#include "telemetrie.h"
 
 #include "usb_ch9.h"
 #include "usb_cdc.h"
@@ -66,6 +69,8 @@ static uint16_t current_address;
 static uint16_t current_config;
 static uint16_t current_interface;
 
+static struct comm_device usb_comm;
+
 static const struct usb_device_descriptor dev_descriptor = {
 	.bLength		= sizeof(struct usb_device_descriptor),
 	.bDescriptorType	= USB_DT_DEVICE,
@@ -317,6 +322,12 @@ static void udp_txcb_setconfig(void)
 	udp_configure_ep(&cfg_descriptor.datain_ep);
 	udp_configure_ep(&cfg_descriptor.dataout_ep);
 
+	ep_ctx[1].fifo = usb_comm.rxfifo;
+	ep_ctx[1].flags |= CTX_FIFO;
+
+	ep_ctx[2].fifo = usb_comm.txfifo;
+	ep_ctx[2].flags |= CTX_FIFO;
+
 	/* set UDP to "configured" */
 	*AT91C_UDP_GLBSTATE = AT91C_UDP_CONFG;
 	udp_print_config();
@@ -473,9 +484,13 @@ static void udp_handle_ep(uint32_t ep)
 
 		if (ctx->flags & CTX_FIFO) {
 			/* get data from fifo */
-			if (fifo_txudp(ctx->fifo, ep, ctx->maxpktsize))
+			if (fifo_txudp(ctx->fifo, ep, ctx->maxpktsize)) {
 				AT91C_UDP_CSR[ep] |= AT91C_UDP_TXPKTRDY;
 
+			} else {
+				ctx->flags &= ~CTX_IN;
+			}
+
 		} else if ((ctx->flags & (CTX_TRANSFER | CTX_IN)) == (CTX_TRANSFER | CTX_IN)) {
 			/* transfer not complete */
 			struct ep_transfer *transfer = ctx->transfer;
@@ -599,6 +614,21 @@ static void udp_isr(void)
 				AT91C_UDP_ENDBUSRES | AT91C_UDP_WAKEUP);
 }
 
+static void trigger_fifo_tx(void)
+{
+	struct ep_ctx *ctx = &ep_ctx[2];
+
+	/* currently transmitting, no need to trigger */
+	// TODO: racy?
+	if (ctx->flags & CTX_IN)
+		return;
+
+	if (fifo_txudp(ctx->fifo, 2, ctx->maxpktsize)) {
+		ctx->flags |= CTX_IN;
+		AT91C_UDP_CSR[2] |= AT91C_UDP_TXPKTRDY;
+	}
+}
+
 void at91_udp_init(void)
 {
 	/* configure & disable Pullup, disable Pullup von VBUS */
@@ -631,6 +661,12 @@ void at91_udp_init(void)
 	aic->AIC_SVR[AT91C_ID_UDP] = (uint32_t)udp_isr;
 	aic->AIC_IECR = (1 << AT91C_ID_UDP);
 
+	usb_comm.rxfifo = fifo_alloc(1024);
+	usb_comm.txfifo = fifo_alloc(1024);
+	usb_comm.trigger_tx = trigger_fifo_tx;
+
+	tdc_register_device(0, &usb_comm);
+
 	pio_trigger_isr(UDP_VBUS_MON);
 }
 

src/fifo.c

@@ -1,4 +1,6 @@
 /***************************************************************************
+ *   sam7fc - FIFOs for use with PDC / USB Hardware                        *
+ *                                                                         *
  *   Copyright (C) 01/2008 by Olaf Rempel                                  *
  *   razzor@kopf-tisch.de                                                  *
  *                                                                         *
@@ -29,7 +31,7 @@
  * all other operations don't need locks:
  * - only fifo_put/fifo_rxpdc are allowed to increment fifo->in
  * - only fifo_get/fifo_txpdc are allowed to increment fifo->out
- * a integer overflow (4gb) of fifo->in / fifo->out could cause trouble
+ * FIXME: a integer overflow (4gb) of fifo->in / fifo->out could cause trouble
  */
 static uint32_t fifo_used(struct fifo *fifo)
 {

src/fixed.c

@@ -1,389 +0,0 @@
-#include <stdint.h>
-
-#include "fixed.h"
-
-#if 0
-fixed itofix(int x)
-{
-	return x << 16;
-}
-
-int fixtoi(fixed x)
-{
-	return (x >> 16) + ((x & 0x8000) >> 15);
-}
-
-/*
-fixed ftofix(double x)
-{
-	if (x > 32767.0) {
-//		errno = ERANGE;
-		return 0x7FFFFFFF;
-	}
-
-	if (x < -32768.0) {
-//		errno = ERANGE;
-		return -0x80000000;
-	}
-
-	return (fixed)(x * 65536.0 + (x < 0 ? -0.5 : 0.5));
-}
-
-double fixtof(fixed x)
-{
-	return (double)x / 65536.0;
-}
-*/
-
-fixed fadd(fixed x, fixed y)
-{
-	return x + y;
-}
-
-fixed fsub(fixed x, fixed y)
-{
-	return x - y;
-}
-
-fixed fmul(fixed x, fixed y)
-{
-	int64_t tmp = (int64_t)x * (int64_t)y;
-	return (fixed)(tmp >> 16);
-}
-
-fixed fdiv(fixed x, fixed y)
-{
-	int64_t tmp = ((int64_t)x) << 32;
-	return (fixed)((tmp / y) >> 16);
-}
-
-/* Ratios for converting between radians and fixed point angles. */
-const fixed fixtorad_r = (fixed)1608;     /* 2pi/256 */
-const fixed radtofix_r = (fixed)2670177;  /* 256/2pi */
-
-/* precalculated fixed point (16.16) cosines for a full circle (0-255) */
-static const fixed _cos_tbl[512] = {
-	65536L,  65531L,  65516L,  65492L,  65457L,  65413L,  65358L,  65294L,
-	65220L,  65137L,  65043L,  64940L,  64827L,  64704L,  64571L,  64429L,
-	64277L,  64115L,  63944L,  63763L,  63572L,  63372L,  63162L,  62943L,
-	62714L,  62476L,  62228L,  61971L,  61705L,  61429L,  61145L,  60851L,
-	60547L,  60235L,  59914L,  59583L,  59244L,  58896L,  58538L,  58172L,
-	57798L,  57414L,  57022L,  56621L,  56212L,  55794L,  55368L,  54934L,
-	54491L,  54040L,  53581L,  53114L,  52639L,  52156L,  51665L,  51166L,
-	50660L,  50146L,  49624L,  49095L,  48559L,  48015L,  47464L,  46906L,
-	46341L,  45769L,  45190L,  44604L,  44011L,  43412L,  42806L,  42194L,
-	41576L,  40951L,  40320L,  39683L,  39040L,  38391L,  37736L,  37076L,
-	36410L,  35738L,  35062L,  34380L,  33692L,  33000L,  32303L,  31600L,
-	30893L,  30182L,  29466L,  28745L,  28020L,  27291L,  26558L,  25821L,
-	25080L,  24335L,  23586L,  22834L,  22078L,  21320L,  20557L,  19792L,
-	19024L,  18253L,  17479L,  16703L,  15924L,  15143L,  14359L,  13573L,
-	12785L,  11996L,  11204L,  10411L,  9616L,   8820L,   8022L,   7224L,
-	6424L,   5623L,   4821L,   4019L,   3216L,   2412L,   1608L,   804L,
-	0L,      -803L,   -1607L,  -2411L,  -3215L,  -4018L,  -4820L,  -5622L,
-	-6423L,  -7223L,  -8021L,  -8819L,  -9615L,  -10410L, -11203L, -11995L,
-	-12784L, -13572L, -14358L, -15142L, -15923L, -16702L, -17478L, -18252L,
-	-19023L, -19791L, -20556L, -21319L, -22077L, -22833L, -23585L, -24334L,
-	-25079L, -25820L, -26557L, -27290L, -28019L, -28744L, -29465L, -30181L,
-	-30892L, -31599L, -32302L, -32999L, -33691L, -34379L, -35061L, -35737L,
-	-36409L, -37075L, -37735L, -38390L, -39039L, -39682L, -40319L, -40950L,
-	-41575L, -42193L, -42805L, -43411L, -44010L, -44603L, -45189L, -45768L,
-	-46340L, -46905L, -47463L, -48014L, -48558L, -49094L, -49623L, -50145L,
-	-50659L, -51165L, -51664L, -52155L, -52638L, -53113L, -53580L, -54039L,
-	-54490L, -54933L, -55367L, -55793L, -56211L, -56620L, -57021L, -57413L,
-	-57797L, -58171L, -58537L, -58895L, -59243L, -59582L, -59913L, -60234L,
-	-60546L, -60850L, -61144L, -61428L, -61704L, -61970L, -62227L, -62475L,
-	-62713L, -62942L, -63161L, -63371L, -63571L, -63762L, -63943L, -64114L,
-	-64276L, -64428L, -64570L, -64703L, -64826L, -64939L, -65042L, -65136L,
-	-65219L, -65293L, -65357L, -65412L, -65456L, -65491L, -65515L, -65530L,
-	-65536L, -65530L, -65515L, -65491L, -65456L, -65412L, -65357L, -65293L,
-	-65219L, -65136L, -65042L, -64939L, -64826L, -64703L, -64570L, -64428L,
-	-64276L, -64114L, -63943L, -63762L, -63571L, -63371L, -63161L, -62942L,
-	-62713L, -62475L, -62227L, -61970L, -61704L, -61428L, -61144L, -60850L,
-	-60546L, -60234L, -59913L, -59582L, -59243L, -58895L, -58537L, -58171L,
-	-57797L, -57413L, -57021L, -56620L, -56211L, -55793L, -55367L, -54933L,
-	-54490L, -54039L, -53580L, -53113L, -52638L, -52155L, -51664L, -51165L,
-	-50659L, -50145L, -49623L, -49094L, -48558L, -48014L, -47463L, -46905L,
-	-46340L, -45768L, -45189L, -44603L, -44010L, -43411L, -42805L, -42193L,
-	-41575L, -40950L, -40319L, -39682L, -39039L, -38390L, -37735L, -37075L,
-	-36409L, -35737L, -35061L, -34379L, -33691L, -32999L, -32302L, -31599L,
-	-30892L, -30181L, -29465L, -28744L, -28019L, -27290L, -26557L, -25820L,
-	-25079L, -24334L, -23585L, -22833L, -22077L, -21319L, -20556L, -19791L,
-	-19023L, -18252L, -17478L, -16702L, -15923L, -15142L, -14358L, -13572L,
-	-12784L, -11995L, -11203L, -10410L, -9615L,  -8819L,  -8021L,  -7223L,
-	-6423L,  -5622L,  -4820L,  -4018L,  -3215L,  -2411L,  -1607L,  -803L,
-	0L,      804L,    1608L,   2412L,   3216L,   4019L,   4821L,   5623L,
-	6424L,   7224L,   8022L,   8820L,   9616L,   10411L,  11204L,  11996L,
-	12785L,  13573L,  14359L,  15143L,  15924L,  16703L,  17479L,  18253L,
-	19024L,  19792L,  20557L,  21320L,  22078L,  22834L,  23586L,  24335L,
-	25080L,  25821L,  26558L,  27291L,  28020L,  28745L,  29466L,  30182L,
-	30893L,  31600L,  32303L,  33000L,  33692L,  34380L,  35062L,  35738L,
-	36410L,  37076L,  37736L,  38391L,  39040L,  39683L,  40320L,  40951L,
-	41576L,  42194L,  42806L,  43412L,  44011L,  44604L,  45190L,  45769L,
-	46341L,  46906L,  47464L,  48015L,  48559L,  49095L,  49624L,  50146L,
-	50660L,  51166L,  51665L,  52156L,  52639L,  53114L,  53581L,  54040L,
-	54491L,  54934L,  55368L,  55794L,  56212L,  56621L,  57022L,  57414L,
-	57798L,  58172L,  58538L,  58896L,  59244L,  59583L,  59914L,  60235L,
-	60547L,  60851L,  61145L,  61429L,  61705L,  61971L,  62228L,  62476L,
-	62714L,  62943L,  63162L,  63372L,  63572L,  63763L,  63944L,  64115L,
-	64277L,  64429L,  64571L,  64704L,  64827L,  64940L,  65043L,  65137L,
-	65220L,  65294L,  65358L,  65413L,  65457L,  65492L,  65516L,  65531L
-};
-
-/* precalculated fixed point (16.16) tangents for a half circle (0-127) */
-static const fixed _tan_tbl[256] = {
-	0L,      804L,    1609L,   2414L,   3220L,   4026L,   4834L,   5644L,
-	6455L,   7268L,   8083L,   8901L,   9721L,   10545L,  11372L,  12202L,
-	13036L,  13874L,  14717L,  15564L,  16416L,  17273L,  18136L,  19005L,
-	19880L,  20762L,  21650L,  22546L,  23449L,  24360L,  25280L,  26208L,
-	27146L,  28093L,  29050L,  30018L,  30996L,  31986L,  32988L,  34002L,
-	35030L,  36071L,  37126L,  38196L,  39281L,  40382L,  41500L,  42636L,
-	43790L,  44963L,  46156L,  47369L,  48605L,  49863L,  51145L,  52451L,
-	53784L,  55144L,  56532L,  57950L,  59398L,  60880L,  62395L,  63947L,
-	65536L,  67165L,  68835L,  70548L,  72308L,  74116L,  75974L,  77887L,
-	79856L,  81885L,  83977L,  86135L,  88365L,  90670L,  93054L,  95523L,
-	98082L,  100736L, 103493L, 106358L, 109340L, 112447L, 115687L, 119071L,
-	122609L, 126314L, 130198L, 134276L, 138564L, 143081L, 147847L, 152884L,
-	158218L, 163878L, 169896L, 176309L, 183161L, 190499L, 198380L, 206870L,
-	216043L, 225990L, 236817L, 248648L, 261634L, 275959L, 291845L, 309568L,
-	329472L, 351993L, 377693L, 407305L, 441808L, 482534L, 531352L, 590958L,
-	665398L, 761030L, 888450L, 1066730L, 1334016L, 1779314L, 2669641L, 5340086L,
-	-2147483647L, -5340085L, -2669640L, -1779313L, -1334015L, -1066729L, -888449L, -761029L,
-	-665397L, -590957L, -531351L, -482533L, -441807L, -407304L, -377692L, -351992L,
-	-329471L, -309567L, -291844L, -275958L, -261633L, -248647L, -236816L, -225989L,
-	-216042L, -206869L, -198379L, -190498L, -183160L, -176308L, -169895L, -163877L,
-	-158217L, -152883L, -147846L, -143080L, -138563L, -134275L, -130197L, -126313L,
-	-122608L, -119070L, -115686L, -112446L, -109339L, -106357L, -103492L, -100735L,
-	-98081L, -95522L, -93053L, -90669L, -88364L, -86134L, -83976L, -81884L,
-	-79855L, -77886L, -75973L, -74115L, -72307L, -70547L, -68834L, -67164L,
-	-65535L, -63946L, -62394L, -60879L, -59397L, -57949L, -56531L, -55143L,
-	-53783L, -52450L, -51144L, -49862L, -48604L, -47368L, -46155L, -44962L,
-	-43789L, -42635L, -41499L, -40381L, -39280L, -38195L, -37125L, -36070L,
-	-35029L, -34001L, -32987L, -31985L, -30995L, -30017L, -29049L, -28092L,
-	-27145L, -26207L, -25279L, -24359L, -23448L, -22545L, -21649L, -20761L,
-	-19879L, -19004L, -18135L, -17272L, -16415L, -15563L, -14716L, -13873L,
-	-13035L, -12201L, -11371L, -10544L, -9720L,  -8900L,  -8082L,  -7267L,
-	-6454L,  -5643L,  -4833L,  -4025L,  -3219L,  -2413L,  -1608L,  -803L
-};
-
-/* precalculated fixed point (16.16) inverse cosines (-1 to 1) */
-static const fixed _acos_tbl[513] = {
-	0x800000L,  0x7C65C7L,  0x7AE75AL,  0x79C19EL,  0x78C9BEL,  0x77EF25L,  0x772953L,  0x76733AL,
-	0x75C991L,  0x752A10L,  0x74930CL,  0x740345L,  0x7379C1L,  0x72F5BAL,  0x72768FL,  0x71FBBCL,
-	0x7184D3L,  0x711174L,  0x70A152L,  0x703426L,  0x6FC9B5L,  0x6F61C9L,  0x6EFC36L,  0x6E98D1L,
-	0x6E3777L,  0x6DD805L,  0x6D7A5EL,  0x6D1E68L,  0x6CC40BL,  0x6C6B2FL,  0x6C13C1L,  0x6BBDAFL,
-	0x6B68E6L,  0x6B1558L,  0x6AC2F5L,  0x6A71B1L,  0x6A217EL,  0x69D251L,  0x698420L,  0x6936DFL,
-	0x68EA85L,  0x689F0AL,  0x685465L,  0x680A8DL,  0x67C17DL,  0x67792CL,  0x673194L,  0x66EAAFL,
-	0x66A476L,  0x665EE5L,  0x6619F5L,  0x65D5A2L,  0x6591E7L,  0x654EBFL,  0x650C26L,  0x64CA18L,
-	0x648890L,  0x64478CL,  0x640706L,  0x63C6FCL,  0x63876BL,  0x63484FL,  0x6309A5L,  0x62CB6AL,
-	0x628D9CL,  0x625037L,  0x621339L,  0x61D69FL,  0x619A68L,  0x615E90L,  0x612316L,  0x60E7F7L,
-	0x60AD31L,  0x6072C3L,  0x6038A9L,  0x5FFEE3L,  0x5FC56EL,  0x5F8C49L,  0x5F5372L,  0x5F1AE7L,
-	0x5EE2A7L,  0x5EAAB0L,  0x5E7301L,  0x5E3B98L,  0x5E0473L,  0x5DCD92L,  0x5D96F3L,  0x5D6095L,
-	0x5D2A76L,  0x5CF496L,  0x5CBEF2L,  0x5C898BL,  0x5C545EL,  0x5C1F6BL,  0x5BEAB0L,  0x5BB62DL,
-	0x5B81E1L,  0x5B4DCAL,  0x5B19E7L,  0x5AE638L,  0x5AB2BCL,  0x5A7F72L,  0x5A4C59L,  0x5A1970L,
-	0x59E6B6L,  0x59B42AL,  0x5981CCL,  0x594F9BL,  0x591D96L,  0x58EBBDL,  0x58BA0EL,  0x588889L,
-	0x58572DL,  0x5825FAL,  0x57F4EEL,  0x57C40AL,  0x57934DL,  0x5762B5L,  0x573243L,  0x5701F5L,
-	0x56D1CCL,  0x56A1C6L,  0x5671E4L,  0x564224L,  0x561285L,  0x55E309L,  0x55B3ADL,  0x558471L,
-	0x555555L,  0x552659L,  0x54F77BL,  0x54C8BCL,  0x549A1BL,  0x546B98L,  0x543D31L,  0x540EE7L,
-	0x53E0B9L,  0x53B2A7L,  0x5384B0L,  0x5356D4L,  0x532912L,  0x52FB6BL,  0x52CDDDL,  0x52A068L,
-	0x52730CL,  0x5245C9L,  0x52189EL,  0x51EB8BL,  0x51BE8FL,  0x5191AAL,  0x5164DCL,  0x513825L,
-	0x510B83L,  0x50DEF7L,  0x50B280L,  0x50861FL,  0x5059D2L,  0x502D99L,  0x500175L,  0x4FD564L,
-	0x4FA967L,  0x4F7D7DL,  0x4F51A6L,  0x4F25E2L,  0x4EFA30L,  0x4ECE90L,  0x4EA301L,  0x4E7784L,
-	0x4E4C19L,  0x4E20BEL,  0x4DF574L,  0x4DCA3AL,  0x4D9F10L,  0x4D73F6L,  0x4D48ECL,  0x4D1DF1L,
-	0x4CF305L,  0x4CC829L,  0x4C9D5AL,  0x4C729AL,  0x4C47E9L,  0x4C1D45L,  0x4BF2AEL,  0x4BC826L,
-	0x4B9DAAL,  0x4B733BL,  0x4B48D9L,  0x4B1E84L,  0x4AF43BL,  0x4AC9FEL,  0x4A9FCDL,  0x4A75A7L,
-	0x4A4B8DL,  0x4A217EL,  0x49F77AL,  0x49CD81L,  0x49A393L,  0x4979AFL,  0x494FD5L,  0x492605L,
-	0x48FC3FL,  0x48D282L,  0x48A8CFL,  0x487F25L,  0x485584L,  0x482BECL,  0x48025DL,  0x47D8D6L,
-	0x47AF57L,  0x4785E0L,  0x475C72L,  0x47330AL,  0x4709ABL,  0x46E052L,  0x46B701L,  0x468DB7L,
-	0x466474L,  0x463B37L,  0x461201L,  0x45E8D0L,  0x45BFA6L,  0x459682L,  0x456D64L,  0x45444BL,
-	0x451B37L,  0x44F229L,  0x44C920L,  0x44A01CL,  0x44771CL,  0x444E21L,  0x44252AL,  0x43FC38L,
-	0x43D349L,  0x43AA5FL,  0x438178L,  0x435894L,  0x432FB4L,  0x4306D8L,  0x42DDFEL,  0x42B527L,
-	0x428C53L,  0x426381L,  0x423AB2L,  0x4211E5L,  0x41E91AL,  0x41C051L,  0x41978AL,  0x416EC5L,
-	0x414601L,  0x411D3EL,  0x40F47CL,  0x40CBBBL,  0x40A2FBL,  0x407A3CL,  0x40517DL,  0x4028BEL,
-	0x400000L,  0x3FD742L,  0x3FAE83L,  0x3F85C4L,  0x3F5D05L,  0x3F3445L,  0x3F0B84L,  0x3EE2C2L,
-	0x3EB9FFL,  0x3E913BL,  0x3E6876L,  0x3E3FAFL,  0x3E16E6L,  0x3DEE1BL,  0x3DC54EL,  0x3D9C7FL,
-	0x3D73ADL,  0x3D4AD9L,  0x3D2202L,  0x3CF928L,  0x3CD04CL,  0x3CA76CL,  0x3C7E88L,  0x3C55A1L,
-	0x3C2CB7L,  0x3C03C8L,  0x3BDAD6L,  0x3BB1DFL,  0x3B88E4L,  0x3B5FE4L,  0x3B36E0L,  0x3B0DD7L,
-	0x3AE4C9L,  0x3ABBB5L,  0x3A929CL,  0x3A697EL,  0x3A405AL,  0x3A1730L,  0x39EDFFL,  0x39C4C9L,
-	0x399B8CL,  0x397249L,  0x3948FFL,  0x391FAEL,  0x38F655L,  0x38CCF6L,  0x38A38EL,  0x387A20L,
-	0x3850A9L,  0x38272AL,  0x37FDA3L,  0x37D414L,  0x37AA7CL,  0x3780DBL,  0x375731L,  0x372D7EL,
-	0x3703C1L,  0x36D9FBL,  0x36B02BL,  0x368651L,  0x365C6DL,  0x36327FL,  0x360886L,  0x35DE82L,
-	0x35B473L,  0x358A59L,  0x356033L,  0x353602L,  0x350BC5L,  0x34E17CL,  0x34B727L,  0x348CC5L,
-	0x346256L,  0x3437DAL,  0x340D52L,  0x33E2BBL,  0x33B817L,  0x338D66L,  0x3362A6L,  0x3337D7L,
-	0x330CFBL,  0x32E20FL,  0x32B714L,  0x328C0AL,  0x3260F0L,  0x3235C6L,  0x320A8CL,  0x31DF42L,
-	0x31B3E7L,  0x31887CL,  0x315CFFL,  0x313170L,  0x3105D0L,  0x30DA1EL,  0x30AE5AL,  0x308283L,
-	0x305699L,  0x302A9CL,  0x2FFE8BL,  0x2FD267L,  0x2FA62EL,  0x2F79E1L,  0x2F4D80L,  0x2F2109L,
-	0x2EF47DL,  0x2EC7DBL,  0x2E9B24L,  0x2E6E56L,  0x2E4171L,  0x2E1475L,  0x2DE762L,  0x2DBA37L,
-	0x2D8CF4L,  0x2D5F98L,  0x2D3223L,  0x2D0495L,  0x2CD6EEL,  0x2CA92CL,  0x2C7B50L,  0x2C4D59L,
-	0x2C1F47L,  0x2BF119L,  0x2BC2CFL,  0x2B9468L,  0x2B65E5L,  0x2B3744L,  0x2B0885L,  0x2AD9A7L,
-	0x2AAAABL,  0x2A7B8FL,  0x2A4C53L,  0x2A1CF7L,  0x29ED7BL,  0x29BDDCL,  0x298E1CL,  0x295E3AL,
-	0x292E34L,  0x28FE0BL,  0x28CDBDL,  0x289D4BL,  0x286CB3L,  0x283BF6L,  0x280B12L,  0x27DA06L,
-	0x27A8D3L,  0x277777L,  0x2745F2L,  0x271443L,  0x26E26AL,  0x26B065L,  0x267E34L,  0x264BD6L,
-	0x26194AL,  0x25E690L,  0x25B3A7L,  0x25808EL,  0x254D44L,  0x2519C8L,  0x24E619L,  0x24B236L,
-	0x247E1FL,  0x2449D3L,  0x241550L,  0x23E095L,  0x23ABA2L,  0x237675L,  0x23410EL,  0x230B6AL,
-	0x22D58AL,  0x229F6BL,  0x22690DL,  0x22326EL,  0x21FB8DL,  0x21C468L,  0x218CFFL,  0x215550L,
-	0x211D59L,  0x20E519L,  0x20AC8EL,  0x2073B7L,  0x203A92L,  0x20011DL,  0x1FC757L,  0x1F8D3DL,
-	0x1F52CFL,  0x1F1809L,  0x1EDCEAL,  0x1EA170L,  0x1E6598L,  0x1E2961L,  0x1DECC7L,  0x1DAFC9L,
-	0x1D7264L,  0x1D3496L,  0x1CF65BL,  0x1CB7B1L,  0x1C7895L,  0x1C3904L,  0x1BF8FAL,  0x1BB874L,
-	0x1B7770L,  0x1B35E8L,  0x1AF3DAL,  0x1AB141L,  0x1A6E19L,  0x1A2A5EL,  0x19E60BL,  0x19A11BL,
-	0x195B8AL,  0x191551L,  0x18CE6CL,  0x1886D4L,  0x183E83L,  0x17F573L,  0x17AB9BL,  0x1760F6L,
-	0x17157BL,  0x16C921L,  0x167BE0L,  0x162DAFL,  0x15DE82L,  0x158E4FL,  0x153D0BL,  0x14EAA8L,
-	0x14971AL,  0x144251L,  0x13EC3FL,  0x1394D1L,  0x133BF5L,  0x12E198L,  0x1285A2L,  0x1227FBL,
-	0x11C889L,  0x11672FL,  0x1103CAL,  0x109E37L,  0x10364BL,  0xFCBDAL,   0xF5EAEL,   0xEEE8CL,
-	0xE7B2DL,   0xE0444L,   0xD8971L,   0xD0A46L,   0xC863FL,   0xBFCBBL,   0xB6CF4L,   0xAD5F0L,
-	0xA366FL,   0x98CC6L,   0x8D6ADL,   0x810DBL,   0x73642L,   0x63E62L,   0x518A6L,   0x39A39L,
-	0x0L
-};
-
-fixed fcos(fixed x)
-{
-	return _cos_tbl[((x + 0x4000) >> 15) & 0x1FF];
-}
-
-fixed fsin(fixed x)
-{
-	return _cos_tbl[((x - 0x400000 + 0x4000) >> 15) & 0x1FF];
-}
-
-fixed ftan(fixed x)
-{
-	return _tan_tbl[((x + 0x4000) >> 15) & 0xFF];
-}
-
-fixed facos(fixed x)
-{
-	/* equivalent to if((x < -65536) || (x > 65536)) */
-	if ((fixed)(x - 0x80000000 + 65536) > (fixed)(-0x80000000 + 65536 + 65536)) {
-//		errno = EDOM;
-		return 0;
-	}
-
-	return _acos_tbl[(x + 65536 + 127) >> 8];
-}
-
-fixed fasin(fixed x)
-{
-	/* equivalent to if((x < -65536) || (x > 65536)) */
-	if ((fixed)(x - 0x80000000 + 65536) > (fixed)(-0x80000000 + 65536 + 65536)) {
-//		errno = EDOM;
-		return 0;
-	}
-
-	return 0x00400000 - _acos_tbl[(x + 65536 + 127) >> 8];
-}
-
-/* Fixed point inverse tangent. Does a binary search on the tan table. */
-fixed fatan(fixed x)
-{
-	int a, b, c;            /* for binary search */
-	fixed d;                /* difference value for search */
-
-	if (x >= 0) {           /* search the first part of tan table */
-		a = 0;
-		b = 127;
-
-	} else {                  /* search the second half instead */
-		a = 128;
-		b = 255;
-	}
-
-	do {
-		c = (a + b) >> 1;
-		d = x - _tan_tbl[c];
-
-		if (d > 0) {
-			a = c + 1;
-
-		} else {
-			if (d < 0)
-				b = c - 1;
-		}
-
-	} while ((a <= b) && (d));
-
-	if (x >= 0)
-		return ((fixed)c) << 15;
-
-	return (-0x00800000L + (((fixed)c) << 15));
-}
-
-/* Like the libc atan2, but for fixed point numbers. */
-fixed fatan2(fixed y, fixed x)
-{
-	fixed r;
-
-	if (x == 0) {
-		if (y == 0) {
-//			errno = EDOM;
-			return 0L;
-
-		} else {
-			return ((y < 0) ? -0x00400000L : 0x00400000L);
-		}
-	}
-
-//	errno = 0;
-	r = fdiv(y,x);
-
-//	if (errno) {
-//		errno = 0;
-//		return ((y < 0) ? -0x00400000L : 0x00400000L);
-//	}
-
-	r = fatan(r);
-
-	if (x >= 0)
-		return r;
-
-	if (y >= 0)
-		return 0x00800000L + r;
-
-	return r - 0x00800000L;
-}
-
-/* this table is used by the fsqrt() function */
-static const int16_t sqrt_table[256] = {
-	0x2D4,   0x103F,  0x16CD,  0x1BDB,  0x201F,  0x23E3,  0x274B,  0x2A6D,
-	0x2D57,  0x3015,  0x32AC,  0x3524,  0x377F,  0x39C2,  0x3BEE,  0x3E08,
-	0x400F,  0x4207,  0x43F0,  0x45CC,  0x479C,  0x4960,  0x4B19,  0x4CC9,
-	0x4E6F,  0x500C,  0x51A2,  0x532F,  0x54B6,  0x5635,  0x57AE,  0x5921,
-	0x5A8D,  0x5BF4,  0x5D56,  0x5EB3,  0x600A,  0x615D,  0x62AB,  0x63F5,
-	0x653B,  0x667D,  0x67BA,  0x68F5,  0x6A2B,  0x6B5E,  0x6C8D,  0x6DBA,
-	0x6EE3,  0x7009,  0x712C,  0x724C,  0x7369,  0x7484,  0x759C,  0x76B1,
-	0x77C4,  0x78D4,  0x79E2,  0x7AEE,  0x7BF7,  0x7CFE,  0x7E04,  0x7F07,
-	0x8007,  0x8106,  0x8203,  0x82FF,  0x83F8,  0x84EF,  0x85E5,  0x86D9,
-	0x87CB,  0x88BB,  0x89AA,  0x8A97,  0x8B83,  0x8C6D,  0x8D56,  0x8E3D,
-	0x8F22,  0x9007,  0x90E9,  0x91CB,  0x92AB,  0x938A,  0x9467,  0x9543,
-	0x961E,  0x96F8,  0x97D0,  0x98A8,  0x997E,  0x9A53,  0x9B26,  0x9BF9,
-	0x9CCA,  0x9D9B,  0x9E6A,  0x9F39,  0xA006,  0xA0D2,  0xA19D,  0xA268,
-	0xA331,  0xA3F9,  0xA4C1,  0xA587,  0xA64D,  0xA711,  0xA7D5,  0xA898,
-	0xA95A,  0xAA1B,  0xAADB,  0xAB9A,  0xAC59,  0xAD16,  0xADD3,  0xAE8F,
-	0xAF4B,  0xB005,  0xB0BF,  0xB178,  0xB230,  0xB2E8,  0xB39F,  0xB455,
-	0xB50A,  0xB5BF,  0xB673,  0xB726,  0xB7D9,  0xB88A,  0xB93C,  0xB9EC,
-	0xBA9C,  0xBB4B,  0xBBFA,  0xBCA8,  0xBD55,  0xBE02,  0xBEAE,  0xBF5A,
-	0xC005,  0xC0AF,  0xC159,  0xC202,  0xC2AB,  0xC353,  0xC3FA,  0xC4A1,
-	0xC548,  0xC5ED,  0xC693,  0xC737,  0xC7DC,  0xC87F,  0xC923,  0xC9C5,
-	0xCA67,  0xCB09,  0xCBAA,  0xCC4B,  0xCCEB,  0xCD8B,  0xCE2A,  0xCEC8,
-	0xCF67,  0xD004,  0xD0A2,  0xD13F,  0xD1DB,  0xD277,  0xD312,  0xD3AD,
-	0xD448,  0xD4E2,  0xD57C,  0xD615,  0xD6AE,  0xD746,  0xD7DE,  0xD876,
-	0xD90D,  0xD9A4,  0xDA3A,  0xDAD0,  0xDB66,  0xDBFB,  0xDC90,  0xDD24,
-	0xDDB8,  0xDE4C,  0xDEDF,  0xDF72,  0xE004,  0xE096,  0xE128,  0xE1B9,
-	0xE24A,  0xE2DB,  0xE36B,  0xE3FB,  0xE48B,  0xE51A,  0xE5A9,  0xE637,
-	0xE6C5,  0xE753,  0xE7E1,  0xE86E,  0xE8FB,  0xE987,  0xEA13,  0xEA9F,
-	0xEB2B,  0xEBB6,  0xEC41,  0xECCB,  0xED55,  0xEDDF,  0xEE69,  0xEEF2,
-	0xEF7B,  0xF004,  0xF08C,  0xF114,  0xF19C,  0xF223,  0xF2AB,  0xF332,
-	0xF3B8,  0xF43E,  0xF4C4,  0xF54A,  0xF5D0,  0xF655,  0xF6DA,  0xF75E,
-	0xF7E3,  0xF867,  0xF8EA,  0xF96E,  0xF9F1,  0xFA74,  0xFAF7,  0xFB79,
-	0xFBFB,  0xFC7D,  0xFCFF,  0xFD80,  0xFE02,  0xFE82,  0xFF03,  0xFF83
-};
-
-fixed fsqrt(fixed n)
-{
-	uint32_t i;
-	fixed s = (n + 65536) >> 1;
-
-	for (i = 0; i < 8; i++)
-	    s = (s + fdiv(n, s)) >> 1;
-
-	return s;
-}
-#endif

src/flightctrl.c

@@ -0,0 +1,264 @@
+/***************************************************************************
+ *   sam7fc - Flight Control                                               *
+ *                                                                         *
+ *   Copyright (C) 03/2008 by Olaf Rempel                                  *
+ *   razzor@kopf-tisch.de                                                  *
+ *                                                                         *
+ *   This program is free software; you can redistribute it and/or modify  *
+ *   it under the terms of the GNU General Public License as published by  *
+ *   the Free Software Foundation; version 2 of the License                *
+ *                                                                         *
+ *   This program is distributed in the hope that it will be useful,       *
+ *   but WITHOUT ANY WARRANTY; without even the implied warranty of        *
+ *   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the         *
+ *   GNU General Public License for more details.                          *
+ *                                                                         *
+ *   You should have received a copy of the GNU General Public License     *
+ *   along with this program; if not, write to the                         *
+ *   Free Software Foundation, Inc.,                                       *
+ *   59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.             *
+ ***************************************************************************/
+#include <stdio.h>
+#include <stdlib.h>	/* abs() */
+
+#include "AT91SAM7S256.h"
+#include "board.h"
+
+#include <at91_adc.h>
+#include <at91_pitc.h>
+#include <at91_tc1.h>
+#include <at91_twi.h>
+#include <telemetrie.h>
+#include <pidctrl.h>
+
+static uint32_t global_state;
+#define MOTOR_RUNNING			0x0001
+#define MOTOR_MIXER_ENABLE		0x0002
+#define STICK_CALIBRATION		0x0004
+#define STICK_CALIBRATION_COMPLETE	0x0008
+
+static struct pid_data pid_nick = {
+	.kp = 1,
+	.ki = 0,
+	.err_sum_max = +1024,
+	.err_sum_min = -1024,
+	.out_max = +256,
+	.out_min = -256,
+};
+TDC_PTR(pid_nick_ki, &pid_nick.ki, "flctrl: pid nick ki", int32_t, TDC_SIGNED);
+TDC_PTR(pid_nick_errsum, &pid_nick.err_sum, "flctrl: pid nick errsum", int32_t, TDC_SIGNED | TDC_READONLY);
+
+static struct pid_data pid_roll = {
+	.kp = 1,
+	.ki = 0,
+	.err_sum_max = +1024,
+	.err_sum_min = -1024,
+	.out_max = +256,
+	.out_min = -256,
+};
+TDC_PTR(pid_roll_ki, &pid_roll.ki, "flctrl: pid roll ki", int32_t, TDC_SIGNED);
+TDC_PTR(pid_roll_errsum, &pid_roll.err_sum, "flctrl: pid roll errsum", int32_t, TDC_SIGNED | TDC_READONLY);
+
+static struct pid_data pid_gier = {
+	.kp = 1,
+	.ki = 0,
+	.err_sum_max = +1024,
+	.err_sum_min = -1024,
+	.out_max = +256,
+	.out_min = -256,
+};
+TDC_PTR(pid_gier_ki, &pid_gier.ki, "flctrl: pid gier ki", int32_t, TDC_SIGNED);
+TDC_PTR(pid_gier_errsum, &pid_gier.err_sum, "flctrl: pid gier errsum", int32_t, TDC_SIGNED | TDC_READONLY);
+
+static void motor_mixer(int32_t gas, int32_t nick, int32_t roll, int32_t gier)
+{
+	static uint8_t pwm[4];
+
+	TDC_VALUE(pwm0, pwm[0], "Motor PWM 1", uint8_t, TDC_UNSIGNED | TDC_READONLY);
+	TDC_VALUE(pwm1, pwm[1], "Motor PWM 2", uint8_t, TDC_UNSIGNED | TDC_READONLY);
+	TDC_VALUE(pwm2, pwm[2], "Motor PWM 3", uint8_t, TDC_UNSIGNED | TDC_READONLY);
+	TDC_VALUE(pwm3, pwm[3], "Motor PWM 4", uint8_t, TDC_UNSIGNED | TDC_READONLY);
+
+	pwm[0] = LIMIT((gas - nick + gier), 0x0F, 0xFF);
+	pwm[1] = LIMIT((gas + nick + gier), 0x0F, 0xFF);
+	pwm[2] = LIMIT((gas + roll - gier), 0x0F, 0xFF);
+	pwm[3] = LIMIT((gas - roll - gier), 0x0F, 0xFF);
+
+	if (!(global_state & MOTOR_RUNNING)) {
+		pwm[0] = 0x00;
+		pwm[1] = 0x00;
+		pwm[2] = 0x00;
+		pwm[3] = 0x00;
+
+	} else if (!(global_state & MOTOR_MIXER_ENABLE)) {
+		pwm[0] = 0x0F;
+		pwm[1] = 0x0F;
+		pwm[2] = 0x0F;
+		pwm[3] = 0x0F;
+	}
+
+	twi_setpwm(pwm);
+}
+
+static uint32_t base_ctrl_cb(struct pitc_timer *timer)
+{
+	if (global_state & STICK_CALIBRATION)
+		global_state |= STICK_CALIBRATION_COMPLETE;
+
+	return PITC_REMOVE_TIMER;
+}
+
+static struct pitc_timer base_ctrl_timer = {
+	.func = base_ctrl_cb,
+};
+
+void base_ctrl(void)
+{
+	static int32_t nick_integral, roll_integral, gier_integral;
+
+	/* get stick switches & values */
+	struct rc_values rc_sw;
+	uint32_t count = rcontrol_getswitches(&rc_sw);
+
+	if (count < 4) {
+		global_state &= ~MOTOR_RUNNING;
+
+	} else if (global_state & STICK_CALIBRATION_COMPLETE) {
+//		rcontrol_calibrate(RC_CAL_END);
+//		rcontrol_calibrate(RC_CAL_SAVE);
+		rcontrol_print_cal();
+		global_state &= ~(STICK_CALIBRATION | STICK_CALIBRATION_COMPLETE);
+
+	} else if (global_state & STICK_CALIBRATION) {
+		/* do nothing during calibration */
+
+	} else if (count >= 4) {
+		/* Motor stop */
+		if (rc_sw.chan[2] < 0 && rc_sw.chan[3] > 0)
+			global_state &= ~MOTOR_RUNNING;
+
+		/* Motor start */
+		if (rc_sw.chan[2] < 0 && rc_sw.chan[3] < 0)
+			global_state |= MOTOR_RUNNING;
+
+		/* Gyro calibration */
+		if (rc_sw.chan[2] > 0 && rc_sw.chan[3] > 0)
+			adc_calibrate(ADC_CAL_GYRO);
+
+		/* ACC + Stick calibration */
+		if (rc_sw.chan[2] > 0 && rc_sw.chan[3] < 0) {
+			adc_calibrate(ADC_CAL_ACC);
+//			rcontrol_calibrate(RC_CAL_START);
+			global_state |= STICK_CALIBRATION;
+			base_ctrl_timer.interval = 1000;
+			pitc_schedule_timer(&base_ctrl_timer);
+		}
+	}
+
+	struct rc_values rc;
+	rcontrol_getvalues(&rc);
+
+	if (rc.chan[2] < 15) {
+		global_state &= ~MOTOR_MIXER_ENABLE;
+
+		/* reset integrals */
+		nick_integral = roll_integral = gier_integral = 0;
+		pid_nick.err_sum = pid_roll.err_sum = pid_gier.err_sum = 0;
+
+	} else {
+		global_state |= MOTOR_MIXER_ENABLE;
+	}
+
+	/* get adc results */
+	static int16_t adc_result[7];
+
+	TDC_PTR(adc_result0, &adc_result[ADC_GYRO_NICK], "ADC_GYRO_NICK", int16_t, TDC_SIGNED | TDC_READONLY);
+	TDC_PTR(adc_result1, &adc_result[ADC_GYRO_ROLL], "ADC_GYRO_ROLL", int16_t, TDC_SIGNED | TDC_READONLY);
+	TDC_PTR(adc_result2, &adc_result[ADC_GYRO_GIER], "ADC_GYRO_GIER", int16_t, TDC_SIGNED | TDC_READONLY);
+	TDC_PTR(adc_result3, &adc_result[ADC_ACC_NICK], "ADC_ACC_NICK", int16_t, TDC_SIGNED | TDC_READONLY);
+	TDC_PTR(adc_result4, &adc_result[ADC_ACC_ROLL], "ADC_ACC_ROLL", int16_t, TDC_SIGNED | TDC_READONLY);
+	TDC_PTR(adc_result5, &adc_result[ADC_ACC_GIER], "ADC_ACC_GIER", int16_t, TDC_SIGNED | TDC_READONLY);
+	TDC_PTR(adc_result6, &adc_result[ADC_VOLTAGE], "ADC_VOLTAGE", int16_t, TDC_SIGNED | TDC_READONLY);
+
+	adc_get_results(adc_result);
+
+	if (count != 0 && adc_result[ADC_VOLTAGE] > 960)
+		*AT91C_PIOA_CODR = LED_GREEN;
+	else
+		*AT91C_PIOA_SODR = LED_GREEN;
+
+	nick_integral += adc_result[ADC_GYRO_NICK];
+	TDC_INT32(nick_integral, "flctrl: Base Integral Nick");
+
+	roll_integral += adc_result[ADC_GYRO_ROLL];
+	TDC_INT32(roll_integral, "flctrl: Base Integral Roll");
+
+	gier_integral += adc_result[ADC_GYRO_GIER];
+	TDC_INT32(gier_integral, "flctrl: Base Integral Gier");
+
+	static int32_t integral_gyro_mix = 1;
+	TDC_INT32(integral_gyro_mix, "flctrl: Mix Integral/ACC (0-1024)");
+
+	static int32_t acc_faktor = 300;
+	TDC_INT32(acc_faktor, "flctrl: Mix Faktor");
+
+	/*
+	 * 90° -> ADC_ACC_* ~210
+	 * 90° -> Integral ~60000
+	 */
+	int32_t mix_integral = 1024 - integral_gyro_mix;
+	int32_t mix_acc = integral_gyro_mix * acc_faktor;
+	nick_integral = ((nick_integral * mix_integral) + (adc_result[ADC_ACC_NICK] * mix_acc)) / 1024;
+	roll_integral = ((roll_integral * mix_integral) + (adc_result[ADC_ACC_ROLL] * mix_acc)) / 1024;
+
+	/*
+	 * hoher gyro_faktor -> bessere stabilität, aber langsames zurückkehren nach vollausschlag
+	 * niedriger integral_faktor -> geringere abweichungbei einseitigem anheben, aber schwingungsanfälliger
+	 */
+	static int32_t integral_faktor = 256;
+	static int32_t gyro_faktor = 4;
+	TDC_INT32(integral_faktor, "flctrl: Integral Divisior");
+	TDC_INT32(gyro_faktor, "flctrl: Integral Gyro Faktor /16");
+
+	static int32_t nick;
+	static int32_t roll;
+	static int32_t gier;
+	nick = (nick_integral / integral_faktor) + (adc_result[ADC_GYRO_NICK] * gyro_faktor) / 16;
+	roll = (roll_integral / integral_faktor) + (adc_result[ADC_GYRO_ROLL] * gyro_faktor) / 16;
+	gier = (gier_integral / integral_faktor) + (adc_result[ADC_GYRO_GIER] * gyro_faktor) / 16;
+	TDC_INT32_RO(nick, "flctrl: Integral + Gyro Nick");
+	TDC_INT32_RO(roll, "flctrl: Integral + Gyro Roll");
+	TDC_INT32_RO(gier, "flctrl: Integral + Gyro Gier");
+
+	static int32_t stick_kp = 8;
+	TDC_INT32(stick_kp, "flctrl: Stick-P /16");
+
+	static int32_t stick_gas, stick_nick, stick_roll, stick_gier;
+	stick_gas = rc.chan[2];
+	stick_nick = (rc.chan[0] * stick_kp) / 16;
+	stick_roll = (rc.chan[1] * stick_kp) / 16;
+	stick_gier = rc.chan[3] / 4;
+
+	TDC_INT32_RO(stick_gas, "Stick Gas");
+	TDC_INT32_RO(stick_nick, "Stick Nick");
+	TDC_INT32_RO(stick_roll, "Stick Roll");
+	TDC_INT32_RO(stick_gier, "Stick Gier");
+
+	static int32_t gier_kp = 32;
+	TDC_INT32(gier_kp, "flctrl: Gier-P (/256)");
+	gier_integral -= gier_kp * stick_gier * abs(stick_gier) / 256;
+
+	static int32_t mixer_gas, mixer_nick, mixer_roll, mixer_gier;
+	mixer_gas = stick_gas;
+	mixer_nick = pid_ctrl(&pid_nick, stick_nick - nick);
+	mixer_roll = pid_ctrl(&pid_roll, stick_roll - roll);
+	mixer_gier = pid_ctrl(&pid_gier, stick_gier - gier);
+
+	/* mix gas/nick/roll/gier -> 4 motors */
+	motor_mixer(mixer_gas, mixer_nick, mixer_roll, mixer_gier);
+
+	TDC_INT32_RO(mixer_gas, "Mixer Gas");
+	TDC_INT32_RO(mixer_nick, "Mixer Nick");
+	TDC_INT32_RO(mixer_roll, "Mixer Roll");
+	TDC_INT32_RO(mixer_gier, "Mixer Gier");
+}

src/memalloc.c

@@ -1,4 +1,6 @@
 /***************************************************************************
+ *   sam7fc - dynamic Memory Allocation                                    *
+ *                                                                         *
  *   Copyright (C) 02/2008 by Olaf Rempel                                  *
  *   razzor@kopf-tisch.de                                                  *
  *                                                                         *

src/pidctrl.c

@@ -19,36 +19,26 @@
 #include <stdint.h>
 
 #include "pidctrl.h"
+#include "board.h"
 
 int32_t pid_ctrl(struct pid_data *pid, int32_t error)
 {
 	int32_t out = 0;
 
-	if (pid->kp)
+	if (pid->kp != 0)
 		out += pid->kp * error;
 
-	if (pid->ki) {
+	if (pid->ki != 0) {
 		pid->err_sum += error;
+		pid->err_sum = LIMIT(pid->err_sum, pid->err_sum_min, pid->err_sum_max);
 
-		if (pid->err_sum > pid->err_sum_max)
+		out += (pid->err_sum / pid->ki);
-			pid->err_sum = pid->err_sum_max;
-
-		if (pid->err_sum < pid->err_sum_min)
-			pid->err_sum = pid->err_sum_min;
-
-		out += pid->ki * pid->ta * pid->err_sum;
 	}
 
-	if (pid->kd) {
+	if (pid->kd != 0) {
-		out += pid->kd * (error - pid->err_old) / pid->ta;
+		out += pid->kd * (error - pid->err_old);
 		pid->err_old = error;
 	}
 
-	if (out > pid->out_max)
+	return LIMIT(out, pid->out_min, pid->out_max);
-		out = pid->out_max;
-
-	if (out < pid->out_min)
-		out = pid->out_min;
-
-	return out;
 }

src/telemetrie.c

@@ -1,4 +1,6 @@
 /***************************************************************************
+ *   sam7fc - Telemetrie Handling                                          *
+ *                                                                         *
  *   Copyright (C) 02/2008 by Olaf Rempel                                  *
  *   razzor@kopf-tisch.de                                                  *
  *                                                                         *
@@ -22,9 +24,12 @@
 #include "board.h"		// ARRAY_SIZE()
 #include "at91_pitc.h"
 #include "telemetrie.h"
+#include "tdc_proto.h"
 #include "memalloc.h"
 #include "fifo.h"
 
+#define TDC_OWN_ADDRESS		TDC_ADDR1
+
 /* extern symbols, defined in ldscript */
 extern struct tdc_value _tdc_value_table;
 extern struct tdc_value _tdc_value_table_end;
@@ -46,7 +51,11 @@ int32_t tdc_transmit(uint32_t addr, struct tdc_pkt_header *head)
 	if (addr >= ARRAY_SIZE(routing_table) || !routing_table[addr])
 		return -1;
 
-	return fifo_put(routing_table[addr]->txfifo, (char *)head, head->size);
+	int32_t retval = fifo_put(routing_table[addr]->txfifo, (char *)head, head->size);
+	if (routing_table[addr]->trigger_tx)
+		routing_table[addr]->trigger_tx();
+
+	return retval;
 }
 
 static int32_t tdc_get_vars(void)
@@ -62,8 +71,9 @@ static int32_t tdc_get_vars(void)
 		struct tdc_getvars_reply *reply = alloc(sizeof(struct tdc_getvars_reply) + datalen);
 		reply->cmd = TDC_REPLY | TDC_ADDR1 | TDC_GETVARS;
 		reply->size = sizeof(struct tdc_getvars_reply) + datalen;
-		reply->id = id;
+		reply->id = (id & 0xFF);
 		reply->flags = value->flags;
+		reply->name_len = datalen;
 		memcpy(reply->name, value->name, datalen);
 
 		uint32_t ret = tdc_transmit(TDC_ADDR0, ((struct tdc_pkt_header *)reply));
@@ -92,7 +102,7 @@ static int32_t tdc_get_value(uint32_t id)
 
 	struct tdc_getvalue_reply *reply = alloc(sizeof(struct tdc_getvalue_reply) + datalen);
 	reply->cmd = TDC_REPLY | TDC_ADDR1 | TDC_GETVALUE;
-	reply->size = sizeof(struct tdc_getvars_reply) + datalen;
+	reply->size = sizeof(struct tdc_getvalue_reply) + datalen;
 	reply->id = id;
 	memcpy(reply->data, value->data, datalen);
 
@@ -102,36 +112,37 @@ static int32_t tdc_get_value(uint32_t id)
 	return ret;
 }
 
-static int32_t tdc_set_value(uint32_t id, uint8_t *data)
+static int32_t tdc_set_value(uint32_t id, uint8_t *data, uint32_t data_size)
 {
 	struct tdc_value *value = &_tdc_value_table + id;
 	if (value >= &_tdc_value_table_end)
 		return -1;
 
-	uint32_t datalen = value->flags & TDC_SIZEMASK;
+	uint32_t len = value->flags & TDC_SIZEMASK;
+
+	if (len != data_size)
+		return -1;
 
 	// TODO: atomic?
-	memcpy(value->data, data, datalen);
+	memcpy(value->data, data, len);
-
+	return len;
-	return 1;
 }
 
 static uint32_t tdc_timer_cb(struct pitc_timer *timer)
 {
 	uint32_t i, j;
 	for (i = 0; i < ARRAY_SIZE(tdc_varmap); i++) {
-		uint32_t tmp = tdc_varmap[i];
+		uint32_t bitmask = tdc_varmap[i];
 
 		for (j = 0; j < 32; j++) {
-			if (!tmp)
+			if (!bitmask)
 				break;
 
-			if (tmp & 0x01) {
+			if (bitmask & 0x01) {
-				if (tdc_get_value(i * 32 + j) <= 0)
+				if (tdc_get_value(i * 32 + j) < 0)
-					return PITC_REMOVE_TIMER;
+					tdc_varmap[i] &= ~(1 << j);
 			}
-
+			bitmask >>= 1;
-			tmp >>= 1;
 		}
 	}
 	return PITC_RESTART_TIMER;
@@ -145,111 +156,183 @@ static int32_t tdc_setup_timer(uint32_t interval, uint32_t *varmap)
 {
 	memcpy(tdc_varmap, varmap, sizeof(tdc_varmap));
 
-	if (interval > 0) {
+	uint32_t i;
+	uint32_t tmp = 0;
+	for (i = 0; i < ARRAY_SIZE(tdc_varmap); i++)
+		tmp |= tdc_varmap[i];
+
+	if ((interval > 0) && (tmp != 0)) {
 		tdc_timer.interval = interval;
-		// TODO: timer already running
 		pitc_schedule_timer(&tdc_timer);
 
 	} else {
-		// TODO: timer stop
+		pitc_remove_timer(&tdc_timer);
 	}
 	return 1;
 }
 
 static const struct tdc_hello_reply hello_reply = {
-	.cmd = TDC_REPLY | TDC_ADDR1 | TDC_HELLO,
+	.cmd = TDC_REPLY | TDC_OWN_ADDRESS | TDC_HELLO,
 	.size = sizeof(struct tdc_hello_reply),
 	.name = "sam7fc-v0.01",
 };
 
-/*
- * returns:
- * -1: on routing error
- *  0: no space left in txfifo (caller should retry)
- * >0: success
- */
-static int32_t tdc_parse_packet(struct tdc_pkt_header *head)
-{
-	/* all replys go to the HOST */
-	if (head->cmd & TDC_REPLY)
-		return tdc_transmit(TDC_ADDR0, head);
-
-	/* forward this packet? */
-	if ((head->cmd & TDC_ADDRMASK) != TDC_ADDR1) {
-		uint32_t addr = (head->cmd & TDC_ADDRMASK) >> 4;
-		return tdc_transmit(addr, head);
-	}
-
-	int32_t ret = -1;
-
-	/* parse the packet */
-	switch (head->cmd & TDC_OPCODEMASK) {
-	case TDC_HELLO:
-		/* answer the hello */
-		ret = tdc_transmit(TDC_ADDR0, (struct tdc_pkt_header *)&hello_reply);
-		break;
-
-	case TDC_GETVARS:
-		ret = tdc_get_vars();
-		break;
-
-	case TDC_GETVALUE: {
-		struct tdc_getvalue_request *pkt = (struct tdc_getvalue_request *)head;
-		ret = tdc_get_value(pkt->id);
-	}	break;
-
-	case TDC_SETVALUE: {
-		struct tdc_setvalue_request *pkt = (struct tdc_setvalue_request *)head;
-		ret = tdc_set_value(pkt->id, pkt->data);
-	}	break;
-
-	case TDC_REQVALUES: {
-		struct tdc_reqvalues_request *pkt = (struct tdc_reqvalues_request *)head;
-		ret = tdc_setup_timer(pkt->interval, pkt->varmap);
-	}	break;
-	};
-
-	/*
-	 * on succes(>0) return size of request,
-	 * and push retry(0) and routing error(-1) up
-	 */
-	return (ret > 0) ? head->size : ret;
-}
-
 void tdc_register_device(uint32_t addr, struct comm_device *device)
 {
 	if (addr < ARRAY_SIZE(routing_table))
 		routing_table[addr] = device;
 }
 
-void tdc_receive(struct comm_device *device)
+struct tdc_pkt_header * tdc_alloc_fullpkt(struct comm_device *device, uint32_t size)
 {
-	while (1) {
+	struct tdc_pkt_header *head = alloc(size);
-		/* peek the header */
-		struct tdc_pkt_header tmp_head;
-		uint32_t len = fifo_peek(device->rxfifo, (char *)&tmp_head, sizeof(tmp_head));
-		if (len != sizeof(tmp_head))
-			return;
 
 	/* peek the whole packet */
-		struct tdc_pkt_header *head = alloc(tmp_head.size);
+	uint32_t len = fifo_peek(device->rxfifo, (char *)head, size);
-		len = fifo_peek(device->rxfifo, (char *)head, tmp_head.size);
+	if (len != size) {
-		if (len != tmp_head.size)
+		free(head);
-			return;
+		head = NULL;
+	}
 
-		/* if it's a hello-request, remember the device as path to the host */
+	return head;
-		if ((head->cmd & (TDC_OPCODEMASK & TDC_DIR)) == TDC_HELLO)
+}
+
+static int32_t tdc_receive(struct comm_device *device)
+{
+	struct tdc_pkt_header tmp_head;
+	struct tdc_pkt_header *head = &tmp_head;
+
+	/* peek the header, return retry(0) if not enough bytes are available */
+	uint32_t len = fifo_peek(device->rxfifo, (char *)head, sizeof(tmp_head));
+	if (len != sizeof(tmp_head))
+		return 0;
+
+	/* assume an error, remove one byte from fifo */
+	uint32_t used_bytes = 1;
+	int32_t ret = -1;
+
+	/* remember the device as path to the host */
+	if ((head->cmd & (TDC_REPLY | TDC_OPCODEMASK)) == TDC_HELLO) {
 		tdc_register_device(TDC_ADDR0, device);
+	}
 
-		/* parse packet, remove data if no restart is needed */
+	/* reply packets / forward packets */
-		int32_t ret = tdc_parse_packet(head);
+	if (head->cmd & TDC_REPLY || (head->cmd & TDC_ADDRMASK) != TDC_OWN_ADDRESS) {
+		/* peek complete packet, return retry(0) if not enough bytes are available */
+		head = tdc_alloc_fullpkt(device, head->size);
+		if (head == NULL)
+			return 0;
+
+		/* reply packets go to ADDR0, forwards to others */
+		uint32_t addr = (head->cmd & TDC_REPLY) ? TDC_ADDR0 : ((head->cmd & TDC_ADDRMASK) >> 4);
+
+		used_bytes = head->size;
+		ret = tdc_transmit(addr, head);
+
+	} else {
+		/* parse cmd */
+		switch (head->cmd & TDC_OPCODEMASK) {
+		case TDC_HELLO: {
+			/* check packet size */
+			struct tdc_pkt_header *pkt = (struct tdc_pkt_header *)head;
+			if (pkt->size != sizeof(*pkt))
+				break;
+
+			/* send reply */
+			ret = tdc_transmit(TDC_ADDR0, (struct tdc_pkt_header *)&hello_reply);
+			used_bytes = pkt->size;
+		}	break;
+
+		case TDC_GETVARS: {
+			struct tdc_pkt_header *pkt = (struct tdc_pkt_header *)head;
+			if (pkt->size != sizeof(*pkt))
+				break;
+
+			/* send reply */
+			ret = tdc_get_vars();
+			used_bytes = pkt->size;
+		}	break;
+
+		case TDC_GETVALUE: {
+			struct tdc_getvalue_request *pkt = (struct tdc_getvalue_request *)head;
+			if (pkt->size != sizeof(*pkt))
+				break;
+
+			/* peek complete packet, return retry(0) if not enough bytes are available */
+			head = tdc_alloc_fullpkt(device, head->size);
+			if (head != NULL) {
+				pkt = (struct tdc_getvalue_request *)head;
+				ret = tdc_get_value(pkt->id);
+				used_bytes = pkt->size;
+
+			} else {
+				ret = 0;
+			}
+		}	break;
+
+		case TDC_SETVALUE: {
+			struct tdc_setvalue_request *pkt = (struct tdc_setvalue_request *)head;
+			if (pkt->size < sizeof(*pkt) +1 || pkt->size > sizeof(*pkt) +8)
+				break;
+
+			/* peek complete packet, return retry(0) if not enough bytes are available */
+			head = tdc_alloc_fullpkt(device, head->size);
+			if (head != NULL) {
+				pkt = (struct tdc_setvalue_request *)head;
+				ret = tdc_set_value(pkt->id, pkt->data, pkt->size - sizeof(*pkt));
+				used_bytes = pkt->size;
+
+			} else {
+				ret = 0;
+			}
+		}	break;
+
+		case TDC_REQVALUES: {
+			struct tdc_reqvalues_request *pkt = (struct tdc_reqvalues_request *)head;
+			if (pkt->size != sizeof(*pkt))
+				break;
+
+			/* peek complete packet, return retry(0) if not enough bytes are available */
+			head = tdc_alloc_fullpkt(device, head->size);
+			if (head != NULL) {
+				pkt = (struct tdc_reqvalues_request *)head;
+				ret = tdc_setup_timer(pkt->interval, pkt->varmap);
+				used_bytes = pkt->size;
+
+			} else {
+				ret = 0;
+			}
+		}	break;
+
+		}
+	}
+
+	/* on success(>0) or routing error(-1) remove the packet */
+	if (ret != 0) {
+		/* remove bytes from fifo */
+		fifo_remove(device->rxfifo, used_bytes);
+	}
+
+	/* free allocated memory */
+	if (head != NULL && head != &tmp_head)
 		free(head);
 
-		/* some tx-fifo was full(0), return to caller and let it retry */
+	return ret;
-		if (!ret)
+}
-			return;
 
-		/* success(>0) or routing error(-1) -> remove the packet */
+void tdc_check(void)
-		fifo_remove(device->rxfifo, tmp_head.size);
+{
+	uint32_t i;
+	for (i = 0; i < ARRAY_SIZE(routing_table); i++) {
+		if (routing_table[i] != NULL) {
+			tdc_receive(routing_table[i]);
+			// TODO: handle retry
+		}
 	}
 }
+
+void tdc_init(void)
+{
+	uint32_t count = &_tdc_value_table_end - &_tdc_value_table;
+	printf("found %ld TDC variables\n\r", count);
+}