diff --git a/at91_init0.s b/at91_init0.s index 7bd3a5e..60e5dae 100644 --- a/at91_init0.s +++ b/at91_init0.s @@ -1,4 +1,6 @@ /*************************************************************************** + * sam7fc - stack/bss/data init * + * * * Copyright (C) 01/2008 by Olaf Rempel * * razzor@kopf-tisch.de * * * diff --git a/at91_init1.c b/at91_init1.c index cf07a06..49780c8 100644 --- a/at91_init1.c +++ b/at91_init1.c @@ -1,4 +1,6 @@ /*************************************************************************** + * sam7fc - PLL Init, IRQ/FIQ Vectors * + * * * Copyright (C) 01/2008 by Olaf Rempel * * razzor@kopf-tisch.de * * * diff --git a/eagle/SAM7_FC.brd b/eagle/SAM7_FC.brd index 973a8a1..cd2a219 100644 Binary files a/eagle/SAM7_FC.brd and b/eagle/SAM7_FC.brd differ diff --git a/eagle/SAM7_FC.sch b/eagle/SAM7_FC.sch index a044597..a8f7c69 100644 Binary files a/eagle/SAM7_FC.sch and b/eagle/SAM7_FC.sch differ diff --git a/eagle/sam7fc-bot.png b/eagle/sam7fc-bot.png new file mode 100644 index 0000000..d7de1ba Binary files /dev/null and b/eagle/sam7fc-bot.png differ diff --git a/eagle/sam7fc-sch.png b/eagle/sam7fc-sch.png new file mode 100644 index 0000000..3fca36a Binary files /dev/null and b/eagle/sam7fc-sch.png differ diff --git a/eagle/sam7fc-top.png b/eagle/sam7fc-top.png new file mode 100644 index 0000000..86e0f5b Binary files /dev/null and b/eagle/sam7fc-top.png differ diff --git a/include/at91_adc.h b/include/at91_adc.h index ba463ae..b992d0a 100644 --- a/include/at91_adc.h +++ b/include/at91_adc.h @@ -3,21 +3,30 @@ #include -#define ADC_GYRO_ROLL 0 -#define ADC_GYRO_NICK 1 -#define ADC_GYRO_YAW 2 -#define ADC_ACC_ROLL 3 -#define ADC_ACC_NICK 4 -#define ADC_ACC_YAW 5 +#define ADC_GYRO_NICK 0 +#define ADC_GYRO_ROLL 1 +#define ADC_GYRO_GIER 2 + +#define ADC_ACC_NICK 3 +#define ADC_ACC_ROLL 4 +#define ADC_ACC_GIER 5 + #define ADC_VOLTAGE 6 -#define ADC_CAL_NONE 0 -#define ADC_CAL_GYRO 1 -#define ADC_CAL_ACC 2 +// TODO: not all flags are public +#define ADC_COMPLETE 0x0001 +#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_ */ diff --git a/include/at91_dbgu.h b/include/at91_dbgu.h index 985f366..4486b3e 100644 --- a/include/at91_dbgu.h +++ b/include/at91_dbgu.h @@ -1,12 +1,10 @@ #ifndef AT91_DBGU_H_ #define AT91_DBGU_H_ -#include - 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_ */ diff --git a/include/at91_pitc.h b/include/at91_pitc.h index 286a722..f7e72ad 100644 --- a/include/at91_pitc.h +++ b/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); diff --git a/include/board.h b/include/board.h index 340bc57..f00c898 100644 --- a/include/board.h +++ b/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_*/ diff --git a/include/fixed.h b/include/fixed.h deleted file mode 100644 index 1202c3b..0000000 --- a/include/fixed.h +++ /dev/null @@ -1,34 +0,0 @@ -#ifndef _FIXED_H_ -#define _FIXED_H_ - -#include - -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_ */ diff --git a/include/pidctrl.h b/include/pidctrl.h index c1cbec6..936180d 100644 --- a/include/pidctrl.h +++ b/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; diff --git a/include/tdc_proto.h b/include/tdc_proto.h new file mode 100644 index 0000000..0e18be6 --- /dev/null +++ b/include/tdc_proto.h @@ -0,0 +1,103 @@ +#ifndef TDCPROTO_H_ +#define TDCPROTO_H_ + +#include + +/* + * 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_ */ diff --git a/include/telemetrie.h b/include/telemetrie.h index 6e233c1..79136b8 100644 --- a/include/telemetrie.h +++ b/include/telemetrie.h @@ -1,108 +1,19 @@ #ifndef TELEMETRIE_H_ #define TELEMETRIE_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)); - - +#include "tdc_proto.h" 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 -#define TDC_UNSIGNED 0x00 -#define TDC_SIGNED 0x10 -#define TDC_FP 0x20 +#if 1 -#define TDC_VALUE(var, desc, type, flags) \ - type * tdc_check_##var(void) { return (&var); } \ +#define TDC_VALUE(name, var, desc, type, flags) \ + 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_##var = tdc_value_##var; + tdc_value_##name = { &var, desc, sizeof(type) | flags }; \ + tdc_value_##name = tdc_value_##name; -#define TDC_UINT8(var, desc) TDC_VALUE(var, desc, uint8_t, TDC_UNSIGNED) -#define TDC_UINT16(var, desc) TDC_VALUE(var, desc, uint16_t, TDC_UNSIGNED) -#define TDC_UINT32(var, desc) TDC_VALUE(var, desc, uint32_t, TDC_UNSIGNED) -#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) -#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_INT64(var, desc) TDC_VALUE(var, desc, int64_t, TDC_SIGNED) -#define TDC_FLOAT(var, desc) TDC_VALUE(var, desc, float, TDC_FP) -#define TDC_DOUBLE(var, desc) TDC_VALUE(var, desc, double, TDC_FP) +#define TDC_PTR(name, ptr, desc, type, flags) \ + static struct tdc_value __attribute__((used, section(".tdc_value"))) \ + tdc_value_##name = { ptr, desc, sizeof(type) | flags }; \ + +#else + +#define TDC_VALUE(name, var, desc, type, flags) +#define TDC_PTR(name, ptr, desc, type, flags); + +#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_*/ diff --git a/main.c b/main.c index 1d0b1e3..34ac970 100644 --- a/main.c +++ b/main.c @@ -1,158 +1,106 @@ -/*************************************************************************** - * Copyright (C) 01/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 "AT91SAM7S256.h" -#include "at91_sysc.h" -#include "at91_dbgu.h" -#include "at91_pitc.h" -#include "at91_adc.h" -#include "at91_tests.h" -#include "at91_udp.h" -#include "at91_pio.h" -#include "at91_twi.h" - -#include "at91_tc1.h" -#include "memalloc.h" - -#include "board.h" -#include -#include - -static uint32_t global_state; - -#define MOTORS_RUNNING 0x0001 -#define STICK_CALIBRATION 0x0002 - -static uint32_t stick_timer_cb(struct pitc_timer *timer) -{ - static uint32_t i; - *AT91C_PIOA_SODR = i; - i = i ^ LED_GREEN; - *AT91C_PIOA_CODR = i; - - struct rc_values rc; - 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 = { - .interval = 10, - .func = &stick_timer_cb, -}; - -int main(void) -{ - /* LED outputs */ - *AT91C_PIOA_PER = LED_GREEN | LED_ORANGE; - *AT91C_PIOA_OER = LED_GREEN | LED_ORANGE; - - /* needed for dbgu */ - at91_sysc_init(); - - at91_dbgu_init(); - at91_dbgu_puts("==========================================================\n\rGood morning Dave\n\r"); - - /* triggers pinchange-isrs */ - at91_pio_init(); - - /* timer */ - at91_pitc_init(); - at91_rttc_test_init(); - - /* twi */ - at91_twi_init(); - at91_twi_test(); - - /* remote control, needs twi */ - at91_tc1_init(); - - /* usb */ - at91_udp_init(); - - /* adc, need timer, twi */ - at91_adc_init(); - - pitc_schedule_timer(&stick_timer); - - printf("static alloc: %5ld bytes\n\r", static_alloc_used()); - - while (1); -} +/*************************************************************************** + * sam7fc - main loop * + * * + * Copyright (C) 01/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 +#include + +#include "AT91SAM7S256.h" +#include "board.h" + +#include "at91_sysc.h" +#include "at91_dbgu.h" +#include "at91_pitc.h" +#include "at91_adc.h" +#include "at91_tests.h" +#include "at91_udp.h" +#include "at91_pio.h" +#include "at91_twi.h" + +#include "at91_tc1.h" +#include "memalloc.h" +#include "telemetrie.h" + +extern void base_ctrl(void); + +volatile static uint32_t run_ctrl; + +static uint32_t base_ctrl_trigger(struct pitc_timer *timer) +{ + /* trigger adc, after ~15us () result should be ready */ + adc_trigger(); + + run_ctrl = 1; + + return PITC_RESTART_TIMER; +} + +static struct pitc_timer base_timer = { + .interval = 2, + .func = &base_ctrl_trigger, +}; + +int main(void) +{ + /* LED outputs */ + *AT91C_PIOA_PER = LED_GREEN | LED_ORANGE; + *AT91C_PIOA_OER = LED_GREEN | LED_ORANGE; + + /* needed for dbgu */ + at91_sysc_init(); + + at91_dbgu_init(); + at91_dbgu_puts("==========================================================\n\rGood morning Dave\n\r"); + + /* triggers pinchange-isrs */ + at91_pio_init(); + + /* timer */ + at91_pitc_init(); + at91_rttc_test_init(); + + /* twi */ + at91_twi_init(); + at91_twi_test(); + + /* remote control, needs twi */ + at91_tc1_init(); + + /* usb */ + at91_udp_init(); + + /* adc, need timer, twi */ + at91_adc_init(); + + pitc_schedule_timer(&base_timer); + + tdc_init(); + + printf("static alloc: %5ld bytes\n\r", static_alloc_used()); + + while (1) { + if (run_ctrl) { + // TODO: racy? + run_ctrl = 0; + + base_ctrl(); + } + tdc_check(); + } +} diff --git a/src/at91_adc.c b/src/at91_adc.c index d12f8ab..133f812 100644 --- a/src/at91_adc.c +++ b/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 #include - #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; - status = status; + uint32_t dummy = *AT91C_ADC_SR; + dummy = dummy; - uint32_t i; - for (i = 0; i < ARRAY_SIZE(adc_offset); i++) - adc_result[i] = adc_tmp[i] - adc_offset[i]; + if (!(adc_status & (ADC_CAL_GYRO | ADC_CAL_ACC))) + return; - /* (adc / 1024) * 3.3V * (11k / 1k) * 100 */ - adc_result[ADC_VOLTAGE] = ((uint32_t)adc_tmp[ADC_VOLTAGE] * 3630) / 1024; + if (adc_status & ADC_CAL_GYRO) { + 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) { - /* trigger next cycle */ - *AT91C_ADC_CR = AT91C_ADC_START; - - 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]; + if (adc_cal_count < ADC_CAL_COUNT_MAX) { + /* trigger next cycle */ + *AT91C_ADC_CR = AT91C_ADC_START; + return PITC_RESTART_TIMER; } else { - adc_cal_data[0] += adc_tmp[ADC_ACC_ROLL]; - adc_cal_data[1] += adc_tmp[ADC_ACC_NICK]; - adc_cal_data[2] += adc_tmp[ADC_ACC_YAW]; - } + if (adc_status & ADC_CAL_GYRO) { + adc_offset[ADC_GYRO_NICK] = adc_cal_data[0] / ADC_CAL_COUNT_MAX; + 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--; - 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]); + adc_calibrate(ADC_CAL_GYRO_COMPLETE); } else { - adc_offset[ADC_ACC_ROLL] = adc_cal_data[0]; - adc_offset[ADC_ACC_NICK] = adc_cal_data[1]; + adc_offset[ADC_ACC_NICK] = adc_cal_data[0] / ADC_CAL_COUNT_MAX; + 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_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_calibrate(ADC_CAL_ACC_COMPLETE); } - - 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", - adc_result[ADC_GYRO_ROLL], adc_result[ADC_ACC_ROLL], - adc_result[ADC_GYRO_NICK], adc_result[ADC_ACC_NICK], - adc_result[ADC_GYRO_YAW], adc_result[ADC_ACC_YAW], - adc_result[ADC_VOLTAGE]); + // TODO: err in retvalue? + if (!(adc_status & (ADC_CAL_GYRO | ADC_CAL_ACC))) + *AT91C_ADC_CR = AT91C_ADC_START; +} - *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) - return; + /* disable interrupt */ + *AT91C_ADC_IDR = AT91C_ADC_ENDRX; - if (mode == ADC_CAL_GYRO || mode == ADC_CAL_ACC) { - adc_cal_count = ADC_CAL_COUNT; - adc_cal_mode = mode; + switch (mode) { + case ADC_CAL_GYRO_COMPLETE: + 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<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; } diff --git a/src/at91_exceptions.c b/src/at91_exceptions.c index cd87668..550d746 100644 --- a/src/at91_exceptions.c +++ b/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? */ - /* TODO: thumb mode enabled? */ - "msr CPSR_c, r0 \n\t" + "orr r1, r0, #I_BIT | F_BIT \n\t" + "msr CPSR_c, r1 \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" diff --git a/src/at91_pio.c b/src/at91_pio.c index 6a990d4..93c700b 100644 --- a/src/at91_pio.c +++ b/src/at91_pio.c @@ -1,4 +1,6 @@ /*************************************************************************** + * sam7fc - Pinchange Interrupt Handler * + * * * Copyright (C) 01/2008 by Olaf Rempel * * razzor@kopf-tisch.de * * * @@ -55,7 +57,7 @@ void at91_pio_init(void) /* enable pinchange interrupts */ struct pio_pinchange_isr *isr; for (isr = &_pio_isr_table; isr < &_pio_isr_table_end; isr++) - *AT91C_PIOA_IER = isr->mask; + *AT91C_PIOA_IER = isr->mask; /* dummy read to clear interrupts */ uint32_t dummy = *AT91C_PIOA_ISR; diff --git a/src/at91_pitc.c b/src/at91_pitc.c index bfd7b9e..82b77be 100644 --- a/src/at91_pitc.c +++ b/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; } diff --git a/src/at91_rttc_test.c b/src/at91_rttc_test.c index 6b58388..37f4167 100644 --- a/src/at91_rttc_test.c +++ b/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 #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) diff --git a/src/at91_sysc.c b/src/at91_sysc.c index 2789d6f..a88730f 100644 --- a/src/at91_sysc.c +++ b/src/at91_sysc.c @@ -1,4 +1,6 @@ /*************************************************************************** + * sam7fc - System Interrupt Dispatcher * + * * * Copyright (C) 01/2008 by Olaf Rempel * * razzor@kopf-tisch.de * * * @@ -31,15 +33,15 @@ struct _irq_regs { /* same IDs as sysc_isrs enum! */ static const struct _irq_regs irq_regs[] = { - /* + /* * PIT (Periodic Intervall Timer) * ISR must read AT91C_PITC_PIVR to clear interrupt * - PITIEN (Periodic Intervall Timer Interrupt Enable) - */ + */ { AT91C_PITC_PIMR, AT91C_PITC_PITIEN, AT91C_PITC_PISR, AT91C_PITC_PITS }, - + /* * DBGU (Debug Unit) * TODO: must use AT91C_DBGU_IDR to fault disable interrupt @@ -58,12 +60,12 @@ static const struct _irq_regs irq_regs[] = { AT91C_US_TXBUFE | AT91C_US_RXBUFF | AT91C_US_COMM_TX | AT91C_US_COMM_RX) }, - + /* * EFC (Embedded Flash Controller / Memory Controller) */ { AT91C_MC_FMR, (AT91C_MC_FRDY | AT91C_MC_LOCKE | AT91C_MC_PROGE), - AT91C_MC_FSR, (AT91C_MC_FRDY | AT91C_MC_LOCKE | AT91C_MC_PROGE) + AT91C_MC_FSR, (AT91C_MC_FRDY | AT91C_MC_LOCKE | AT91C_MC_PROGE) }, /* @@ -84,7 +86,7 @@ static const struct _irq_regs irq_regs[] = { { AT91C_RTTC_RTMR, (AT91C_RTTC_ALMIEN | AT91C_RTTC_RTTINCIEN), AT91C_RTTC_RTSR, (AT91C_RTTC_ALMS | AT91C_RTTC_RTTINC) }, - + /* * RSTC (Reset Controller) * - AT91C_RSTC_URSTIEN (User Reset Interrupt Enable) @@ -104,27 +106,27 @@ static const struct _irq_regs irq_regs[] = { AT91C_PMC_PCK1RDY | AT91C_PMC_PCK2RDY), AT91C_PMC_SR, (AT91C_PMC_MOSCS | AT91C_PMC_LOCK | AT91C_PMC_MCKRDY | AT91C_PMC_PCK0RDY | - AT91C_PMC_PCK1RDY | AT91C_PMC_PCK2RDY) + AT91C_PMC_PCK1RDY | AT91C_PMC_PCK2RDY) }, -}; +}; /* check Interrupts from internal hardware */ static void at91_sysc_isr(void) { uint32_t id; for (id = 0; id < AT91_SYSIRQ_COUNT; id++) { - + /* check if interrups are enabled */ if (*(irq_regs[id].mask_register) & irq_regs[id].mask) { /* check if interrupts are active */ uint32_t status = *(irq_regs[id].status_register); if (status & irq_regs[id].status) { - + /* execute handler */ if (sysc_isrs[id]) { sysc_isrs[id](status); - + } else { /* disable interrupts */ *(irq_regs[id].mask_register) &= ~irq_regs[id].mask; @@ -138,9 +140,9 @@ void at91_sysc_init(void) { /* high priority, level triggered, own vector */ AT91S_AIC *aic = AT91C_BASE_AIC; - aic->AIC_SMR[AT91C_ID_SYS] = IRQPRIO_SYSC | + aic->AIC_SMR[AT91C_ID_SYS] = IRQPRIO_SYSC | AT91C_AIC_SRCTYPE_INT_HIGH_LEVEL; - + aic->AIC_SVR[AT91C_ID_SYS] = (uint32_t)at91_sysc_isr; aic->AIC_IECR = (1<= AT91_SYSIRQ_COUNT) return; - + sysc_isrs[irq] = isr; } diff --git a/src/at91_tc1.c b/src/at91_tc1.c index ad22fdc..37e8fd1 100644 --- a/src/at91_tc1.c +++ b/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; diff --git a/src/at91_twi.c b/src/at91_twi.c index 7358848..3dc15e3 100644 --- a/src/at91_twi.c +++ b/src/at91_twi.c @@ -1,4 +1,6 @@ /*************************************************************************** + * sam7fc - TWI/I2C Handling * + * * * Copyright (C) 02/2008 by Olaf Rempel * * razzor@kopf-tisch.de * * * diff --git a/src/at91_udp.c b/src/at91_udp.c index f0d0699..ec6cf0c 100644 --- a/src/at91_udp.c +++ b/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); } diff --git a/src/fifo.c b/src/fifo.c index 7a3611b..8fb3c3d 100644 --- a/src/fifo.c +++ b/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) { diff --git a/src/fixed.c b/src/fixed.c deleted file mode 100644 index 78fa449..0000000 --- a/src/fixed.c +++ /dev/null @@ -1,389 +0,0 @@ -#include - -#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 diff --git a/src/flightctrl.c b/src/flightctrl.c new file mode 100644 index 0000000..8ea24e6 --- /dev/null +++ b/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 +#include /* abs() */ + +#include "AT91SAM7S256.h" +#include "board.h" + +#include +#include +#include +#include +#include +#include + +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"); +} diff --git a/src/memalloc.c b/src/memalloc.c index 4d6afde..af60ff4 100644 --- a/src/memalloc.c +++ b/src/memalloc.c @@ -1,4 +1,6 @@ /*************************************************************************** + * sam7fc - dynamic Memory Allocation * + * * * Copyright (C) 02/2008 by Olaf Rempel * * razzor@kopf-tisch.de * * * diff --git a/src/pidctrl.c b/src/pidctrl.c index 0e40888..0419a65 100644 --- a/src/pidctrl.c +++ b/src/pidctrl.c @@ -19,36 +19,26 @@ #include #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) - 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; + out += (pid->err_sum / pid->ki); } - if (pid->kd) { - out += pid->kd * (error - pid->err_old) / pid->ta; + if (pid->kd != 0) { + out += pid->kd * (error - pid->err_old); pid->err_old = error; } - if (out > pid->out_max) - out = pid->out_max; - - if (out < pid->out_min) - out = pid->out_min; - - return out; + return LIMIT(out, pid->out_min, pid->out_max); } diff --git a/src/telemetrie.c b/src/telemetrie.c index 4df776c..5876d3c 100644 --- a/src/telemetrie.c +++ b/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); - - return 1; + memcpy(value->data, data, len); + return len; } 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 (tdc_get_value(i * 32 + j) <= 0) - return PITC_REMOVE_TIMER; + if (bitmask & 0x01) { + if (tdc_get_value(i * 32 + j) < 0) + tdc_varmap[i] &= ~(1 << j); } - - tmp >>= 1; + bitmask >>= 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) { - /* 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; + struct tdc_pkt_header *head = alloc(size); - /* peek the whole packet */ - struct tdc_pkt_header *head = alloc(tmp_head.size); - len = fifo_peek(device->rxfifo, (char *)head, tmp_head.size); - if (len != tmp_head.size) - return; + /* peek the whole packet */ + uint32_t len = fifo_peek(device->rxfifo, (char *)head, size); + if (len != size) { + free(head); + head = NULL; + } - /* if it's a hello-request, remember the device as path to the host */ - if ((head->cmd & (TDC_OPCODEMASK & TDC_DIR)) == TDC_HELLO) - tdc_register_device(TDC_ADDR0, device); + return head; +} - /* parse packet, remove data if no restart is needed */ - int32_t ret = tdc_parse_packet(head); +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); + } + + /* reply packets / forward packets */ + 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 */ - if (!ret) - return; + return ret; +} - /* success(>0) or routing error(-1) -> remove the packet */ - fifo_remove(device->rxfifo, tmp_head.size); +void tdc_check(void) +{ + 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); +}