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

Compare commits

base: razzor:master
Branches Tags
razzor:master
razzor:rtos
razzor:bootloader
..
compare: razzor:rtos
Branches Tags
razzor:master
razzor:rtos
razzor:bootloader

8 Commits
master ... rtos

Author SHA1 Message Date
Olaf Rempel d4a0911db9 new toolchain 2009-06-21 12:52:51 +02:00
Olaf Rempel d50c024cbd test 2008-06-23 15:15:56 +02:00
Olaf Rempel e6155fb7ad move exception handlers in C files 2008-06-23 15:15:56 +02:00
Olaf Rempel 021be80589 working semaphore & threading 2008-06-23 15:15:56 +02:00
Olaf Rempel 5a37574a47 repair makefile 2008-06-23 15:15:56 +02:00
Olaf Rempel 9e8158277c context, spinlock and semaphore functions 2008-06-23 15:15:56 +02:00
Olaf Rempel 991b088b0d working store & restore 2008-06-23 15:15:56 +02:00
Olaf Rempel 011d784923 working init-ctx version 2008-06-23 15:15:56 +02:00
41 changed files with 873 additions and 2819 deletions
Show Stats Expand all files Collapse all files

3
Makefile
View File

@ -73,7 +73,6 @@ openocd:
$(shell $(OPENOCD) -f scripts/openocd.cfg)
install: all
# ./scripts/download.sh
$(TOOLCHAIN)/dfu-util -i 0 -D $(BUILD)/$(TARGET).elf.bin
./scripts/download.sh
-include $(shell find $(BUILD) -name *.d 2> /dev/null)

46
at91_init0.s
View File

@ -1,6 +1,4 @@
/***************************************************************************
* sam7fc - stack/bss/data init *
* *
* Copyright (C) 01/2008 by Olaf Rempel *
* razzor@kopf-tisch.de *
* *
@ -18,6 +16,9 @@
* Free Software Foundation, Inc., *
* 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA. *
***************************************************************************/
.equ AIC_IVR, (256)
.equ AIC_FVR, (260)
.equ AIC_EOICR, (304)
.equ AT91C_BASE_AIC, (0xFFFFF000)
.equ IRQ_Stack_Size, (3 * 8 * 4)
@ -28,6 +29,7 @@
.equ ARM_MODE_IRQ, 0x12
.equ ARM_MODE_SVC, 0x13
.equ ARM_MODE_ABT, 0x17
.equ ARM_MODE_SYS, 0x1F
.equ I_BIT, 0x80
.equ F_BIT, 0x40
@ -45,14 +47,14 @@ rsvdvec: ldr pc, [pc, #24] /* 0x14 reserved */
irqvec: ldr pc, [pc, #24] /* 0x18 IRQ */
fiqvec: ldr pc, [pc, #24] /* 0x1c FIQ */
.extern ABT_Handler
.extern IRQ_Handler
.extern FIQ_Handler
.extern SWI_Handler
.extern ABT_Handler
.extern IRQ_Handler
/* 0x80000000 will result in Prefetch Abort */
.word InitReset
.word 0x80000000
.word 0x80000000
.word SWI_Handler
.word ABT_Handler
.word ABT_Handler
.word 0x80000000
@ -77,7 +79,7 @@ InitReset:
mov sp, r0
sub r0, r0, #FIQ_Stack_Size
/* store AIC Base in r8_fiq for faster access */
/* store AIC Base in ARM_MODE_FIQ:r8 for faster access */
ldr r8, =AT91C_BASE_AIC
/* Setup IRQ Mode Stack */
@ -91,7 +93,7 @@ InitReset:
sub r0, r0, #ABT_Stack_Size
/* Setup Supervisor Mode Stack (IRQ & NMI enabled) */
msr CPSR_c, #ARM_MODE_SVC
msr CPSR_c, #ARM_MODE_SYS
mov sp, r0
/* Relocate .data section */
@ -123,3 +125,31 @@ LoopZI: cmp r1, r2
/* exit dummy for newlib */
exit: b .
.endfunc
.global FIQ_Handler
.func FIQ_Handler
FIQ_Handler:
/* Save r0 to ARM_MODE_FIQ:r9 */
mov r9, r0
/* get FIQ Vector from AIC and thus clear FIQ */
ldr r0, [r8, #AIC_FVR]
/* Switch to ARM_MODE_SVC and save registers there */
msr CPSR_c, #ARM_MODE_SYS | I_BIT | F_BIT
stmfd sp!, { r1-r3, r12, lr }
/* execute FIQ in SVC_MODE */
mov r14, pc
bx r0
/* restore registers and switch back to ARM_MODE_FIQ */
ldmia sp!, { r1-r3, r12, lr }
msr CPSR_c, #ARM_MODE_FIQ | I_BIT | F_BIT
/* restore the ARM_MODE_SVC:r0 */
mov r0, r9
/* restore PC using the LR_fiq directly */
subs pc, lr, #4
.endfunc

150
at91_init1.c
View File

@ -1,6 +1,4 @@
/***************************************************************************
* sam7fc - PLL Init, IRQ/FIQ Vectors *
* *
* Copyright (C) 01/2008 by Olaf Rempel *
* razzor@kopf-tisch.de *
* *
@ -82,95 +80,91 @@ void at91_init1(void)
aic->AIC_SPU = (uint32_t)empty_isr;
}
/* TODO: make it static */
uint32_t nested_count = 0;
__attribute__((naked)) void IRQ_Handler(void)
{
asm volatile (
".equ ARM_MODE_IRQ, 0x12 \n\t"
".equ ARM_MODE_SVC, 0x13 \n\t"
".equ I_BIT, 0x80 \n\t"
".equ AIC_IVR, (256) \n\t"
".equ AIC_EOICR, (304) \n\t"
".equ AIC_IVR_OFF, (256) \n\t"
".equ AIC_EOICR_OFF, (304) \n\t"
".equ AT91C_BASE_AIC, (0xFFFFF000) \n\t"
/* Adjust and save lr_irq on IRQ stack */
"sub lr, lr, #4 \n\t"
"stmfd sp!, { lr } \n\t"
/* Save SPSR (for nested interrupts) */
"mrs r14, SPSR \n\t"
"stmfd sp!, { r14 } \n\t"
/* Save r0 on IRQ stack */
"stmfd sp!, { r0 } \n\t"
/* Write in the IVR to support Protect Mode */
"ldr r14, =AT91C_BASE_AIC \n\t"
"ldr r0, [r14, #AIC_IVR] \n\t"
"str r14, [r14, #AIC_IVR] \n\t"
/* Enable Interrupt and switch to SVC mode */
"msr CPSR_c, #ARM_MODE_SVC \n\t"
/* Save scratch/used registers and lr on SVC Stack */
"stmfd sp!, { r1-r3, r12, r14 } \n\t"
/* Branch to the routine pointed by the AIC_IVR */
"mov r14, pc \n\t"
"bx r0 \n\t"
/* Restore scratch/used registers and lr from SVC Stack */
"ldmia sp!, { r1-r3, r12, r14 } \n\t"
/* Disable Interrupt and switch back to IRQ mode */
"msr CPSR_c, #ARM_MODE_IRQ | I_BIT \n\t"
/* Mark the End of Interrupt on the AIC */
"ldr r14, =AT91C_BASE_AIC \n\t"
"str r14, [r14, #AIC_EOICR] \n\t"
/* Restore SPSR_irq and r0 from IRQ stack */
"ldmia sp!, { r0 } \n\t"
"ldmia sp!, { r14 } \n\t"
"msr SPSR_cxsf, r14 \n\t"
/* Restore adjusted lr_irq from IRQ stack */
"ldmia sp!, { pc }^ \n\t"
);
}
__attribute__((naked)) void FIQ_Handler(void)
{
asm volatile (
".equ ARM_MODE_FIQ, 0x11 \n\t"
".equ ARM_MODE_SVC, 0x13 \n\t"
".equ ARM_MODE_IRQ, 0x12 \n\t"
".equ ARM_MODE_SYS, 0x1F \n\t"
".equ I_BIT, 0x80 \n\t"
".equ F_BIT, 0x40 \n\t"
".equ AIC_FVR, (260) \n\t"
/* Manage Exception Entry */
/* Save LR_irq to IRQ stack */
"sub lr, lr, #4 \n\t"
"stmdb sp!, { lr } \n\t"
/* Save r0 to r9_fiq */
"mov r9, r0 \n\t"
/* Save r0 and SPSR to IRQ stack */
"mrs r14, SPSR \n\t"
"stmdb sp!, { r0, r14 } \n\t"
/* get FIQ Vector from AIC and thus clear FIQ */
"ldr r0, [r8, #AIC_FVR] \n\t"
/* count nested interrupts */
"ldr r14, =nested_count \n\t"
"ldr r0, [r14] \n\t"
"add r0, r0, #1 \n\t"
"str r0, [r14] \n\t"
/* Switch to SVC and save registers there */
"msr CPSR_c, #ARM_MODE_SVC | I_BIT | F_BIT \n\t"
"stmfd sp!, { r1-r3, r12, lr } \n\t"
/* De-assert the NIRQ and clear the source in Protect Mode */
"ldr r14, =AT91C_BASE_AIC \n\t"
"ldr r0, [r14, #AIC_IVR_OFF] \n\t"
"str r14, [r14, #AIC_IVR_OFF] \n\t"
/* execute FIQ in SVC_MODE */
"mov r14, pc \n\t"
"bx r0 \n\t"
/* Enable Interrupt and Switch in Supervisor Mode */
"msr CPSR_c, #ARM_MODE_SYS \n\t"
/* restore registers and switch back to FIQ */
"ldmia sp!, { r1-r3, r12, lr } \n\t"
"msr CPSR_c, #ARM_MODE_FIQ | I_BIT | F_BIT \n\t"
/* Save scratch/used registers and LR in User Stack */
"stmdb sp!, { r1-r3, r12, r14 } \n\t"
/* restore the r0 from r9_fiq */
"mov r0, r9 \n\t"
/* Branch to the routine pointed by the AIC_IVR */
"mov r14, pc \n\t"
"bx r0 \n\t"
/* Restore scratch/used registers and LR from User Stack */
"ldmia sp!, { r1-r3, r12, r14 } \n\t"
/* Disable Interrupt and switch back in IRQ mode */
"msr CPSR_c, #ARM_MODE_IRQ | I_BIT \n\t"
/* Mark the End of Interrupt on the AIC */
"ldr r14, =AT91C_BASE_AIC \n\t"
"str r14, [r14, #AIC_EOICR_OFF] \n\t"
/* count nested interrupts */
"ldr r14, =nested_count \n\t"
"ldr r0, [r14] \n\t"
"subs r0, r0, #1 \n\t"
"str r0, [r14] \n\t"
"beq irq_ctx_save \n\t"
/* Restore r0 and SPSR_irq from IRQ stack */
"ldmia sp!, { r0, r14 } \n\t"
"msr SPSR_cxsf, r14 \n\t"
/* Restore adjusted LR_irq from IRQ stack */
"ldmia sp!, { pc }^ \n\t"
/* save the userspace context to current_context */
"irq_ctx_save: \n\t"
/* get top of struct register_context */
"ldr r0, =current_context \n\t"
"ldr r0, [r0] \n\t"
"add r0, r0, #68 \n\t"
/* save usermode cpsr & r2-14 */
"mrs r1, spsr \n\t"
"stmdb r0, {r1-r14}^ \n\t"
"nop \n\t"
"sub r0, r0, #56 \n\t"
/* save r0-1 and svc_lr (= pc) */
"ldmia sp!, {r1, r2} \n\t"
"stmdb r0!, {r1, r2, r14} \n\t"
/* restore PC using the lr_fiq directly */
"subs pc, lr, #4 \n\t"
);
}

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
View File

Binary file not shown.
Side by Side

Before

Width:  |  Height:  |  Size: 41 KiB

BIN
eagle/sam7fc-sch.png
View File

Binary file not shown.
Side by Side

Before

Width:  |  Height:  |  Size: 203 KiB

BIN
eagle/sam7fc-top.png
View File

Binary file not shown.
Side by Side

Before

Width:  |  Height:  |  Size: 36 KiB

32
include/at91_adc.h
View File

@ -1,32 +0,0 @@
#ifndef AT91_ADC_H_
#define AT91_ADC_H_
#include <stdint.h>
#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
// 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_ */

4
include/at91_dbgu.h
View File

@ -1,10 +1,12 @@
#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,7 +17,6 @@ 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);

5
include/at91_tc1.h
View File

@ -5,18 +5,13 @@
#define RC_CAL_START 0
#define RC_CAL_END 1
#define RC_CAL_LOAD 2
#define RC_CAL_SAVE 3
struct rc_values {
int16_t chan[MAX_CHANNELS];
};
uint32_t rcontrol_getvalues(struct rc_values *rc);
uint32_t rcontrol_getswitches(struct rc_values *rc);
void rcontrol_calibrate(uint32_t mode);
void rcontrol_print_cal(void);
void at91_tc1_init(void);

3
include/at91_tests.h
View File

@ -3,4 +3,7 @@
void at91_rttc_test_init(void);
void at91_adc_test_init(void);
void at91_adc_printresults(void);
#endif /*AT91_TESTS_H_*/

67
include/at91_twi.h
View File

@ -1,16 +1,16 @@
#ifndef AT91TWI_H_
#define AT91TWI_H_
#include <stdint.h>
#include <list.h>
/* TWI slave addresses */
#define TWI_ADDR_BL1 0x21
#define TWI_ADDR_BL2 0x22
#define TWI_ADDR_BL3 0x23
#define TWI_ADDR_BL4 0x24
#define TWI_ADDR_EEPROM 0x50
#define TWI_ADDR_EEPROM 0x40
/* TWIBOOT commands */
/* TWIBOOT commands */
#define CMD_WAIT 0x00
#define CMD_GET_INFO 0x10
#define CMD_GET_SIGNATURE 0x11
@ -25,72 +25,31 @@
//#define CMD_GET_INFO 0x10
#define CMD_SET_PWM 0x21
#define CMD_GET_STATUS 0x22
#define CMD_SET_PARAM 0x23
#define CMD_GET_PARAM 0x24
//#define CMD_SET_PARAM 0x23
//#define CMD_GET_PARAM 0x24
#define CMD_BOOT_LOADER 0x2F
struct blmc_status {
uint8_t pwm_ist;
uint8_t pwm_soll;
uint16_t rpm;
uint16_t current;
uint16_t voltage;
} __attribute__ ((packed));
struct blmc_param {
uint16_t spinup_ticks;
uint8_t spinup_tick;
uint8_t spinup_step;
uint8_t spinup_wait;
uint8_t spinup_pwm;
uint8_t pwm_min;
uint8_t pwm_max;
uint16_t current_limit;
uint16_t current_max;
uint16_t voltage_min;
uint16_t crc16;
} __attribute__ ((packed));
#define EE_PARAMETER_SET_START 0x0000
/* remote control calibration data */
#define EE_RC_CAL_DATA (EE_PARAMETER_SET_START)
#define EE_RC_CAL_DATA_SIZE 48
/* ACC calibration data */
#define EE_ACC_CAL_DATA (EE_RC_CAL_DATA + EE_RC_CAL_DATA_SIZE)
#define EE_ACC_CAL_DATA_SIZE 6
#define EE_PARAMETER_SET_END (EE_ACC_CAL_DATA + EE_ACC_CAL_DATA_SIZE)
struct twi_cmd {
struct blmc_cmd {
uint32_t cmd; /* cmd byte(s) */
uint8_t mode; /* read/write, cmdlen (1-3 bytes) */
uint16_t size; /* data size */
uint8_t size; /* data size */
uint8_t *data; /* read/write data */
};
/* same bits as TWI_MMR[8..15] */
#define TWI_MODE_READ 0x10
#define TWI_MODE_WRITE 0x00
#define TWI_MODE_0_ARG 0x01
#define TWI_MODE_1_ARG 0x02
#define TWI_MODE_2_ARG 0x03
#define BLMC_CMD_READ 0x10
#define BLMC_CMD_WRITE 0x00
#define BLMC_CMD_0_ARG 0x01
#define BLMC_CMD_1_ARG 0x02
#define BLMC_CMD_2_ARG 0x03
uint32_t twi_read_eeprom(uint32_t addr, uint8_t *buf, uint32_t size);
uint32_t twi_write_eeprom(uint32_t addr, uint8_t *buf, uint32_t size);
uint32_t twi_setpwm(uint8_t *values);
uint32_t twi_cmd(uint8_t addr, struct twi_cmd *cmd);
uint32_t twi_cmd(uint8_t addr, struct blmc_cmd *cmd);
void at91_twi_init(void);
void at91_twi_test(void);

14
include/at91_udp.h
View File

@ -1,20 +1,6 @@
#ifndef AT91_UDP_H_
#define AT91_UDP_H_
#include <stdint.h>
void ep_transfer_send(uint32_t ep,
char *data,
uint32_t length,
void (*complete_cb)(void));
void ep_transfer_receive(uint32_t ep,
char *data,
uint32_t length,
void (*complete_cb)(void));
void ep_send_stall(uint32_t ep);
void at91_udp_init(void);
#endif /*AT91_UDP_H_*/

14
include/board.h
View File

@ -12,16 +12,16 @@
#define BAUD_TO_DIV(BAUD) (MCK / (16 * BAUD))
/* LED PIOs */
#define LED_ORANGE AT91C_PIO_PA24
#define LED_GREEN AT91C_PIO_PA23
#define LED_ORANGE AT91C_PIO_PA17
#define LED_GREEN AT91C_PIO_PA18
/* Taster PIOs */
#define TAST1 AT91C_PIO_PA0
#define TAST2 AT91C_PIO_PA1
#define TAST1 AT91C_PIO_PA19
#define TAST2 AT91C_PIO_PA20
/* USB PIOs */
#define UDP_VBUS_MON AT91C_PIO_PA8
#define UDP_PULLUP AT91C_PIO_PA16
#define UDP_VBUS_MON AT91C_PIO_PA24
#define UDP_PULLUP AT91C_PIO_PA25
/* ATMEL IDs */
#define USB_VENDOR_ID 0x03EB
@ -40,6 +40,4 @@
#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_*/

23
include/pidctrl.h
View File

@ -1,23 +0,0 @@
#ifndef _PIDCTRL_H_
#define _PIDCTRL_H_
#include <stdint.h>
struct pid_data {
int32_t kp;
int32_t ki;
int32_t err_sum;
int32_t err_sum_max;
int32_t err_sum_min;
int32_t kd;
int32_t err_old;
int32_t out_max;
int32_t out_min;
};
int32_t pid_ctrl(struct pid_data *pid, int32_t error);
#endif /* _PIDCTRL_H_ */

70
include/rtos/context.h Normal file
View File

@ -0,0 +1,70 @@
#ifndef _CONTEXT_H_
#define _CONTEXT_H_
#include <stdint.h>
#include "rtos/spinlock.h"
struct register_context {
uint32_t r0;
uint32_t r1;
uint32_t pc;
uint32_t cpsr;
uint32_t r2;
uint32_t r3;
uint32_t r4;
uint32_t r5;
uint32_t r6;
uint32_t r7;
uint32_t r8;
uint32_t r9;
uint32_t r10;
uint32_t r11;
uint32_t r12;
uint32_t sp;
uint32_t r14;
};
enum ctx_state {
CONTEXT_NULL = 0,
CONTEXT_RUNNING = 1,
CONTEXT_READY = 2,
CONTEXT_INTERRUPTED = 3,
CONTEXT_SLEEP = 4,
CONTEXT_SLEEP_QUEUE = 5,
};
struct context {
/* regs *MUST* be first in struct! */
struct register_context regs;
void *stack;
uint8_t state;
uint8_t priority;
/* next pointers for run & sleep queues */
struct context *run_queue;
struct context *sleep_queue;
};
extern struct context *current_context;
void isr_context_yield(void);
void context_yield(void);
uint8_t isr_context_wait(struct spinlock *lock);
uint8_t context_wait(struct spinlock *lock);
uint8_t context_wait_queue(struct spinlock *lock, struct context **queue);
uint8_t context_wait_pri_queue(struct spinlock *lock, struct context **queue);
void isr_context_signal(struct context *c);
void context_signal(struct context *c);
uint32_t context_signal_queue(struct context **queue);
void isr_context_interrupt(struct context *c);
void context_interrupt(struct context *c);
uint32_t context_interrupt_queue(struct context *c, struct context **queue);
struct context * create_ctx(uint32_t stacksize, uint8_t priority, void (* code)(void *arg), void *arg);
void init_context(void);
#endif /* _CONTEXT_H_ */

22
include/rtos/semaphore.h Normal file
View File

@ -0,0 +1,22 @@
#ifndef _SEMSPHORE_H_
#define _SEMSPHORE_H_
#include <stdint.h>
#include "rtos/context.h"
#include "rtos/spinlock.h"
struct semaphore {
struct context *sleep_queue;
struct spinlock lock;
int32_t count;
};
uint8_t sem_wait(struct semaphore *sem);
void sem_post(struct semaphore *sem);
void sem_interrupt(struct semaphore *sem, struct context *c);
int32_t sem_get_count(struct semaphore *sem);
void sem_init(struct semaphore *sem, int32_t count);
#endif /* _SEMSPHORE_H_ */

19
include/rtos/spinlock.h Normal file
View File

@ -0,0 +1,19 @@
#ifndef _SPINLOCK_H_
#define _SPINLOCK_H_
#include <stdint.h>
struct spinlock {
uint8_t locked;
uint8_t priority_unlocked;
};
void isr_spinlock_lock(struct spinlock *lock);
void spinlock_lock(struct spinlock *lock);
void isr_spinlock_unlock(struct spinlock *lock);
void spinlock_unlock(struct spinlock *lock);
void spinlock_init(struct spinlock *lock);
#endif /* _SPINLOCK_H_ */

103
include/tdc_proto.h
View File

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

177
main.c
View File

@ -1,106 +1,71 @@
/***************************************************************************
* 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 <stdio.h>
#include <string.h>
#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();
}
}
/***************************************************************************
* 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 <stdint.h>
#include <stdio.h>
#include <string.h>
#include "AT91SAM7S256.h"
#include "board.h"
#include "at91_sysc.h"
#include "at91_dbgu.h"
#include "at91_pio.h"
#include "rtos/context.h"
#include "rtos/semaphore.h"
static struct semaphore sem;
void testisr(uint32_t status, uint32_t input)
{
printf("testisr: sem_post()\n\r");
sem_post(&sem);
}
void testfunc(void *p)
{
while (1) {
printf("testfunc(%ld): sem_wait()\n\r", (uint32_t)p);
sem_wait(&sem);
}
}
int main(void)
{
/* LED outputs */
*AT91C_PIOA_PER = LED_GREEN | LED_ORANGE;
*AT91C_PIOA_OER = LED_GREEN | LED_ORANGE;
at91_sysc_init();
at91_dbgu_init();
at91_dbgu_puts("==========================================================\n\rGood morning Dave\n\r");
at91_pio_init();
struct context *text_ctx1 = create_ctx(512, 0x80, testfunc, (void *)1);
printf("test_ctx(1)=%p\n\r", text_ctx1);
struct context *text_ctx2 = create_ctx(512, 0x80, testfunc, (void *)2);
printf("test_ctx(2)=%p\n\r", text_ctx2);
sem_init(&sem, 0);
init_context();
}
PIO_PINCHANGE_ISR(TAST1, testisr);

238
src/at91_adc.c
View File

@ -1,238 +0,0 @@
/***************************************************************************
* sam7fc - ADC routines / calibration *
* *
* 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 <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"
static uint32_t adc_status;
static uint16_t adc_tmp[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 void at91_adc_isr(void)
{
AT91S_PDC *pdc = AT91C_BASE_PDC_ADC;
pdc->PDC_RPR = (uint32_t) &adc_tmp;
pdc->PDC_RCR = ARRAY_SIZE(adc_tmp);
pdc->PDC_PTCR = AT91C_PDC_RXTEN;
/* clear interrupts */
uint32_t dummy = *AT91C_ADC_SR;
dummy = dummy;
if (!(adc_status & (ADC_CAL_GYRO | ADC_CAL_ACC)))
return;
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;
return PITC_RESTART_TIMER;
} else {
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_calibrate(ADC_CAL_GYRO_COMPLETE);
} else {
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;
adc_calibrate(ADC_CAL_ACC_COMPLETE);
}
return PITC_REMOVE_TIMER;
}
}
static struct pitc_timer adc_cal_timer = {
.interval = 1,
.func = &adc_calibrate_cb,
};
void adc_trigger(void)
{
// 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)
{
/* disable interrupt */
*AT91C_ADC_IDR = AT91C_ADC_ENDRX;
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)
{
/* enable ADC clock */
*AT91C_PMC_PCER = (1 << AT91C_ID_ADC);
/* ADC Software reset */
AT91S_ADC *adc = AT91C_BASE_ADC;
adc->ADC_CR = AT91C_ADC_SWRST;
/*
* ADC config: 10bit, no sleep
* 4.8MHz (48MHz / ((4 +1) * 2) = 4.8MHz)
* 96 cycles Startup ((11 +1) * 8 / 4.8MHz = 20us)
* 3 cycles SH ((2 +1) / 4.8MHz = 625ns)
* Conversion time per channel @5MHz ~2us
*/
adc->ADC_MR = AT91C_ADC_TRGEN_DIS |
AT91C_ADC_LOWRES_10_BIT |
AT91C_ADC_SLEEP_NORMAL_MODE |
(AT91C_ADC_PRESCAL & (4 << 8)) |
(AT91C_ADC_STARTUP & (11 << 16)) |
(AT91C_ADC_SHTIM & (2 << 24));
/* setup PDC */
AT91S_PDC *pdc = AT91C_BASE_PDC_ADC;
pdc->PDC_RPR = (uint32_t) &adc_tmp;
pdc->PDC_RCR = ARRAY_SIZE(adc_tmp);
pdc->PDC_PTCR = AT91C_PDC_RXTEN;
/* enable 7 channels (0-1-2-4-5-6-7), PDC Interrupt */
adc->ADC_CHER = 0xF7;
adc->ADC_IER = AT91C_ADC_ENDRX;
/* low priority, level triggered, own vector */
AT91S_AIC *aic = AT91C_BASE_AIC;
aic->AIC_SMR[AT91C_ID_ADC] = IRQPRIO_ADC | AT91C_AIC_SRCTYPE_INT_HIGH_LEVEL;
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);
}

28
src/at91_dbgu.c
View File

@ -1,6 +1,4 @@
/***************************************************************************
* sam7fc - Debug Unit RS232 Port *
* *
* Copyright (C) 01/2008 by Olaf Rempel *
* razzor@kopf-tisch.de *
* *
@ -26,18 +24,27 @@
#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, DBGU_TX_CHUNKS) == 0)
if (fifo_txpdc(txfifo, AT91C_BASE_PDC_DBGU, 16) == 0)
*AT91C_DBGU_IDR = AT91C_US_TXBUFE;
}
@ -48,11 +55,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(DBGU_BAUDRATE);
dbgu->DBGU_BRGR = BAUD_TO_DIV(115200);
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(DBGU_FIFO_SIZE);
txfifo = fifo_alloc(1024);
/* enable TX PDC */
dbgu->DBGU_PTCR = AT91C_PDC_TXTEN;
@ -63,18 +70,21 @@ 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;
}

10
src/at91_exceptions.c
View File

@ -1,6 +1,4 @@
/***************************************************************************
* sam7fc - Abort Handler with Register/Stackdump *
* *
* Copyright (C) 01/2008 by Olaf Rempel *
* razzor@kopf-tisch.de *
* *
@ -163,6 +161,7 @@ void at91_abt_handler(uint32_t cpsr, uint32_t *registers)
dbgu_putchar('\n');
}
__attribute__((naked)) void ABT_Handler(void)
{
asm volatile (
@ -183,8 +182,9 @@ __attribute__((naked)) void ABT_Handler(void)
"mov r3, lr \n\t"
/* enter previous mode and get lr(r14), sp(r13) */
"orr r1, r0, #I_BIT | F_BIT \n\t"
"msr CPSR_c, r1 \n\t"
/* TODO: interrupts might be enabled? */
/* TODO: thumb mode enabled? */
"msr CPSR_c, r0 \n\t"
"mov r1, sp \n\t"
"mov r2, lr \n\t"
@ -195,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"

4
src/at91_pio.c
View File

@ -1,6 +1,4 @@
/***************************************************************************
* sam7fc - Pinchange Interrupt Handler *
* *
* Copyright (C) 01/2008 by Olaf Rempel *
* razzor@kopf-tisch.de *
* *
@ -57,7 +55,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;

104
src/at91_pitc.c
View File

@ -1,104 +0,0 @@
/***************************************************************************
* sam7fc - Periodic Timer Handling *
* *
* 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_pitc.h"
#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;
static void _pitc_schedule_timer(struct pitc_timer *timer)
{
timer->nextrun = timer->interval + pitc_ticks;
struct pitc_timer *search;
list_for_each_entry(search, &timer_list, list)
if (search->nextrun > timer->nextrun)
break;
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 */
pitc_ticks += (*AT91C_PITC_PIVR & AT91C_PITC_PICNT) >> 20;
struct pitc_timer *search, *tmp;
list_for_each_entry_safe(search, tmp, &timer_list, list) {
/* if this entry not scheduled yet, abort search */
if (pitc_ticks < search->nextrun)
break;
/* remove from list */
list_del(&search->list);
/* exec handler */
if ((search->interval == 0) || search->func(search) == PITC_REMOVE_TIMER) {
/* one-shot timer, mark as completed */
search->nextrun = 0;
continue;
}
/* interval timer, reschedule it */
_pitc_schedule_timer(search);
}
}
uint32_t pitc_get_ticks(void)
{
return pitc_ticks;
}
void at91_pitc_init(void)
{
sysc_register_isr(AT91_SYSIRQ_PIT, &pitc_isr);
*AT91C_PITC_PIMR = (AT91C_PITC_PIV & HZ_TO_PIV(PITC_HZ)) |
AT91C_PITC_PITEN |
AT91C_PITC_PITIEN;
}

48
src/at91_rttc_test.c
View File

@ -1,48 +0,0 @@
/***************************************************************************
* 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"
#include "board.h"
static void rtt_isr(uint32_t status)
{
*AT91C_RTTC_RTAR = *AT91C_RTTC_RTVR +1;
}
void at91_rttc_test_init(void)
{
/* calculate SLOWCK from MAINCK and measured MAINF */
uint32_t prescaler = MAINCK * 16 / (*AT91C_CKGR_MCFR & AT91C_CKGR_MAINF);
sysc_register_isr(AT91_SYSIRQ_RTT, &rtt_isr);
/*
* AT91C_RTTC_RTTINCIEN doesn't work
* use AT91C_RTTC_ALMIEN and increment RTAR in isr
*/
*AT91C_RTTC_RTAR = *AT91C_RTTC_RTVR +1;
*AT91C_RTTC_RTMR = (AT91C_RTTC_RTPRES & prescaler) |
AT91C_RTTC_ALMIEN |
AT91C_RTTC_RTTRST;
printf("rttc running at %ld Hz\n\r", prescaler);
}

30
src/at91_sysc.c
View File

@ -1,6 +1,4 @@
/***************************************************************************
* sam7fc - System Interrupt Dispatcher *
* *
* Copyright (C) 01/2008 by Olaf Rempel *
* razzor@kopf-tisch.de *
* *
@ -33,15 +31,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
@ -60,12 +58,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)
},
/*
@ -86,7 +84,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)
@ -106,27 +104,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;
@ -140,9 +138,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<<AT91C_ID_SYS);
}
@ -151,6 +149,6 @@ void sysc_register_isr(enum syscirqs irq, SYSCISR *isr)
{
if (irq >= AT91_SYSIRQ_COUNT)
return;
sysc_isrs[irq] = isr;
}

257
src/at91_tc1.c
View File

@ -1,257 +0,0 @@
/***************************************************************************
* sam7fc - RC-PPM Signal decoder *
* *
* 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 <stdint.h>
#include <stdio.h>
#include <stdlib.h> /* abs() */
#include "AT91SAM7S256.h"
#include "board.h"
#include "at91_tc1.h"
#include "at91_twi.h"
/* Hard limits for ISR */
#define PULSE_MIN 0x0500
#define PULSE_MAX 0x0D00
#define PULSE_TIMEOUT 0x0F00
#define PULSE_CENTER 0x08C0
#define PULSE_SWITCH 0x01F0
/* moving average filters */
#define PULSE_FILTER_FAST (1<<2)
#define PULSE_FILTER_SLOW (1<<4)
#define PULSE_FILTER_DIFF 16 /* point to switch filters */
#define PULSE_MID_DIFF 50 /* minimum diff to center */
#define VALUE_RANGE 256
#define ROUND_DIV256(x) ((x >> 8) + ((x & 0x80) ? 1 : 0))
struct channel_data {
uint16_t width;
uint16_t width_slow;
uint16_t filter; /* 0 - fast filter, 1 - slow filter */
uint16_t min; /* minimum value during calibration */
uint16_t mid; /* center value */
uint16_t max; /* maximum value */
};
/* check eeprom parameter size (uint16_t min/mid/max) */
#if ((MAX_CHANNELS * 3 * 2) != EE_RC_CAL_DATA_SIZE)
#error "invalid EE_RC_CAL_DATA_SIZE"
#endif
static struct channel_data ch_data[MAX_CHANNELS];
static uint32_t count, valid, cal_in_progress;
static void ppm_isr(void)
{
static uint32_t i;
/* RC Compare -> no TIOA1 edge for 2.5ms */
uint32_t status = *AT91C_TC1_SR;
if (status & AT91C_TC_CPCS) {
/* average channel count */
count = ((count * 7) + (i << 8)) / 8;
/* at least 4 channels and a stable channel count */
if ((ROUND_DIV256(count) == i) && (i >= 4)) {
if (valid < 10)
valid++;
} else if (valid > 0) {
valid--;
}
/* reset index */
i = 0;
}
/* edge on TIOA1 */
if (status & AT91C_TC_LDRAS) {
/* get impulse width */
uint16_t width = *AT91C_TC1_RA;
/* valid range: 1 - 2ms */
if (width > PULSE_MIN && width < PULSE_MAX) {
if (i < ARRAY_SIZE(ch_data)) {
/* calc both filters */
ch_data[i].width = ((ch_data[i].width * (PULSE_FILTER_FAST -1)) + width) / PULSE_FILTER_FAST;
ch_data[i].width_slow = ((ch_data[i].width_slow * (PULSE_FILTER_SLOW -1)) + width) / PULSE_FILTER_SLOW;
if (cal_in_progress) {
/* use slow filter values, calc center */
ch_data[i].min = MIN(ch_data[i].width_slow, ch_data[i].min);
ch_data[i].max = MAX(ch_data[i].width_slow, ch_data[i].max);
ch_data[i].mid = (ch_data[i].min + ch_data[i].max) / 2;
}
}
i++;
}
}
}
uint32_t rcontrol_getvalues(struct rc_values *rc)
{
if (valid < 5)
return 0;
uint32_t i;
uint32_t cnt = MIN(ROUND_DIV256(count), ARRAY_SIZE(ch_data));
for (i = 0; i < cnt; i++) {
/* switch between fast and slow filter */
uint16_t filter = (abs(ch_data[i].width - ch_data[i].width_slow) < PULSE_FILTER_DIFF);
/*
* transition fast -> slow filter
* slow filter is lagging behind, so give it a boost
*/
if (filter && !ch_data[i].filter && !cal_in_progress)
ch_data[i].width_slow = ch_data[i].width;
ch_data[i].filter = filter;
uint16_t width = (filter) ? ch_data[i].width_slow : ch_data[i].width;
/* expand the value to +/- VALUE_RANGE */
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));
// TODO: stick mapping
/* keep result in range */
rc->chan[i] = LIMIT(tmp, -VALUE_RANGE, +VALUE_RANGE);
}
return cnt;
}
uint32_t rcontrol_getswitches(struct rc_values *rc)
{
if (valid < 5)
return 0;
uint32_t i;
uint32_t cnt = MIN(ROUND_DIV256(count), ARRAY_SIZE(ch_data));
for (i = 0; i < cnt; i++) {
if (ch_data[i].width > (PULSE_CENTER + PULSE_SWITCH))
rc->chan[i] = VALUE_RANGE;
else if (ch_data[i].width < (PULSE_CENTER - PULSE_SWITCH))
rc->chan[i] = -VALUE_RANGE;
else
rc->chan[i] = 0;
}
return cnt;
}
void rcontrol_calibrate(uint32_t mode)
{
uint32_t i;
uint8_t buf[EE_RC_CAL_DATA_SIZE];
uint16_t *ptr = (uint16_t *)buf;
switch (mode) {
case RC_CAL_START:
cal_in_progress = 1;
for (i = 0; i < ARRAY_SIZE(ch_data); i++) {
/* use hard limits as hint */
ch_data[i].max = PULSE_MIN;
ch_data[i].mid = (PULSE_MIN + PULSE_MAX) / 2;
ch_data[i].min = PULSE_MAX;
}
break;
case RC_CAL_END:
cal_in_progress = 0;
for (i = 0; i < ARRAY_SIZE(ch_data); i++) {
/* treat current position as center */
ch_data[i].mid = ch_data[i].width_slow;
/* 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 -RANGE..0 */
if (ch_data[i].max - ch_data[i].mid < PULSE_MID_DIFF)
ch_data[i].mid = ch_data[i].max;
}
break;
case RC_CAL_LOAD:
twi_read_eeprom(EE_RC_CAL_DATA, buf, EE_RC_CAL_DATA_SIZE);
for (i = 0; i < ARRAY_SIZE(ch_data); i++) {
ch_data[i].min = *ptr++;
ch_data[i].mid = *ptr++;
ch_data[i].max = *ptr++;
}
break;
case RC_CAL_SAVE:
for (i = 0; i < ARRAY_SIZE(ch_data); i++) {
*ptr++ = ch_data[i].min;
*ptr++ = ch_data[i].mid;
*ptr++ = ch_data[i].max;
}
twi_write_eeprom(EE_RC_CAL_DATA, buf, EE_RC_CAL_DATA_SIZE);
break;
}
}
void rcontrol_print_cal(void)
{
uint32_t i;
printf("stick-calibration:\n\r");
for (i = 0; i < ARRAY_SIZE(ch_data); i++) {
printf(" %ld: %d(%+d) - %d(0) - %d(%+d)\n\r", i,
ch_data[i].min, ch_data[i].min - ch_data[i].mid,
ch_data[i].mid,
ch_data[i].max, ch_data[i].max - ch_data[i].mid
);
}
}
void at91_tc1_init(void)
{
/* enable TC1 clock */
*AT91C_PMC_PCER = (1 << AT91C_ID_TC1);
/* MCK /32, trigger & capture on falling TIOA1 edge */
*AT91C_TC1_CMR = AT91C_TC_CLKS_TIMER_DIV3_CLOCK | AT91C_TC_LDRA_FALLING |
AT91C_TC_ETRGEDG_FALLING | AT91C_TC_ABETRG;
/* enable RA load and RC compare interrupt */
*AT91C_TC1_IER = AT91C_TC_LDRAS | AT91C_TC_CPCS;
/* RC Compare Interrupt if no rising Edge on TIOA1 for 2.56ms */
*AT91C_TC1_RC = PULSE_TIMEOUT;
/* enable & trigger the clock */
*AT91C_TC1_CCR = AT91C_TC_CLKEN | AT91C_TC_SWTRG;
/* level triggered, own vector */
AT91S_AIC *aic = AT91C_BASE_AIC;
aic->AIC_SMR[AT91C_ID_TC1] = IRQPRIO_TC1 | AT91C_AIC_SRCTYPE_INT_HIGH_LEVEL;
aic->AIC_SVR[AT91C_ID_TC1] = (uint32_t)ppm_isr;
aic->AIC_IECR = (1 << AT91C_ID_TC1);
rcontrol_calibrate(RC_CAL_LOAD);
rcontrol_print_cal();
}

329
src/at91_twi.c
View File

@ -1,329 +0,0 @@
/***************************************************************************
* sam7fc - TWI/I2C Handling *
* *
* 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 <stdint.h>
#include <stdio.h>
#include "AT91SAM7S256.h"
#include "board.h"
#include "at91_twi.h"
/*
* undocumented TWI_SR flags, at least OVRE seems to be present on a sam7s256
* taken from linux-2.6.24/include/asm-arm/arch-at91/at91_twi.h
*/
#define AT91_TWI_OVRE (1<<6) /* Overrun */
#define AT91_TWI_UNRE (1<<7) /* Underrun */
/*
* while ((cldiv = ((MCK / (2 * TWI)) -3) / (1 << ckdiv)) > 255)
* ckdiv++ ;
*
* works for TWI >= 100kHz
*/
#define TWI_CLK(x) (((MCK / (2 * (x))) -3)<<8 | ((MCK / (2 * (x))) -3))
enum twi_states {
TWI_IDLE = 0x00,
TWI_ERROR,
TWI_GENERIC_CMD = 0x10,
TWI_BLMC_UPDATE = 0x20,
};
static volatile uint32_t twi_state = TWI_IDLE;
static uint8_t *twi_data;
static uint32_t twi_size;
static uint32_t twi_count;
static void twi_isr(void)
{
/* get status */
uint32_t status = *AT91C_TWI_SR;
status &= *AT91C_TWI_IMR;
/* NACK - disable all interrupts and go to state TWI_ERROR */
if (status & AT91C_TWI_NACK) {
*AT91C_TWI_IDR = AT91C_TWI_TXCOMP | AT91C_TWI_RXRDY | AT91C_TWI_TXRDY | AT91C_TWI_NACK;
twi_state = TWI_ERROR;
return;
}
/* tx register ready for new data */
if (status & AT91C_TWI_TXRDY) {
if (twi_count != twi_size) {
/* feed next byte */
*AT91C_TWI_THR = twi_data[twi_count++];
} else {
/* wait for TXCOMP */
*AT91C_TWI_IDR = AT91C_TWI_RXRDY | AT91C_TWI_TXRDY;
*AT91C_TWI_IER = AT91C_TWI_TXCOMP | AT91C_TWI_NACK;
}
}
/* rx register has data */
if (status & AT91C_TWI_RXRDY) {
/* get data */
twi_data[twi_count++] = *AT91C_TWI_RHR;
/* transfer complete? */
if (twi_count == twi_size) {
/* send STOP and wait for TXCOMP */
*AT91C_TWI_CR = AT91C_TWI_STOP;
*AT91C_TWI_IDR = AT91C_TWI_TXRDY;
*AT91C_TWI_IER = AT91C_TWI_TXCOMP | AT91C_TWI_NACK;
}
}
/* transfer really complete? */
if (status & AT91C_TWI_TXCOMP) {
uint32_t addr = (*AT91C_TWI_MMR >> 16) & 0x7F;
/* are we doing a blmc update? */
if (twi_state == TWI_BLMC_UPDATE && addr != TWI_ADDR_BL4) {
/* increase address */
*AT91C_TWI_MMR += (1<<16);
/* send next value to next blmc */
*AT91C_TWI_THR = *twi_data++;
} else {
*AT91C_TWI_IDR = AT91C_TWI_TXCOMP | AT91C_TWI_RXRDY | AT91C_TWI_TXRDY | AT91C_TWI_NACK;
twi_state = TWI_IDLE;
}
}
}
uint32_t twi_setpwm(uint8_t *values)
{
if (twi_state == TWI_ERROR)
twi_state = TWI_IDLE;
if (twi_state != TWI_IDLE)
return 1;
twi_state = TWI_BLMC_UPDATE;
twi_data = values;
twi_size = 0;
twi_count = 0;
*AT91C_TWI_MMR = (TWI_ADDR_BL1 << 16) | AT91C_TWI_IADRSZ_1_BYTE;
*AT91C_TWI_IADR = CMD_SET_PWM;
*AT91C_TWI_THR = *twi_data++;
*AT91C_TWI_IER = AT91C_TWI_TXRDY | AT91C_TWI_NACK;
return 0;
}
uint32_t twi_cmd(uint8_t addr, struct twi_cmd *cmd)
{
if (twi_state == TWI_ERROR)
twi_state = TWI_IDLE;
if (twi_state != TWI_IDLE)
return 1;
/* TODO: locking needed? */
twi_state = TWI_GENERIC_CMD;
/* read transfer, or write transfer with payload */
if (cmd->mode & TWI_MODE_READ || cmd->size != 0) {
/* set address, direction, argument count and command bytes */
*AT91C_TWI_MMR = (addr << 16) | (cmd->mode & 0xFF) << 8;
*AT91C_TWI_IADR = cmd->cmd;
/* write transfer without payload */
} else {
/* use one cmd byte as payload (needed to start transfer) */
cmd->mode--;
*AT91C_TWI_MMR = (addr << 16) | (cmd->mode & 0xFF) << 8;
*AT91C_TWI_IADR = (cmd->cmd) >> 8;
}
/* isr needs data & size parameters */
twi_data = cmd->data;
twi_size = cmd->size;
twi_count = 0;
if (cmd->mode & TWI_MODE_READ) {
*AT91C_TWI_CR = AT91C_TWI_START;
*AT91C_TWI_IER = AT91C_TWI_RXRDY | AT91C_TWI_NACK;
} else {
*AT91C_TWI_THR = (twi_size != 0) ? cmd->data[twi_count++] : (cmd->cmd & 0xFF);
*AT91C_TWI_IER = AT91C_TWI_TXRDY | AT91C_TWI_NACK;
}
/*
* wait for end
* TODO: locking needed?
* TODO: timeout?
*/
while (twi_state != TWI_IDLE && twi_state != TWI_ERROR);
if (twi_state != TWI_IDLE) {
twi_state = TWI_IDLE;
return 1;
}
return 0;
}
uint32_t twi_read_eeprom(uint32_t addr, uint8_t *buf, uint32_t size)
{
struct twi_cmd cmd = {
.cmd = (addr & 0x7FFF),
.mode = TWI_MODE_READ | TWI_MODE_1_ARG,
.size = (size & 0x7FFF),
.data = buf,
};
if (twi_cmd(TWI_ADDR_EEPROM, &cmd) != 0)
size = 0;
return size;
}
uint32_t twi_write_eeprom(uint32_t addr, uint8_t *buf, uint32_t size)
{
uint32_t len = size;
while (len > 0) {
uint32_t count = 0x40 - (addr & 0x3F);
if (count > len)
count = len;
/* TODO: write complete polling */
volatile uint32_t x;
for (x = 0; x < 200000; x++);
struct twi_cmd cmd = {
.cmd = (addr & 0x7FFF),
.mode = TWI_MODE_WRITE | TWI_MODE_1_ARG,
.size = count,
.data = buf,
};
if (twi_cmd(TWI_ADDR_EEPROM, &cmd) != 0)
break;
addr += count;
buf += count;
len -= count;
}
return size - len;
}
void at91_twi_test(void)
{
uint32_t i;
for (i = TWI_ADDR_BL1; i <= TWI_ADDR_BL4; i++) {
printf("twi[0x%02lx] ", i);
struct twi_cmd cmd = {
.cmd = CMD_BOOT_LOADER,
.mode = TWI_MODE_WRITE | TWI_MODE_0_ARG,
};
twi_cmd(i, &cmd);
/* TODO: sleep */
volatile uint32_t x;
for (x = 0; x < 200000; x++);
uint8_t buf[16];
buf[0] = '\0';
cmd.cmd = CMD_GET_INFO;
cmd.mode = TWI_MODE_READ | TWI_MODE_0_ARG;
cmd.size = sizeof(buf);
cmd.data = buf;
twi_cmd(i, &cmd);
printf("boot:'%s' ", buf);
/* TODO: single 32bit write */
buf[0] = 0xFF;
buf[1] = 0xFF;
buf[2] = 0xFF;
cmd.cmd = CMD_GET_SIGNATURE;
cmd.size = 4;
twi_cmd(i, &cmd);
printf("sig:0x%02x%02x%02x\n\r", buf[0], buf[1], buf[2]);
cmd.cmd = CMD_BOOT_APPLICATION;
cmd.mode = TWI_MODE_WRITE | TWI_MODE_0_ARG;
cmd.size = 0;
twi_cmd(i, &cmd);
/* TODO: sleep */
for (x = 0; x < 200000; x++);
buf[0] = '\0';
cmd.cmd = CMD_GET_INFO;
cmd.mode = TWI_MODE_READ | TWI_MODE_0_ARG;
cmd.size = sizeof(buf);
cmd.data = buf;
twi_cmd(i, &cmd);
printf(" app :'%s' ", buf);
struct blmc_param param;
cmd.cmd = CMD_GET_PARAM;
cmd.mode = TWI_MODE_READ | TWI_MODE_0_ARG;
cmd.size = sizeof(param);
cmd.data = (uint8_t *)&param;
twi_cmd(i, &cmd);
printf("pwm:0x%02x-0x%02x Ilimit:0x%03x Imax:0x%03x\n\r",
param.pwm_min, param.pwm_max,
param.current_limit, param.current_max);
}
}
void at91_twi_init(void)
{
/* enable Clock */
*AT91C_PMC_PCER = (1 << AT91C_ID_TWI);
/* SDA & SCL from Peripheral A, Open Drain, no Pullup */
AT91S_PIO *pio = AT91C_BASE_PIOA;
/* do a software reset (bus not connected) */
*AT91C_TWI_CR = AT91C_TWI_SWRST;
pio->PIO_MDER = AT91C_PA3_TWD | AT91C_PA4_TWCK;
pio->PIO_PPUDR = AT91C_PA3_TWD | AT91C_PA4_TWCK;
pio->PIO_ASR = AT91C_PA3_TWD | AT91C_PA4_TWCK;
pio->PIO_PDR = AT91C_PA3_TWD | AT91C_PA4_TWCK;
/* set TWI Clock */
*AT91C_TWI_CWGR = TWI_CLK(400000); //| (5<<16);
/* disable all (known) interrupts */
*AT91C_TWI_IDR = AT91C_TWI_TXCOMP | AT91C_TWI_RXRDY | AT91C_TWI_TXRDY | AT91C_TWI_NACK;
/* level triggered, own vector */
AT91S_AIC *aic = AT91C_BASE_AIC;
aic->AIC_SMR[AT91C_ID_TWI] = IRQPRIO_TWI | AT91C_AIC_SRCTYPE_INT_HIGH_LEVEL;
aic->AIC_SVR[AT91C_ID_TWI] = (uint32_t)twi_isr;
aic->AIC_IECR = (1 << AT91C_ID_TWI);
/* enable teh monster */
*AT91C_TWI_CR = AT91C_TWI_MSEN;
}

683
src/at91_udp.c
View File

@ -1,683 +0,0 @@
/***************************************************************************
* sam7fc - USB Device Port with logical Serial Port *
* *
* 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 <stdio.h>
#include "AT91SAM7S256.h"
#include "at91_pio.h"
#include "board.h"
#include "fifo.h"
#include "telemetrie.h"
#include "usb_ch9.h"
#include "usb_cdc.h"
#include "usb_dfu.h"
#define csr_clear_flags(csr, flags) \
while ((csr) & (flags)) \
(csr) &= ~(flags);
#define csr_set_flags(csr, flags) \
while (((csr) & (flags)) != (flags)) \
(csr) |= (flags);
struct ep_transfer {
uint16_t length;
uint16_t curpos;
char *data;
void (*complete_cb)(void);
};
struct ep_ctx {
uint16_t maxpktsize;
uint16_t flags;
union {
struct ep_transfer *transfer;
struct fifo *fifo;
};
};
#define CTX_TRANSFER 0x01 /* ctx use ep_transfer struct */
#define CTX_FIFO 0x02 /* ctx use fifo */
#define CTX_IN 0x04 /* write to the host */
#define CTX_OUT 0x08 /* read from the host */
#define CTX_RXBANK0 0x10
#define CTX_RXBANK1 0x20
static struct ep_transfer ep0_transfer;
static struct ep_ctx ep_ctx[4];
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,
.bcdUSB = 0x0110,
.bMaxPacketSize0 = 8,
.idVendor = USB_VENDOR_ID,
.idProduct = USB_PRODUCT_ID +1,
.bcdDevice = 0x0001,
.iProduct = 0x01,
.bNumConfigurations = 1,
};
struct my_config {
struct usb_config_descriptor cfg;
struct usb_interface_descriptor ctrl_iface;
struct usb_cdc_header_desc cdc_header;
struct usb_cdc_call_mgmt_descriptor cdc_call_mgmt;
struct usb_cdc_acm_descriptor cdc_acm;
struct usb_cdc_union_desc cdc_union;
struct usb_endpoint_descriptor notify_ep;
struct usb_interface_descriptor data_iface;
struct usb_endpoint_descriptor dataout_ep;
struct usb_endpoint_descriptor datain_ep;
struct usb_interface_descriptor dfu_iface;
struct usb_dfu_descriptor dfu;
} __attribute__ ((packed));
static const struct my_config cfg_descriptor = {
.cfg = {
.bLength = sizeof(struct usb_config_descriptor),
.bDescriptorType = USB_DT_CONFIG,
.wTotalLength = sizeof(struct my_config),
.bNumInterfaces = 3,
.bConfigurationValue = 1,
.bmAttributes = USB_CONFIG_ATT_SELFPOWER | USB_CONFIG_ATT_WAKEUP,
.bMaxPower = 50,
},
.ctrl_iface = {
.bLength = sizeof(struct usb_interface_descriptor),
.bDescriptorType = USB_DT_INTERFACE,
.bInterfaceNumber = 0,
.bNumEndpoints = 1,
.bInterfaceClass = USB_CLASS_COMM,
.bInterfaceSubClass = USB_CDC_SUBCLASS_ACM,
.bInterfaceProtocol = 1,
},
.cdc_header = {
.bLength = sizeof(struct usb_cdc_header_desc),
.bDescriptorType = USB_DT_CS_INTERFACE,
.bDescriptorSubType = USB_CDC_HEADER_TYPE,
.bcdCDC = 0x0110,
},
.cdc_call_mgmt = {
.bLength = sizeof(struct usb_cdc_call_mgmt_descriptor),
.bDescriptorType = USB_DT_CS_INTERFACE,
.bDescriptorSubType = USB_CDC_CALL_MANAGEMENT_TYPE,
.bmCapabilities = USB_CDC_CALL_MGMT_CAP_CALL_MGMT,
.bDataInterface = 1,
},
.cdc_acm = {
.bLength = sizeof(struct usb_cdc_acm_descriptor),
.bDescriptorType = USB_DT_CS_INTERFACE,
.bDescriptorSubType = USB_CDC_ACM_TYPE,
.bmCapabilities = (USB_CDC_CAP_BRK | USB_CDC_CAP_LINE | USB_CDC_COMM_FEATURE),
},
.cdc_union = {
.bLength = sizeof(struct usb_cdc_union_desc),
.bDescriptorType = USB_DT_CS_INTERFACE,
.bDescriptorSubType = USB_CDC_UNION_TYPE,
.bMasterInterface0 = 0,
.bSlaveInterface0 = 1,
},
.notify_ep = {
.bLength = sizeof(struct usb_endpoint_descriptor),
.bDescriptorType = USB_DT_ENDPOINT,
.bEndpointAddress = USB_DIR_IN | 0x03,
.bmAttributes = USB_ENDPOINT_XFER_INT,
.wMaxPacketSize = 64,
.bInterval = 10,
},
.data_iface = {
.bLength = sizeof(struct usb_interface_descriptor),
.bDescriptorType = USB_DT_INTERFACE,
.bInterfaceNumber = 1,
.bNumEndpoints = 2,
.bInterfaceClass = USB_CLASS_CDC_DATA,
},
.dataout_ep = {
.bLength = sizeof(struct usb_endpoint_descriptor),
.bDescriptorType = USB_DT_ENDPOINT,
.bEndpointAddress = USB_DIR_OUT | 0x01,
.bmAttributes = USB_ENDPOINT_XFER_BULK,
.wMaxPacketSize = 64,
},
.datain_ep = {
.bLength = sizeof(struct usb_endpoint_descriptor),
.bDescriptorType = USB_DT_ENDPOINT,
.bEndpointAddress = USB_DIR_IN | 0x02,
.bmAttributes = USB_ENDPOINT_XFER_BULK,
.wMaxPacketSize = 64,
},
.dfu_iface = {
.bLength = sizeof(struct usb_interface_descriptor),
.bDescriptorType = USB_DT_INTERFACE,
.bInterfaceNumber = 2,
.bInterfaceClass = USB_CLASS_APP_SPEC,
.bInterfaceSubClass = 0x01, /* DFU */
.bInterfaceProtocol = 0x01,
},
.dfu = {
.bLength = sizeof(struct usb_dfu_descriptor),
.bDescriptorType = USB_TYPE_DFU,
.bmAttributes = USB_DFU_CAN_DOWNLOAD | USB_DFU_CAN_UPLOAD | USB_DFU_MANIFEST_TOL | USB_DFU_WILL_DETACH,
.wDetachTimeOut = 0xff00,
.wTransferSize = AT91C_IFLASH_PAGE_SIZE,
.bcdDFUVersion = 0x0101,
},
};
/* not const! */
static struct usb_cdc_line_coding cdc_line_coding = {
.dwDTERate = 9600,
.bCharFormat = USB_CDC_1_STOP_BITS,
.bParityType = USB_CDC_NO_PARITY,
.bDataBits = 8,
};
/* not const! */
static struct dfu_status dfu_status = {
.bStatus = DFU_STATUS_OK,
.bState = DFU_STATE_appIDLE,
};
static const struct usb_string_descriptor usb_string0 = {
/* String 0 - Language */
.bLength = sizeof(struct usb_string_descriptor) + 1 * sizeof(uint16_t),
.bDescriptorType = USB_DT_STRING,
.wData = { 0x0409 /* English */ },
};
static const struct usb_string_descriptor usb_string1 = {
/* String 1 "sam7fc" */
.bLength = sizeof(struct usb_string_descriptor) + 6 * sizeof(uint16_t),
.bDescriptorType = USB_DT_STRING,
.wData = {
0x0073, 0x0061, 0x006d, 0x0037, 0x0066, 0x0063,
},
};
static const struct usb_string_descriptor *usb_strings[] = {
&usb_string0, &usb_string1,
};
void ep_transfer_send(uint32_t ep, char *data, uint32_t length,
void (*complete_cb)(void))
{
struct ep_ctx *ctx = &ep_ctx[ep];
// printf("ep_transfer_send(%ld) size=%ld flags=0x%x\n\r", ep, length, ctx->flags);
if (!(ctx->flags & CTX_TRANSFER) || (ctx->flags & (CTX_IN | CTX_OUT)))
return;
/* from buffer to usb */
ctx->flags |= CTX_IN;
struct ep_transfer *transfer = ctx->transfer;
transfer->length = length;
transfer->curpos = 0;
transfer->data = data;
transfer->complete_cb = complete_cb;
uint32_t maxsize = ctx->maxpktsize;
/* get data from transfer */
while (transfer->curpos < transfer->length && maxsize--)
AT91C_UDP_FDR[ep] = transfer->data[transfer->curpos++];
/* trigger tx */
AT91C_UDP_CSR[ep] |= AT91C_UDP_TXPKTRDY;
}
void ep_transfer_receive(uint32_t ep, char *data, uint32_t length,
void (*complete_cb)(void))
{
struct ep_ctx *ctx = &ep_ctx[ep];
// printf("ep_transfer_receive(%ld) size=%ld flags=0x%x\n\r", ep, length, ctx->flags);
if (!(ctx->flags & CTX_TRANSFER) || (ctx->flags & (CTX_IN | CTX_OUT)))
return;
/* from usb to buffer */
ctx->flags |= CTX_OUT;
struct ep_transfer *transfer = ctx->transfer;
transfer->length = length;
transfer->curpos = 0;
transfer->data = data;
transfer->complete_cb = complete_cb;
}
/* stalls the endpoint */
static void ep_send_stall(uint32_t ep)
{
printf("usb stall\n\r");
AT91C_UDP_CSR[ep] |= AT91C_UDP_FORCESTALL;
}
static void udp_configure_ep(const struct usb_endpoint_descriptor *desc)
{
/* get endpoint address, set Max Packet Size */
uint32_t ep = desc->bEndpointAddress & USB_ENDPOINT_NUMBER_MASK;
ep_ctx[ep].maxpktsize = desc->wMaxPacketSize;
/* get endpoint type (ctrl, iso, bulb, int) */
uint32_t eptype = desc->bmAttributes & USB_ENDPOINT_XFERTYPE_MASK;
if (desc->bEndpointAddress & USB_ENDPOINT_DIR_MASK) {
eptype |= 0x04;
} else {
ep_ctx[ep].flags |= CTX_RXBANK0;
}
/* configure UDP endpoint and enable interrupt */
AT91C_UDP_CSR[ep] = AT91C_UDP_EPEDS | (eptype << 8);
*AT91C_UDP_IER = (1 << ep);
}
static void udp_print_config(void)
{
printf("usb: addr=%d cfg=%d if=%d\n\r",
current_address, current_config, current_interface);
}
/*
* set local address
* (USB_REQ_SET_ADDRESS callback)
*/
static void udp_txcb_setaddress(void)
{
*AT91C_UDP_FADDR = (AT91C_UDP_FEN | current_address);
*AT91C_UDP_GLBSTATE = AT91C_UDP_FADDEN;
udp_print_config();
}
/*
* configure endpoints
* (USB_REQ_SET_CONFIGURATION callback)
*/
static void udp_txcb_setconfig(void)
{
udp_configure_ep(&cfg_descriptor.notify_ep);
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();
}
static void ep_handle_ctrlrequest(struct usb_ctrlrequest *req)
{
// printf("typ:0x%02x req:0x%02x val:0x%04x idx:0x%04x len:0x%04x\n\r",
// req->bRequestType, req->bRequest, req->wValue, req->wIndex, req->wLength);
switch (req->bRequestType & (USB_TYPE_MASK | USB_RECIP_MASK)) {
case (USB_TYPE_STANDARD | USB_RECIP_DEVICE): /* 0x00/0x80 */
switch (req->bRequest) {
case USB_REQ_SET_ADDRESS: /* 0x05 */
current_address = req->wValue;
ep_transfer_send(0, NULL, 0, udp_txcb_setaddress);
break;
case USB_REQ_GET_DESCRIPTOR: /* 0x06 */
switch (req->wValue >> 8) {
case USB_DT_DEVICE: /* 0x01 */
ep_transfer_send(0, (char *)&dev_descriptor,
MIN(sizeof(dev_descriptor), req->wLength),
NULL);
break;
case USB_DT_CONFIG: /* 0x02 */
ep_transfer_send(0, (char *)&cfg_descriptor,
MIN(sizeof(cfg_descriptor), req->wLength),
NULL);
break;
case USB_DT_STRING: /* 0x03 */
;
uint8_t index = req->wValue & 0xFF;
if (index < ARRAY_SIZE(usb_strings)) {
ep_transfer_send(0, (char *)usb_strings[index],
MIN(usb_strings[index]->bLength, req->wLength),
NULL);
} else {
ep_send_stall(0);
}
break;
case USB_DT_CS_DEVICE: /* 0x21 */
ep_transfer_send(0, (char *)&cfg_descriptor.dfu,
MIN(sizeof(cfg_descriptor.dfu), req->wLength),
NULL);
break;
default:
ep_send_stall(0);
break;
}
break;
case USB_REQ_SET_CONFIGURATION: /* 0x09 */
current_config = req->wValue;
ep_transfer_send(0, NULL, 0, udp_txcb_setconfig);
break;
default:
ep_send_stall(0);
break;
}
break;
case (USB_TYPE_STANDARD | USB_RECIP_INTERFACE): /* 0x01/0x81 */
switch (req->bRequest) {
case USB_REQ_SET_INTERFACE: /* 0x0b */
current_interface = req->wValue;
ep_transfer_send(0, NULL, 0, udp_print_config);
break;
default:
ep_send_stall(0);
break;
}
break;
case (USB_TYPE_CLASS | USB_RECIP_INTERFACE): /* 0x21/0xA1 */
// TODO: follow current_interface
switch (req->bRequest) {
case USB_REQ_DFU_DETACH: /* 0x00 */
dfu_status.bStatus = DFU_STATE_appDETACH;
ep_transfer_send(0, NULL, 0, NULL);
break;
case USB_REQ_DFU_GETSTATUS: /* 0x03 */
ep_transfer_send(0, (char *)&dfu_status, sizeof(dfu_status), NULL);
break;
case USB_CDC_REQ_SET_LINE_CODING: /* 0x20 */
ep_transfer_receive(0, (char *)&cdc_line_coding, sizeof(cdc_line_coding), NULL);
break;
case USB_CDC_REQ_GET_LINE_CODING: /* 0x21 */
ep_transfer_send(0, (char *)&cdc_line_coding, sizeof(cdc_line_coding), NULL);
break;
case USB_CDC_REQ_SET_CONTROL_LINE_STATE: /* 0x22 */
ep_transfer_send(0, NULL, 0, NULL);
break;
default:
ep_send_stall(0);
break;
}
break;
default:
ep_send_stall(0);
break;
}
}
static void udp_handle_ep(uint32_t ep)
{
/* endpoint enabled? */
AT91_REG *csr = &AT91C_UDP_CSR[ep];
if (!(*csr & AT91C_UDP_EPEDS))
return;
/* clear STALLSENT interrupt */
if (*csr & AT91C_UDP_STALLSENT)
csr_clear_flags(*csr, (AT91C_UDP_STALLSENT | AT91C_UDP_FORCESTALL));
/* ctrl request packet? */
if (*csr & AT91C_UDP_RXSETUP) {
struct usb_ctrlrequest req;
uint8_t *p;
for (p = (uint8_t *)&req; p < (uint8_t *)(&req +1); p++)
*p = AT91C_UDP_FDR[ep];
/* ack bank0 *now */
if (*csr & AT91C_UDP_RX_DATA_BK0)
csr_clear_flags(*csr, AT91C_UDP_RX_DATA_BK0);
/* set data phase transfer direction */
if (req.bRequestType & USB_DIR_IN)
*csr |= AT91C_UDP_DIR;
/* clear interrupt - *MUST* use csr_clear_flags() here */
csr_clear_flags(*csr, AT91C_UDP_RXSETUP);
ep_handle_ctrlrequest(&req);
}
void (* transfer_cb)(void) = NULL;
/* transmit complete? */
if (*csr & AT91C_UDP_TXCOMP) {
struct ep_ctx *ctx = &ep_ctx[ep];
if (ctx->flags & CTX_FIFO) {
/* get data from fifo */
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;
if (transfer->length != transfer->curpos) {
uint32_t maxsize = ctx->maxpktsize;
/* get data from transfer */
while (transfer->curpos < transfer->length && maxsize--)
AT91C_UDP_FDR[ep] = transfer->data[transfer->curpos++];
/* trigger tx */
AT91C_UDP_CSR[ep] |= AT91C_UDP_TXPKTRDY;
/* transfer complete, execute callback */
} else {
ctx->flags &= ~CTX_IN;
transfer_cb = transfer->complete_cb;
}
}
/* clear interrupt */
*csr &= ~(AT91C_UDP_TXCOMP);
}
/* data ready to read? */
if (*csr & (AT91C_UDP_RX_DATA_BK0 | AT91C_UDP_RX_DATA_BK1)) {
struct ep_ctx *ctx = &ep_ctx[ep];
uint16_t len = (*csr & AT91C_UDP_RXBYTECNT) >> 16;
// TODO: only ep0 status OUT?
if (!len && (ctx->flags & CTX_TRANSFER)) {
ctx->flags &= ~(CTX_OUT | CTX_IN);
ctx->transfer->length = 0;
ctx->transfer->curpos = 0;
}
if (ctx->flags & CTX_FIFO) {
fifo_rxudp(ctx->fifo, ep, len);
} else if ((ctx->flags & (CTX_TRANSFER | CTX_OUT)) == (CTX_TRANSFER | CTX_OUT)) {
/* transfer not complete */
struct ep_transfer *transfer = ctx->transfer;
if (transfer->length != transfer->curpos) {
/* get data from transfer */
while (transfer->curpos < transfer->length && len--)
transfer->data[transfer->curpos++] = AT91C_UDP_FDR[ep];
}
/* test again */
if (transfer->length == transfer->curpos) {
ctx->flags &= ~CTX_OUT;
transfer_cb = transfer->complete_cb;
}
}
if (ctx->flags & CTX_RXBANK0) {
if (*csr & AT91C_UDP_RX_DATA_BK0)
csr_clear_flags(*csr, AT91C_UDP_RX_DATA_BK0);
/* all but ep0 have ping pong buffers */
if (ep > 0)
ctx->flags = (ctx->flags & ~CTX_RXBANK0) | CTX_RXBANK1;
} else if (ctx->flags & CTX_RXBANK1) {
if (*csr & AT91C_UDP_RX_DATA_BK1)
csr_clear_flags(*csr, AT91C_UDP_RX_DATA_BK1);
ctx->flags = (ctx->flags & ~CTX_RXBANK1) | CTX_RXBANK0;
}
}
if (transfer_cb)
transfer_cb();
}
static void udp_isr(void)
{
uint32_t isr = *AT91C_UDP_ISR;
if (isr & AT91C_UDP_ENDBUSRES) {
AT91S_UDP *udp = AT91C_BASE_UDP;
/* reset all endpoints */
udp->UDP_RSTEP = (AT91C_UDP_EP0 | AT91C_UDP_EP1 |
AT91C_UDP_EP2 | AT91C_UDP_EP3) ;
udp->UDP_RSTEP = 0;
/* init ep0 */
struct ep_ctx *ctx = &ep_ctx[0];
ctx->maxpktsize = 8;
ctx->flags = CTX_TRANSFER | CTX_RXBANK0;
ctx->transfer = &ep0_transfer;
ctx->transfer->length = 0;
ctx->transfer->curpos = 0;
/* Configure endpoint0 as Control EP */
udp->UDP_CSR[0] = (AT91C_UDP_EPEDS | AT91C_UDP_EPTYPE_CTRL);
/* enable ep0 Interrupt, disable all others */
udp->UDP_IER = AT91C_UDP_EPINT0;
udp->UDP_IDR = AT91C_UDP_EPINT1 | AT91C_UDP_EPINT2 | AT91C_UDP_EPINT3 |
AT91C_UDP_RXSUSP | AT91C_UDP_RXRSM | AT91C_UDP_SOFINT |
AT91C_UDP_WAKEUP;
if (dfu_status.bStatus == DFU_STATE_appDETACH) {
void (* bootloader)(void) = (void *)0x13c000;
bootloader();
while (1);
}
}
/* Handle Endpoint Interrupts */
uint32_t i;
for (i = 0; i < 4; i++) {
if (isr & *AT91C_UDP_IMR & (1<<i))
udp_handle_ep(i);
}
/* clear all unhandled interrupts */
*AT91C_UDP_ICR = isr & (AT91C_UDP_RXSUSP | AT91C_UDP_RXRSM |
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 */
AT91PS_PIO pio = AT91C_BASE_PIOA;
pio->PIO_CODR = UDP_PULLUP;
pio->PIO_PER = UDP_PULLUP;
pio->PIO_OER = UDP_PULLUP;
/* UDPCK (48MHz) = PLLCK / 2 */
*AT91C_CKGR_PLLR |= AT91C_CKGR_USBDIV_1;
/* enable UDP clocks */
*AT91C_PMC_SCER = AT91C_PMC_UDP;
*AT91C_PMC_PCER = (1 << AT91C_ID_UDP);
/* enable transmitter */
*AT91C_UDP_TXVC &= ~AT91C_UDP_TXVDIS;
/* clear & disable all UDP interrupts */
*AT91C_UDP_IDR = AT91C_UDP_EPINT0 | AT91C_UDP_EPINT1 | AT91C_UDP_EPINT2 |
AT91C_UDP_EPINT3 | AT91C_UDP_RXSUSP | AT91C_UDP_RXRSM |
AT91C_UDP_SOFINT | AT91C_UDP_WAKEUP;
*AT91C_UDP_ICR = AT91C_UDP_RXSUSP | AT91C_UDP_RXRSM | AT91C_UDP_SOFINT |
AT91C_UDP_ENDBUSRES | AT91C_UDP_WAKEUP ;
/* level triggered, own vector */
AT91S_AIC *aic = AT91C_BASE_AIC;
aic->AIC_SMR[AT91C_ID_UDP] = IRQPRIO_UDP | AT91C_AIC_SRCTYPE_INT_HIGH_LEVEL;
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);
}
static void udp_vbus_monitor(uint32_t status, uint32_t input)
{
if (input & UDP_VBUS_MON)
/* usb connected -> enable pullup */
*AT91C_PIOA_CODR = UDP_PULLUP;
else
/* usb got diconnected -> disable pullup */
*AT91C_PIOA_SODR = UDP_PULLUP;
}
PIO_PINCHANGE_ISR(UDP_VBUS_MON, udp_vbus_monitor);

4
src/fifo.c
View File

@ -1,6 +1,4 @@
/***************************************************************************
* sam7fc - FIFOs for use with PDC / USB Hardware *
* *
* Copyright (C) 01/2008 by Olaf Rempel *
* razzor@kopf-tisch.de *
* *
@ -31,7 +29,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
* FIXME: a integer overflow (4gb) of fifo->in / fifo->out could cause trouble
* a integer overflow (4gb) of fifo->in / fifo->out could cause trouble
*/
static uint32_t fifo_used(struct fifo *fifo)
{

264
src/flightctrl.c
View File

@ -1,264 +0,0 @@
/***************************************************************************
* 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,6 +1,4 @@
/***************************************************************************
* sam7fc - dynamic Memory Allocation *
* *
* Copyright (C) 02/2008 by Olaf Rempel *
* razzor@kopf-tisch.de *
* *

44
src/pidctrl.c
View File

@ -1,44 +0,0 @@
/***************************************************************************
* 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 <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 != 0)
out += pid->kp * error;
if (pid->ki != 0) {
pid->err_sum += error;
pid->err_sum = LIMIT(pid->err_sum, pid->err_sum_min, pid->err_sum_max);
out += (pid->err_sum / pid->ki);
}
if (pid->kd != 0) {
out += pid->kd * (error - pid->err_old);
pid->err_old = error;
}
return LIMIT(out, pid->out_min, pid->out_max);
}

308
src/rtos/context.c Normal file
View File

@ -0,0 +1,308 @@
#include <stdint.h>
#include <stdio.h>
#include <string.h>
#include "atomic.h"
#include "memalloc.h"
#include "rtos/context.h"
#include "rtos/spinlock.h"
/* linked list of ready contexts, #1 is the running thread */
struct context *volatile run_queue = NULL;
/* pointer to the running thread */
struct context *current_context = NULL;
/*
* SWI_Handler:
* Entry with SVC mode, IRQs are disabled, FIQ is enabled
*/
__attribute__((naked)) void SWI_Handler(void)
{
/* store register context to current_context */
asm volatile (
/* r0-1 -> svc_stack */
"stmdb sp!, {r0-r1} \n\t"
/* get top of struct register_context */
"ldr r0, =current_context \n\t"
"ldr r0, [r0] \n\t"
"add r0, r0, #68 \n\t"
/* save usermode cpsr & r2-14 */
"mrs r1, spsr \n\t"
"stmdb r0, {r1-r14}^ \n\t"
"nop \n\t"
"sub r0, r0, #56 \n\t"
/* save r0-1 and svc_lr (= pc) */
"ldmia sp!, {r1, r2} \n\t"
"stmdb r0!, {r1, r2, r14} \n\t"
);
/* we're no longer #1 in run_queue, switch to new #1 */
if (current_context != run_queue)
current_context = run_queue;
current_context->state = CONTEXT_RUNNING;
if (((uint32_t *)current_context->stack)[0] != 0xdeadbeef)
printf("<- task stack corrupt (%p)\n\r", current_context);
/* restore register context from current_context */
asm volatile (
/* get pointer to struct register_context */
"ldr r0, =current_context \n\t"
"ldr r0, [r0] \n\t"
/* get values of r0-1 and pc (= svc_lr) */
"ldmia r0!, {r1-r2,r14} \n\t"
"stmdb sp!, {r1-r2} \n\t"
/* restore usermode cpsr & r2-14 */
"ldmia r0, {r1-r14}^ \n\t"
"nop \n\t"
"msr spsr, r1 \n\t"
/* get r0-1 from svc_stack, jump back */
"ldmia sp!, {r0, r1} \n\t"
"movs pc, lr \n\t"
);
}
static uint8_t isr_context_switch(void)
{
asm volatile ("swi");
// TODO: return previous state of now running thread
return CONTEXT_READY;
}
/* inserts context into run_queue */
static void isr_context_ready(struct context *ctx)
{
struct context *q = run_queue;
struct context *volatile *qprev = &run_queue;
while (q && (q->priority <= ctx->priority)) {
qprev = &q->run_queue;
q = q->run_queue;
}
ctx->run_queue = q;
*qprev = ctx;
}
/* process yields, try to switch to process with lower or same prio */
void isr_context_yield(void)
{
run_queue = current_context->run_queue;
isr_context_ready(current_context);
if (current_context != run_queue) {
current_context->state = CONTEXT_READY;
isr_context_switch();
}
}
void context_yield(void)
{
disable_irqs();
isr_context_yield();
restore_irqs();
}
static uint8_t __isr_context_wait(struct spinlock *lock, uint8_t sleepstate)
{
isr_spinlock_unlock(lock);
run_queue = current_context->run_queue;
current_context->state = sleepstate;
uint8_t retval = isr_context_switch();
isr_spinlock_lock(lock);
return retval;
}
uint8_t isr_context_wait(struct spinlock *lock)
{
return __isr_context_wait(lock, CONTEXT_SLEEP);
}
uint8_t context_wait(struct spinlock *lock)
{
disable_irqs();
uint8_t retval = __isr_context_wait(lock, CONTEXT_SLEEP);
restore_irqs();
return retval;
}
uint8_t context_wait_queue(struct spinlock *lock, struct context **queue)
{
disable_irqs();
current_context->sleep_queue = *queue;
*queue = current_context;
uint8_t retval = __isr_context_wait(lock, CONTEXT_SLEEP_QUEUE);
restore_irqs();
return retval;
}
uint8_t context_wait_pri_queue(struct spinlock *lock, struct context **queue)
{
disable_irqs();
struct context *q = *queue;
while (q && (q->priority <= lock->priority_unlocked)) {
queue = &q->sleep_queue;
q = q->sleep_queue;
}
current_context->sleep_queue = q;
*queue = current_context;
uint8_t retval = __isr_context_wait(lock, CONTEXT_SLEEP_QUEUE);
restore_irqs();
return retval;
}
void isr_context_signal(struct context *c)
{
if (c->state == CONTEXT_SLEEP) {
c->state = CONTEXT_READY;
isr_context_ready(c);
}
}
void context_signal(struct context *c)
{
disable_irqs();
isr_context_signal(c);
restore_irqs();
}
uint32_t context_signal_queue(struct context **queue)
{
disable_irqs();
uint32_t retval = 0;
if (*queue) {
struct context *c = *queue;
*queue = c->sleep_queue;
c->state = CONTEXT_READY;
isr_context_ready(c);
retval = 1;
}
restore_irqs();
return retval;
}
void isr_context_interrupt(struct context *c)
{
if (c->state == CONTEXT_SLEEP) {
c->state = CONTEXT_INTERRUPTED;
isr_context_ready(c);
}
}
void context_interrupt(struct context *c)
{
disable_irqs();
isr_context_signal(c);
restore_irqs();
}
uint32_t context_interrupt_queue(struct context *c, struct context **queue)
{
disable_irqs();
struct context *q = *queue;
while (q && (q != c)) {
queue = &q->sleep_queue;
q = q->sleep_queue;
}
uint32_t retval = 0;
if (q) {
*queue = c->sleep_queue;
c->state = CONTEXT_INTERRUPTED;
isr_context_ready(c);
retval = 1;
}
restore_irqs();
return retval;
}
struct context * create_ctx(uint32_t stacksize, uint8_t priority, void (* code)(void *arg), void *arg)
{
uint32_t *stack = static_alloc(sizeof(struct context) + stacksize);
memset(stack, 0, stacksize);
// TODO: check on context-switch for corruption */
stack[0] = 0xdeadbeef;
struct context *ctx = (struct context *)((uint8_t *)stack + stacksize);
ctx->regs.r0 = (uint32_t)arg;
ctx->regs.pc = (uint32_t)code;
ctx->regs.cpsr = 0x0000001F;
ctx->regs.sp = (uint32_t)ctx;
ctx->regs.r1 = 0x01010101;
ctx->regs.r2 = 0x02020202;
ctx->regs.r3 = 0x03030303;
ctx->regs.r4 = 0x04040404;
ctx->regs.r5 = 0x05050505;
ctx->regs.r6 = 0x06060606;
ctx->regs.r7 = 0x07070707;
ctx->regs.r8 = 0x08080808;
ctx->regs.r9 = 0x09090909;
ctx->regs.r10 = 0x10101010;
ctx->regs.r11 = 0x11111111;
ctx->regs.r12 = 0x12121212;
ctx->regs.r14 = 0x14141414;
ctx->stack = stack;
ctx->priority = priority;
disable_irqs();
isr_context_ready(ctx);
restore_irqs();
return ctx;
}
void init_context(void)
{
/*
* create shallow idle context
* idle context runs in SVC mode, so no real stack is needed
*/
current_context = create_ctx(4, 255, NULL, NULL);
printf("idle_ctx=%p\n\r", current_context);
/* For now, we're the only thread, so this simply safes our context */
disable_irqs();
isr_context_switch();
restore_irqs();
/* idle loop */
while (1) {
// TODO: this sucks, context switch after interrupt are way better..
context_yield();
}
}

56
src/rtos/semaphore.c Normal file
View File

@ -0,0 +1,56 @@
#include <stdint.h>
#include "rtos/context.h"
#include "rtos/semaphore.h"
#include "rtos/spinlock.h"
uint8_t sem_wait(struct semaphore *sem)
{
uint32_t retval = CONTEXT_READY;
spinlock_lock(&sem->lock);
sem->count--;
if (sem->count < 0)
retval = context_wait_pri_queue(&sem->lock, &sem->sleep_queue);
spinlock_unlock(&sem->lock);
return retval;
}
void sem_post(struct semaphore *sem)
{
spinlock_lock(&sem->lock);
sem->count++;
if (sem->count <= 0)
context_signal_queue(&sem->sleep_queue);
spinlock_unlock(&sem->lock);
}
void sem_interrupt(struct semaphore *sem, struct context *c)
{
spinlock_lock(&sem->lock);
context_interrupt_queue(c, &sem->sleep_queue);
spinlock_unlock(&sem->lock);
}
int32_t sem_get_count(struct semaphore *sem)
{
int32_t retval;
spinlock_lock(&sem->lock);
retval = sem->count;
spinlock_unlock(&sem->lock);
return retval;
}
void sem_init(struct semaphore *sem, int32_t count)
{
sem->sleep_queue = 0;
spinlock_init(&sem->lock);
sem->count = 0;
}

40
src/rtos/spinlock.c Normal file
View File

@ -0,0 +1,40 @@
#include <stdint.h>
#include "atomic.h"
#include "rtos/context.h"
#include "rtos/spinlock.h"
void isr_spinlock_lock(struct spinlock *lock)
{
lock->priority_unlocked = current_context->priority;
current_context->priority = 0;
lock->locked = 1;
}
void spinlock_lock(struct spinlock *lock)
{
disable_irqs()
isr_spinlock_lock(lock);
restore_irqs();
}
void isr_spinlock_unlock(struct spinlock *lock)
{
current_context->priority = lock->priority_unlocked;
lock->locked = 0;
// TODO: check for context-switch...
}
void spinlock_unlock(struct spinlock *lock)
{
disable_irqs()
isr_spinlock_unlock(lock);
restore_irqs();
}
void spinlock_init(struct spinlock *lock)
{
lock->locked = 0;
}

338
src/telemetrie.c
View File

@ -1,338 +0,0 @@
/***************************************************************************
* sam7fc - Telemetrie Handling *
* *
* 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 <stdint.h>
#include <string.h>
#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;
/* max. 8x 32 = 256 variables */
static uint32_t tdc_varmap[8];
/* array of devices, that are used to reach address X */
static struct comm_device *routing_table[8];
/*
* returns:
* -1: on routing error
* 0: no space left in txfifo (caller should retry)
* >0: success
*/
int32_t tdc_transmit(uint32_t addr, struct tdc_pkt_header *head)
{
if (addr >= ARRAY_SIZE(routing_table) || !routing_table[addr])
return -1;
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)
{
/* restart point */
static uint32_t id;
struct tdc_value *value = &_tdc_value_table + id;
while (value < &_tdc_value_table_end) {
uint32_t datalen = strlen(value->name);
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 & 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));
free(reply);
/* push routing error(-1) and retry(0) */
if (ret <= 0)
return ret;
id++;
value++;
}
/* dump complete, reset restart point */
id = 0;
return 1;
}
static int32_t tdc_get_value(uint32_t id)
{
struct tdc_value *value = &_tdc_value_table + id;
if (value >= &_tdc_value_table_end)
return -1;
uint32_t datalen = value->flags & TDC_SIZEMASK;
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_getvalue_reply) + datalen;
reply->id = id;
memcpy(reply->data, value->data, datalen);
int32_t ret = tdc_transmit(TDC_ADDR0, ((struct tdc_pkt_header *)reply));
free(reply);
return ret;
}
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 len = value->flags & TDC_SIZEMASK;
if (len != data_size)
return -1;
// TODO: atomic?
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 bitmask = tdc_varmap[i];
for (j = 0; j < 32; j++) {
if (!bitmask)
break;
if (bitmask & 0x01) {
if (tdc_get_value(i * 32 + j) < 0)
tdc_varmap[i] &= ~(1 << j);
}
bitmask >>= 1;
}
}
return PITC_RESTART_TIMER;
}
static struct pitc_timer tdc_timer = {
.func = tdc_timer_cb,
};
static int32_t tdc_setup_timer(uint32_t interval, uint32_t *varmap)
{
memcpy(tdc_varmap, varmap, sizeof(tdc_varmap));
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;
pitc_schedule_timer(&tdc_timer);
} else {
pitc_remove_timer(&tdc_timer);
}
return 1;
}
static const struct tdc_hello_reply hello_reply = {
.cmd = TDC_REPLY | TDC_OWN_ADDRESS | TDC_HELLO,
.size = sizeof(struct tdc_hello_reply),
.name = "sam7fc-v0.01",
};
void tdc_register_device(uint32_t addr, struct comm_device *device)
{
if (addr < ARRAY_SIZE(routing_table))
routing_table[addr] = device;
}
struct tdc_pkt_header * tdc_alloc_fullpkt(struct comm_device *device, uint32_t size)
{
struct tdc_pkt_header *head = alloc(size);
/* peek the whole packet */
uint32_t len = fifo_peek(device->rxfifo, (char *)head, size);
if (len != size) {
free(head);
head = NULL;
}
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);
}
/* 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);
return ret;
}
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);
}