razzor/sam7fc
1
0
Fork 0
Code Issues Pull Requests Releases Activity

working version

Browse Source
This commit is contained in:
Olaf Rempel 2008-06-23 15:33:07 +02:00
parent a8b04b8c9a
commit 8583ff4f02
32 changed files with 979 additions and 957 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
at91_init0.s
View File

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

2
at91_init1.c
View File

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

BIN
eagle/SAM7_FC.brd
View File

Binary file not shown.

BIN
eagle/SAM7_FC.sch
View File

Binary file not shown.

BIN
eagle/sam7fc-bot.png Normal file
View File

Binary file not shown.
Side by Side

After

Width:  |  Height:  |  Size: 41 KiB

BIN
eagle/sam7fc-sch.png Normal file
View File

Binary file not shown.
Side by Side

After

Width:  |  Height:  |  Size: 203 KiB

BIN
eagle/sam7fc-top.png Normal file
View File

Binary file not shown.
Side by Side

After

Width:  |  Height:  |  Size: 36 KiB

29
include/at91_adc.h
View File

@ -3,21 +3,30 @@
#include <stdint.h>
#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_ */

4
include/at91_dbgu.h
View File

@ -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_ */

1
include/at91_pitc.h
View File

@ -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);

2
include/board.h
View File

@ -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_*/

34
include/fixed.h
View File

@ -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_ */

2
include/pidctrl.h
View File

@ -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;

103
include/tdc_proto.h Normal file
View File

@ -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_ */

152
include/telemetrie.h
View File

@ -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_*/

110
main.c
View File

@ -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"
#include <stdio.h>
#include <string.h>
extern void base_ctrl(void);
static uint32_t global_state;
volatile static uint32_t run_ctrl;
#define MOTORS_RUNNING 0x0001
#define STICK_CALIBRATION 0x0002
static uint32_t stick_timer_cb(struct pitc_timer *timer)
static uint32_t base_ctrl_trigger(struct pitc_timer *timer)
{
static uint32_t i;
*AT91C_PIOA_SODR = i;
i = i ^ LED_GREEN;
*AT91C_PIOA_CODR = i;
/* trigger adc, after ~15us () result should be ready */
adc_trigger();
struct rc_values rc;
uint32_t count = rcontrol_getswitches(&rc);
run_ctrl = 1;
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,
static struct pitc_timer base_timer = {
.interval = 2,
.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();
}
}

188
src/at91_adc.c
View File

@ -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;
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)
{
if (adc_cal_count < ADC_CAL_COUNT_MAX) {
/* 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];
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;
}
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<<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]);
}

28
src/at91_dbgu.c
View File

@ -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;
}

9
src/at91_exceptions.c
View File

@ -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"

2
src/at91_pio.c
View File

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

35
src/at91_pitc.c
View File

@ -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;
}

25
src/at91_rttc_test.c
View File

@ -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)

2
src/at91_sysc.c
View File

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

20
src/at91_tc1.c
View File

@ -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;

2
src/at91_twi.c
View File

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

38
src/at91_udp.c
View File

@ -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);
}

4
src/fifo.c
View File

@ -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)
{

389
src/fixed.c
View File

@ -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

264
src/flightctrl.c Normal file
View File

@ -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");
}

2
src/memalloc.c
View File

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

26
src/pidctrl.c
View File

@ -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)
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);
}

269
src/telemetrie.c
View File

@ -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;
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)
return 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);
}
/* parse packet, remove data if no restart is needed */
int32_t ret = tdc_parse_packet(head);
/* 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);
}