Compare commits

...

13 Commits
rtos ... master

35 changed files with 1623 additions and 641 deletions

View File

@ -10,8 +10,8 @@ SIZE = $(TOOLCHAIN)/bin/arm-elf-size
OBJCOPY = $(TOOLCHAIN)/bin/arm-elf-objcopy OBJCOPY = $(TOOLCHAIN)/bin/arm-elf-objcopy
OBJDUMP = $(TOOLCHAIN)/bin/arm-elf-objdump OBJDUMP = $(TOOLCHAIN)/bin/arm-elf-objdump
INCDIRS = include $(TOOLCHAIN)/lib/gcc/arm-elf/4.1.1/include $(TOOLCHAIN)/arm-elf/include INCDIRS = include $(TOOLCHAIN)/lib/gcc/arm-elf/4.3.3/include $(TOOLCHAIN)/arm-elf/include
LIBDIRS = $(TOOLCHAIN)/arm-elf/lib $(TOOLCHAIN)/lib/gcc/arm-elf/4.1.1 LIBDIRS = $(TOOLCHAIN)/arm-elf/lib $(TOOLCHAIN)/lib/gcc/arm-elf/4.3.3
# ------ # ------
@ -73,6 +73,7 @@ openocd:
$(shell $(OPENOCD) -f scripts/openocd.cfg) $(shell $(OPENOCD) -f scripts/openocd.cfg)
install: all install: all
./scripts/download.sh # ./scripts/download.sh
$(TOOLCHAIN)/dfu-util -i 0 -D $(BUILD)/$(TARGET).elf.bin
-include $(shell find $(BUILD) -name *.d 2> /dev/null) -include $(shell find $(BUILD) -name *.d 2> /dev/null)

View File

@ -1,4 +1,6 @@
/*************************************************************************** /***************************************************************************
* sam7fc - stack/bss/data init *
* *
* Copyright (C) 01/2008 by Olaf Rempel * * Copyright (C) 01/2008 by Olaf Rempel *
* razzor@kopf-tisch.de * * razzor@kopf-tisch.de *
* * * *
@ -16,13 +18,10 @@
* Free Software Foundation, Inc., * * Free Software Foundation, Inc., *
* 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA. * * 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 AT91C_BASE_AIC, (0xFFFFF000)
.equ IRQ_Stack_Size, (3 * 8 * 4) .equ IRQ_Stack_Size, (3 * 8 * 4)
.equ FIQ_Stack_Size, (3 * 8 * 4) .equ FIQ_Stack_Size, (0 * 8 * 4)
.equ ABT_Stack_Size, 192 .equ ABT_Stack_Size, 192
.equ ARM_MODE_FIQ, 0x11 .equ ARM_MODE_FIQ, 0x11
@ -46,15 +45,19 @@ rsvdvec: ldr pc, [pc, #24] /* 0x14 reserved */
irqvec: ldr pc, [pc, #24] /* 0x18 IRQ */ irqvec: ldr pc, [pc, #24] /* 0x18 IRQ */
fiqvec: ldr pc, [pc, #24] /* 0x1c FIQ */ fiqvec: ldr pc, [pc, #24] /* 0x1c FIQ */
.extern ABT_Handler
.extern IRQ_Handler
.extern FIQ_Handler
/* 0x80000000 will result in Prefetch Abort */ /* 0x80000000 will result in Prefetch Abort */
.word InitReset .word InitReset
.word 0x80000000 .word 0x80000000
.word 0x80000000 .word 0x80000000
.word ABT_Handler_Entry .word ABT_Handler
.word ABT_Handler_Entry .word ABT_Handler
.word 0x80000000 .word 0x80000000
.word IRQ_Handler_Entry .word IRQ_Handler
.word FIQ_Handler_Entry .word FIQ_Handler
.endfunc .endfunc
@ -74,7 +77,7 @@ InitReset:
mov sp, r0 mov sp, r0
sub r0, r0, #FIQ_Stack_Size sub r0, r0, #FIQ_Stack_Size
/* store AIC Base in ARM_MODE_FIQ:r8 for faster access */ /* store AIC Base in r8_fiq for faster access */
ldr r8, =AT91C_BASE_AIC ldr r8, =AT91C_BASE_AIC
/* Setup IRQ Mode Stack */ /* Setup IRQ Mode Stack */
@ -120,122 +123,3 @@ LoopZI: cmp r1, r2
/* exit dummy for newlib */ /* exit dummy for newlib */
exit: b . exit: b .
.endfunc .endfunc
.global ABT_Handler_Entry
.func ABT_Handler_Entry
ABT_Handler_Entry:
/* disable interrupts (F_BIT not set on entry) */
msr CPSR_c, #ARM_MODE_ABT | I_BIT | F_BIT
/* store all registers */
stmfd sp!, { r0-r12 }
/* saved cpsr (from aborted mode) */
mrs r0, SPSR
/* address of abort (pc) */
mov r3, lr
/* enter previous mode and get lr(r14), sp(r13) */
/* TODO: interrupts might be enabled? */
/* TODO: thumb mode enabled? */
msr CPSR_c, r0
mov r1, sp
mov r2, lr
/* return to abort mode */
msr CPSR_c, #ARM_MODE_ABT | I_BIT | F_BIT
/* store remaining registers (r1-r3 == r13-r15) */
stmfd sp!, { r1-r3 }
mov r1, sp
/* execute C Handler (cpsr, registers) */
ldr r5, =at91_abt_handler
mov lr, pc
bx r5
b .
.endfunc
.global FIQ_Handler_Entry
.func FIQ_Handler_Entry
FIQ_Handler_Entry:
/* 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_SVC | 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
.global IRQ_Handler_Entry
.func IRQ_Handler_Entry
IRQ_Handler_Entry:
/* Manage Exception Entry */
/* Adjust and save LR_irq in IRQ stack */
sub lr, lr, #4
stmfd sp!, { lr }
/* Save SPSR need to be saved for nested interrupt */
mrs r14, SPSR
stmfd sp!, { r14 }
/* Save and r0 in IRQ stack */
stmfd sp!, { r0 }
/* Write in the IVR to support Protect Mode */
/* No effect in Normal Mode */
/* De-assert the NIRQ and clear the source in Protect Mode */
ldr r14, =AT91C_BASE_AIC
ldr r0, [r14, #AIC_IVR]
str r14, [r14, #AIC_IVR]
/* Enable Interrupt and Switch in Supervisor Mode */
msr CPSR_c, #ARM_MODE_SVC
/* Save scratch/used registers and LR in User Stack */
stmfd sp!, { r1-r3, r12, r14 }
/* Branch to the routine pointed by the AIC_IVR */
mov r14, pc
bx r0
/* Restore scratch/used registers and LR from User Stack */
ldmia sp!, { r1-r3, r12, r14 }
/* Disable Interrupt and switch back in IRQ mode */
msr CPSR_c, #ARM_MODE_IRQ | I_BIT
/* Mark the End of Interrupt on the AIC */
ldr r14, =AT91C_BASE_AIC
str r14, [r14, #AIC_EOICR]
/* Restore SPSR_irq and r0 from IRQ stack */
ldmia sp!, { r0 }
/* Restore SPSR_irq and r0 from IRQ stack */
ldmia sp!, { r14 }
msr SPSR_cxsf, r14
/* Restore adjusted LR_irq from IRQ stack directly in the PC */
ldmia sp!, { pc }^
.endfunc
.end

View File

@ -1,4 +1,6 @@
/*************************************************************************** /***************************************************************************
* sam7fc - PLL Init, IRQ/FIQ Vectors *
* *
* Copyright (C) 01/2008 by Olaf Rempel * * Copyright (C) 01/2008 by Olaf Rempel *
* razzor@kopf-tisch.de * * razzor@kopf-tisch.de *
* * * *
@ -79,3 +81,96 @@ void at91_init1(void)
} }
aic->AIC_SPU = (uint32_t)empty_isr; aic->AIC_SPU = (uint32_t)empty_isr;
} }
__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 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 I_BIT, 0x80 \n\t"
".equ F_BIT, 0x40 \n\t"
".equ AIC_FVR, (260) \n\t"
/* Save r0 to r9_fiq */
"mov r9, r0 \n\t"
/* get FIQ Vector from AIC and thus clear FIQ */
"ldr r0, [r8, #AIC_FVR] \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"
/* execute FIQ in SVC_MODE */
"mov r14, pc \n\t"
"bx r0 \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"
/* restore the r0 from r9_fiq */
"mov r0, r9 \n\t"
/* restore PC using the lr_fiq directly */
"subs pc, lr, #4 \n\t"
);
}

Binary file not shown.

Binary file not shown.

BIN
eagle/sam7fc-bot.png Normal file

Binary file not shown.

After

Width:  |  Height:  |  Size: 41 KiB

BIN
eagle/sam7fc-sch.png Normal file

Binary file not shown.

After

Width:  |  Height:  |  Size: 203 KiB

BIN
eagle/sam7fc-top.png Normal file

Binary file not shown.

After

Width:  |  Height:  |  Size: 36 KiB

32
include/at91_adc.h Normal file
View File

@ -0,0 +1,32 @@
#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_ */

View File

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

View File

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

View File

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

View File

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

View File

@ -1,14 +1,14 @@
#ifndef AT91TWI_H_ #ifndef AT91TWI_H_
#define AT91TWI_H_ #define AT91TWI_H_
#include <list.h> #include <stdint.h>
/* TWI slave addresses */ /* TWI slave addresses */
#define TWI_ADDR_BL1 0x21 #define TWI_ADDR_BL1 0x21
#define TWI_ADDR_BL2 0x22 #define TWI_ADDR_BL2 0x22
#define TWI_ADDR_BL3 0x23 #define TWI_ADDR_BL3 0x23
#define TWI_ADDR_BL4 0x24 #define TWI_ADDR_BL4 0x24
#define TWI_ADDR_EEPROM 0x40 #define TWI_ADDR_EEPROM 0x50
/* TWIBOOT commands */ /* TWIBOOT commands */
#define CMD_WAIT 0x00 #define CMD_WAIT 0x00
@ -25,31 +25,72 @@
//#define CMD_GET_INFO 0x10 //#define CMD_GET_INFO 0x10
#define CMD_SET_PWM 0x21 #define CMD_SET_PWM 0x21
#define CMD_GET_STATUS 0x22 #define CMD_GET_STATUS 0x22
//#define CMD_SET_PARAM 0x23 #define CMD_SET_PARAM 0x23
//#define CMD_GET_PARAM 0x24 #define CMD_GET_PARAM 0x24
#define CMD_BOOT_LOADER 0x2F #define CMD_BOOT_LOADER 0x2F
struct blmc_status {
uint8_t pwm_ist;
uint8_t pwm_soll;
struct blmc_cmd { 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 {
uint32_t cmd; /* cmd byte(s) */ uint32_t cmd; /* cmd byte(s) */
uint8_t mode; /* read/write, cmdlen (1-3 bytes) */ uint8_t mode; /* read/write, cmdlen (1-3 bytes) */
uint8_t size; /* data size */ uint16_t size; /* data size */
uint8_t *data; /* read/write data */ uint8_t *data; /* read/write data */
}; };
/* same bits as TWI_MMR[8..15] */ /* same bits as TWI_MMR[8..15] */
#define BLMC_CMD_READ 0x10 #define TWI_MODE_READ 0x10
#define BLMC_CMD_WRITE 0x00 #define TWI_MODE_WRITE 0x00
#define BLMC_CMD_0_ARG 0x01 #define TWI_MODE_0_ARG 0x01
#define BLMC_CMD_1_ARG 0x02 #define TWI_MODE_1_ARG 0x02
#define BLMC_CMD_2_ARG 0x03 #define TWI_MODE_2_ARG 0x03
uint32_t twi_read_eeprom(uint32_t addr, uint8_t *buf, uint32_t size); 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_write_eeprom(uint32_t addr, uint8_t *buf, uint32_t size);
uint32_t twi_setpwm(uint8_t *values); uint32_t twi_setpwm(uint8_t *values);
uint32_t twi_cmd(uint8_t addr, struct blmc_cmd *cmd); uint32_t twi_cmd(uint8_t addr, struct twi_cmd *cmd);
void at91_twi_init(void); void at91_twi_init(void);
void at91_twi_test(void); void at91_twi_test(void);

View File

@ -1,6 +1,20 @@
#ifndef AT91_UDP_H_ #ifndef AT91_UDP_H_
#define 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); void at91_udp_init(void);
#endif /*AT91_UDP_H_*/ #endif /*AT91_UDP_H_*/

View File

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

23
include/pidctrl.h Normal file
View File

@ -0,0 +1,23 @@
#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_ */

103
include/tdc_proto.h Normal file
View File

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

View File

@ -1,108 +1,19 @@
#ifndef TELEMETRIE_H_ #ifndef TELEMETRIE_H_
#define 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 comm_device {
struct fifo *rxfifo; struct fifo *rxfifo;
struct fifo *txfifo; struct fifo *txfifo;
void (*trigger_tx)(void);
}; };
void tdc_register_device(uint32_t addr, struct comm_device *device); void tdc_register_device(uint32_t addr, struct comm_device *device);
int32_t tdc_transmit(uint32_t addr, struct tdc_pkt_header *head); int32_t tdc_transmit(uint32_t addr, struct tdc_pkt_header *head);
void tdc_receive(struct comm_device *device); void tdc_check(void);
void tdc_init(void);
struct tdc_value { struct tdc_value {
void *data; void *data;
@ -110,26 +21,45 @@ struct tdc_value {
uint32_t flags; uint32_t flags;
}; };
#define TDC_SIZEMASK 0x0F #if 1
#define TDC_UNSIGNED 0x00
#define TDC_SIGNED 0x10
#define TDC_FP 0x20
#define TDC_VALUE(var, desc, type, flags) \ #define TDC_VALUE(name, var, desc, type, flags) \
type * tdc_check_##var(void) { return (&var); } \ type * tdc_check_##name(void) { return (&var); } \
static struct tdc_value __attribute__((used, section(".tdc_value"))) \ static struct tdc_value __attribute__((used, section(".tdc_value"))) \
tdc_value_##var = { &var, desc, sizeof(type) | flags }; \ tdc_value_##name = { &var, desc, sizeof(type) | flags }; \
tdc_value_##var = tdc_value_##var; tdc_value_##name = tdc_value_##name;
#define TDC_UINT8(var, desc) TDC_VALUE(var, desc, uint8_t, TDC_UNSIGNED) #define TDC_PTR(name, ptr, desc, type, flags) \
#define TDC_UINT16(var, desc) TDC_VALUE(var, desc, uint16_t, TDC_UNSIGNED) static struct tdc_value __attribute__((used, section(".tdc_value"))) \
#define TDC_UINT32(var, desc) TDC_VALUE(var, desc, uint32_t, TDC_UNSIGNED) tdc_value_##name = { ptr, desc, sizeof(type) | flags }; \
#define TDC_UINT64(var, desc) TDC_VALUE(var, desc, uint64_t, TDC_UNSIGNED)
#define TDC_INT8(var, desc) TDC_VALUE(var, desc, int8_t, TDC_SIGNED) #else
#define TDC_INT16(var, desc) TDC_VALUE(var, desc, int16_t, TDC_SIGNED)
#define TDC_INT32(var, desc) TDC_VALUE(var, desc, int32_t, TDC_SIGNED) #define TDC_VALUE(name, var, desc, type, flags)
#define TDC_INT64(var, desc) TDC_VALUE(var, desc, int64_t, TDC_SIGNED) #define TDC_PTR(name, ptr, desc, type, flags);
#define TDC_FLOAT(var, desc) TDC_VALUE(var, desc, float, TDC_FP)
#define TDC_DOUBLE(var, desc) TDC_VALUE(var, desc, double, TDC_FP) #endif
#define TDC_UINT8(var, desc) TDC_VALUE(var, var, desc, uint8_t, TDC_UNSIGNED)
#define TDC_UINT16(var, desc) TDC_VALUE(var, var, desc, uint16_t, TDC_UNSIGNED)
#define TDC_UINT32(var, desc) TDC_VALUE(var, var, desc, uint32_t, TDC_UNSIGNED)
#define TDC_UINT64(var, desc) TDC_VALUE(var, var, desc, uint64_t, TDC_UNSIGNED)
#define TDC_INT8(var, desc) TDC_VALUE(var, var, desc, int8_t, TDC_SIGNED)
#define TDC_INT16(var, desc) TDC_VALUE(var, var, desc, int16_t, TDC_SIGNED)
#define TDC_INT32(var, desc) TDC_VALUE(var, var, desc, int32_t, TDC_SIGNED)
#define TDC_INT64(var, desc) TDC_VALUE(var, var, desc, int64_t, TDC_SIGNED)
#define TDC_FLOAT(var, desc) TDC_VALUE(var, var, desc, float, TDC_FP)
#define TDC_DOUBLE(var, desc) TDC_VALUE(var, var, desc, double, TDC_FP)
#define TDC_UINT8_RO(var, desc) TDC_VALUE(var, var, desc, uint8_t, TDC_UNSIGNED | TDC_READONLY)
#define TDC_UINT16_RO(var, desc) TDC_VALUE(var, var, desc, uint16_t, TDC_UNSIGNED | TDC_READONLY)
#define TDC_UINT32_RO(var, desc) TDC_VALUE(var, var, desc, uint32_t, TDC_UNSIGNED | TDC_READONLY)
#define TDC_UINT64_RO(var, desc) TDC_VALUE(var, var, desc, uint64_t, TDC_UNSIGNED | TDC_READONLY)
#define TDC_INT8_RO(var, desc) TDC_VALUE(var, var, desc, int8_t, TDC_SIGNED | TDC_READONLY)
#define TDC_INT16_RO(var, desc) TDC_VALUE(var, var, desc, int16_t, TDC_SIGNED | TDC_READONLY)
#define TDC_INT32_RO(var, desc) TDC_VALUE(var, var, desc, int32_t, TDC_SIGNED | TDC_READONLY)
#define TDC_INT64_RO(var, desc) TDC_VALUE(var, var, desc, int64_t, TDC_SIGNED | TDC_READONLY)
#define TDC_FLOAT_RO(var, desc) TDC_VALUE(var, var, desc, float, TDC_FP | TDC_READONLY)
#define TDC_DOUBLE_RO(var, desc) TDC_VALUE(var, var, desc, double, TDC_FP | TDC_READONLY)
#endif /*TELEMETRIE_H_*/ #endif /*TELEMETRIE_H_*/

63
main.c
View File

@ -1,4 +1,6 @@
/*************************************************************************** /***************************************************************************
* sam7fc - main loop *
* *
* Copyright (C) 01/2008 by Olaf Rempel * * Copyright (C) 01/2008 by Olaf Rempel *
* razzor@kopf-tisch.de * * razzor@kopf-tisch.de *
* * * *
@ -16,10 +18,16 @@
* Free Software Foundation, Inc., * * Free Software Foundation, Inc., *
* 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA. * * 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA. *
***************************************************************************/ ***************************************************************************/
#include <stdio.h>
#include <string.h>
#include "AT91SAM7S256.h" #include "AT91SAM7S256.h"
#include "board.h"
#include "at91_sysc.h" #include "at91_sysc.h"
#include "at91_dbgu.h" #include "at91_dbgu.h"
#include "at91_pitc.h" #include "at91_pitc.h"
#include "at91_adc.h"
#include "at91_tests.h" #include "at91_tests.h"
#include "at91_udp.h" #include "at91_udp.h"
#include "at91_pio.h" #include "at91_pio.h"
@ -27,34 +35,25 @@
#include "at91_tc1.h" #include "at91_tc1.h"
#include "memalloc.h" #include "memalloc.h"
#include "telemetrie.h"
#include "board.h" extern void base_ctrl(void);
#include <stdio.h>
#include <string.h>
static uint32_t pitc_test(struct pitc_timer *timer) volatile static uint32_t run_ctrl;
static uint32_t base_ctrl_trigger(struct pitc_timer *timer)
{ {
static uint32_t i; /* trigger adc, after ~15us () result should be ready */
*AT91C_PIOA_SODR = i; adc_trigger();
i = i ^ LED_GREEN;
*AT91C_PIOA_CODR = i;
/*
struct rc_values rc;
uint32_t count = rcontrol_getvalues(&rc);
printf("%ld channels: ", count); run_ctrl = 1;
uint32_t j;
for (j = 0; j < count; j++)
printf("%+5d ", rc.chan[j]);
printf("\r");
*/
return PITC_RESTART_TIMER; return PITC_RESTART_TIMER;
} }
static struct pitc_timer pitc_test_timer = { static struct pitc_timer base_timer = {
.interval = 10, .interval = 2,
.func = &pitc_test, .func = &base_ctrl_trigger,
}; };
int main(void) int main(void)
@ -75,21 +74,33 @@ int main(void)
/* timer */ /* timer */
at91_pitc_init(); at91_pitc_init();
at91_rttc_test_init(); at91_rttc_test_init();
at91_tc1_init();
/* adc, need timer */
at91_adc_test_init();
/* twi */ /* twi */
at91_twi_init(); at91_twi_init();
at91_twi_test(); at91_twi_test();
/* remote control, needs twi */
at91_tc1_init();
/* usb */ /* usb */
at91_udp_init(); 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()); printf("static alloc: %5ld bytes\n\r", static_alloc_used());
pitc_schedule_timer(&pitc_test_timer); while (1) {
if (run_ctrl) {
// TODO: racy?
run_ctrl = 0;
while (1); base_ctrl();
}
tdc_check();
}
} }

View File

@ -1,4 +1,6 @@
/*************************************************************************** /***************************************************************************
* sam7fc - ADC routines / calibration *
* *
* Copyright (C) 01/2008 by Olaf Rempel * * Copyright (C) 01/2008 by Olaf Rempel *
* razzor@kopf-tisch.de * * razzor@kopf-tisch.de *
* * * *
@ -18,38 +20,181 @@
***************************************************************************/ ***************************************************************************/
#include <stdint.h> #include <stdint.h>
#include <stdio.h> #include <stdio.h>
#include "AT91SAM7S256.h" #include "AT91SAM7S256.h"
#include "board.h" #include "board.h"
#include "at91_adc.h"
#include "at91_pitc.h" #include "at91_pitc.h"
#include "at91_twi.h"
#include "telemetrie.h"
static uint16_t adc_result[4]; 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) static void at91_adc_isr(void)
{ {
AT91S_PDC *pdc = AT91C_BASE_PDC_ADC; AT91S_PDC *pdc = AT91C_BASE_PDC_ADC;
pdc->PDC_RPR = (uint32_t) &adc_result; pdc->PDC_RPR = (uint32_t) &adc_tmp;
pdc->PDC_RCR = ARRAY_SIZE(adc_result); pdc->PDC_RCR = ARRAY_SIZE(adc_tmp);
pdc->PDC_PTCR = AT91C_PDC_RXTEN; pdc->PDC_PTCR = AT91C_PDC_RXTEN;
/* clear interrupts */ /* clear interrupts */
AT91S_ADC *adc = AT91C_BASE_ADC; uint32_t dummy = *AT91C_ADC_SR;
uint32_t status = adc->ADC_SR; dummy = dummy;
status = status;
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_trigger(struct pitc_timer *timer) static uint32_t adc_calibrate_cb(struct pitc_timer *timer)
{ {
*AT91C_ADC_CR = AT91C_ADC_START; if (adc_cal_count < ADC_CAL_COUNT_MAX) {
return PITC_RESTART_TIMER; /* 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_timer = { static struct pitc_timer adc_cal_timer = {
.interval = 100, .interval = 1,
.func = &adc_trigger, .func = &adc_calibrate_cb,
}; };
void at91_adc_test_init(void) 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 */ /* enable ADC clock */
*AT91C_PMC_PCER = (1 << AT91C_ID_ADC); *AT91C_PMC_PCER = (1 << AT91C_ID_ADC);
@ -74,12 +219,12 @@ void at91_adc_test_init(void)
/* setup PDC */ /* setup PDC */
AT91S_PDC *pdc = AT91C_BASE_PDC_ADC; AT91S_PDC *pdc = AT91C_BASE_PDC_ADC;
pdc->PDC_RPR = (uint32_t) &adc_result; pdc->PDC_RPR = (uint32_t) &adc_tmp;
pdc->PDC_RCR = ARRAY_SIZE(adc_result); pdc->PDC_RCR = ARRAY_SIZE(adc_tmp);
pdc->PDC_PTCR = AT91C_PDC_RXTEN; pdc->PDC_PTCR = AT91C_PDC_RXTEN;
/* enable 4 channels, PDC Interrupt */ /* enable 7 channels (0-1-2-4-5-6-7), PDC Interrupt */
adc->ADC_CHER = 0xF0; adc->ADC_CHER = 0xF7;
adc->ADC_IER = AT91C_ADC_ENDRX; adc->ADC_IER = AT91C_ADC_ENDRX;
/* low priority, level triggered, own vector */ /* low priority, level triggered, own vector */
@ -88,5 +233,6 @@ void at91_adc_test_init(void)
aic->AIC_SVR[AT91C_ID_ADC] = (uint32_t)at91_adc_isr; aic->AIC_SVR[AT91C_ID_ADC] = (uint32_t)at91_adc_isr;
aic->AIC_IECR = (1<<AT91C_ID_ADC); aic->AIC_IECR = (1<<AT91C_ID_ADC);
pitc_schedule_timer(&adc_timer); adc_calibrate(ADC_CAL_ACC_LOAD);
adc_calibrate(ADC_CAL_GYRO);
} }

View File

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

View File

@ -1,4 +1,6 @@
/*************************************************************************** /***************************************************************************
* sam7fc - Abort Handler with Register/Stackdump *
* *
* Copyright (C) 01/2008 by Olaf Rempel * * Copyright (C) 01/2008 by Olaf Rempel *
* razzor@kopf-tisch.de * * razzor@kopf-tisch.de *
* * * *
@ -59,8 +61,8 @@ ARM7TDMI 32bit Undefined Address Instruction Fetch Abort
--- Registerdump (cpsr:0xa0000013 - SVC): --- Registerdump (cpsr:0xa0000013 - SVC):
r0:0x00000002 r1:0x0000000c r2:0x00200038 r3:0x80000000 r0:0x00000002 r1:0x0000000c r2:0x00200038 r3:0x80000000
r4:0x0020fffc r5:0x00101788 r6:0x00000180 r7:0x41000000 r4:0x0020fffc r5:0x00101788 r6:0x00000180 r7:0x41000000
r8:0x00008400 r9:0x00100004 r10:0x03000294 fp:0x0020fe84 r8:0x00008400 r9:0x00100004 sl:0x03000294 fp:0x0020fe84
r12:0x0020fe10 sp:0x0020fe70 lr:0x00102174 pc:0x80000004 ip:0x0020fe10 sp:0x0020fe70 lr:0x00102174 pc:0x80000004
--- Stackdump: --- Stackdump:
0x0020fe70: 0x0020fe7c 0x00101648 0x001015a8 0x0020feaf 0x0020fe70: 0x0020fe7c 0x00101648 0x001015a8 0x0020feaf
0x0020fe80: 0x0020fec0 0x0020fe90 0x00101780 0x00101620 0x0020fe80: 0x0020fec0 0x0020fe90 0x00101780 0x00101620
@ -142,9 +144,9 @@ void at91_abt_handler(uint32_t cpsr, uint32_t *registers)
dbgu_hexvar(" r7:", registers[10]); dbgu_hexvar(" r7:", registers[10]);
dbgu_hexvar("\n r8:", registers[11]); dbgu_hexvar("\n r8:", registers[11]);
dbgu_hexvar(" r9:", registers[12]); dbgu_hexvar(" r9:", registers[12]);
dbgu_hexvar(" r10:", registers[13]); dbgu_hexvar(" sl:", registers[13]);
dbgu_hexvar(" fp:", registers[14]); dbgu_hexvar(" fp:", registers[14]);
dbgu_hexvar("\nr12:", registers[15]); dbgu_hexvar("\n ip:", registers[15]);
dbgu_hexvar(" sp:", registers[0]); dbgu_hexvar(" sp:", registers[0]);
dbgu_hexvar(" lr:", registers[1]); dbgu_hexvar(" lr:", registers[1]);
dbgu_hexvar(" pc:", registers[2]); dbgu_hexvar(" pc:", registers[2]);
@ -160,3 +162,43 @@ void at91_abt_handler(uint32_t cpsr, uint32_t *registers)
} }
dbgu_putchar('\n'); dbgu_putchar('\n');
} }
__attribute__((naked)) void ABT_Handler(void)
{
asm volatile (
".equ ARM_MODE_ABT, 0x17 \n\t"
".equ I_BIT, 0x80 \n\t"
".equ F_BIT, 0x40 \n\t"
/* disable interrupts (F_BIT not set on entry) */
"msr CPSR_c, #ARM_MODE_ABT | I_BIT | F_BIT \n\t"
/* store all registers */
"stmfd sp!, { r0-r12 } \n\t"
/* saved cpsr (from aborted mode) */
"mrs r0, SPSR \n\t"
/* address of abort (pc) */
"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"
"mov r1, sp \n\t"
"mov r2, lr \n\t"
/* return to abort mode */
"msr CPSR_c, #ARM_MODE_ABT | I_BIT | F_BIT \n\t"
/* store remaining registers (r1-r3 == r13-r15) */
"stmfd sp!, { r1-r3 } \n\t"
"mov r1, sp \n\t"
/* execute C Handler (cpsr, *registers) */
"ldr r5, =at91_abt_handler \n\t"
"mov lr, pc \n\t"
"bx r5 \n\t"
"b . \n\t"
);
}

View File

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

View File

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

View File

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

View File

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

View File

@ -1,4 +1,6 @@
/*************************************************************************** /***************************************************************************
* sam7fc - RC-PPM Signal decoder *
* *
* Copyright (C) 01/2008 by Olaf Rempel * * Copyright (C) 01/2008 by Olaf Rempel *
* razzor@kopf-tisch.de * * razzor@kopf-tisch.de *
* * * *
@ -22,13 +24,15 @@
#include "AT91SAM7S256.h" #include "AT91SAM7S256.h"
#include "board.h" #include "board.h"
#include "at91_tc1.h" #include "at91_tc1.h"
#include "at91_pio.h" #include "at91_twi.h"
/* Hard limits for ISR */ /* Hard limits for ISR */
#define PULSE_MIN 0x0500 #define PULSE_MIN 0x0500
#define PULSE_MAX 0x0D00 #define PULSE_MAX 0x0D00
#define PULSE_TIMEOUT 0x0F00 #define PULSE_TIMEOUT 0x0F00
#define PULSE_CENTER 0x08C0 #define PULSE_CENTER 0x08C0
#define PULSE_SWITCH 0x01F0
/* moving average filters */ /* moving average filters */
#define PULSE_FILTER_FAST (1<<2) #define PULSE_FILTER_FAST (1<<2)
@ -49,6 +53,11 @@ struct channel_data {
uint16_t max; /* maximum 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 struct channel_data ch_data[MAX_CHANNELS];
static uint32_t count, valid, cal_in_progress; static uint32_t count, valid, cal_in_progress;
@ -123,17 +132,32 @@ uint32_t rcontrol_getvalues(struct rc_values *rc)
uint16_t width = (filter) ? ch_data[i].width_slow : ch_data[i].width; uint16_t width = (filter) ? ch_data[i].width_slow : ch_data[i].width;
/* expand the value to +/- VALUE_RANGE */ /* expand the value to +/- VALUE_RANGE */
int32_t tmp = (width - ch_data[i].mid) * 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)); 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 */ /* keep result in range */
if (tmp > VALUE_RANGE) rc->chan[i] = LIMIT(tmp, -VALUE_RANGE, +VALUE_RANGE);
tmp = VALUE_RANGE; }
return cnt;
}
if (tmp < -VALUE_RANGE) uint32_t rcontrol_getswitches(struct rc_values *rc)
tmp = -VALUE_RANGE; {
if (valid < 5)
return 0;
rc->chan[i] = tmp; 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; return cnt;
} }
@ -141,6 +165,8 @@ uint32_t rcontrol_getvalues(struct rc_values *rc)
void rcontrol_calibrate(uint32_t mode) void rcontrol_calibrate(uint32_t mode)
{ {
uint32_t i; uint32_t i;
uint8_t buf[EE_RC_CAL_DATA_SIZE];
uint16_t *ptr = (uint16_t *)buf;
switch (mode) { switch (mode) {
case RC_CAL_START: case RC_CAL_START:
@ -159,23 +185,42 @@ void rcontrol_calibrate(uint32_t mode)
/* treat current position as center */ /* treat current position as center */
ch_data[i].mid = ch_data[i].width_slow; ch_data[i].mid = ch_data[i].width_slow;
/* if center is near minimum, clamp output to 0..+1024 */ /* if center is near minimum, clamp output to 0..+RANGE */
if (ch_data[i].mid - ch_data[i].min < PULSE_MID_DIFF) if (ch_data[i].mid - ch_data[i].min < PULSE_MID_DIFF)
ch_data[i].mid = ch_data[i].min; ch_data[i].mid = ch_data[i].min;
/* if center is near maximum, clamp output to -1024..0 */ /* if center is near maximum, clamp output to -RANGE..0 */
if (ch_data[i].max - ch_data[i].mid < PULSE_MID_DIFF) if (ch_data[i].max - ch_data[i].mid < PULSE_MID_DIFF)
ch_data[i].mid = ch_data[i].max; ch_data[i].mid = ch_data[i].max;
} }
break; 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) void rcontrol_print_cal(void)
{ {
uint32_t i; uint32_t i;
printf("stick-calibration:\n\r");
for (i = 0; i < ARRAY_SIZE(ch_data); i++) { for (i = 0; i < ARRAY_SIZE(ch_data); i++) {
printf("%ld: %d(%+d) - %d(0) - %d(%+d)\n\r", 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].min, ch_data[i].min - ch_data[i].mid,
ch_data[i].mid, ch_data[i].mid,
ch_data[i].max, ch_data[i].max - ch_data[i].mid ch_data[i].max, ch_data[i].max - ch_data[i].mid
@ -206,22 +251,7 @@ void at91_tc1_init(void)
aic->AIC_SMR[AT91C_ID_TC1] = IRQPRIO_TC1 | AT91C_AIC_SRCTYPE_INT_HIGH_LEVEL; 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_SVR[AT91C_ID_TC1] = (uint32_t)ppm_isr;
aic->AIC_IECR = (1 << AT91C_ID_TC1); aic->AIC_IECR = (1 << AT91C_ID_TC1);
rcontrol_calibrate(RC_CAL_LOAD);
rcontrol_print_cal();
} }
static void tast_monitor(uint32_t status, uint32_t input)
{
if (!(input & TAST1) && (cal_in_progress == 0)) {
printf("start calibration\n\r");
rcontrol_calibrate(RC_CAL_START);
} else if (!(input & TAST2) && (cal_in_progress == 1)) {
printf("end calibration\n\r");
rcontrol_calibrate(RC_CAL_END);
rcontrol_print_cal();
}
}
PIO_PINCHANGE_ISR(TAST1 | TAST2, tast_monitor);

View File

@ -1,4 +1,6 @@
/*************************************************************************** /***************************************************************************
* sam7fc - TWI/I2C Handling *
* *
* Copyright (C) 02/2008 by Olaf Rempel * * Copyright (C) 02/2008 by Olaf Rempel *
* razzor@kopf-tisch.de * * razzor@kopf-tisch.de *
* * * *
@ -22,7 +24,6 @@
#include "AT91SAM7S256.h" #include "AT91SAM7S256.h"
#include "board.h" #include "board.h"
#include "at91_twi.h" #include "at91_twi.h"
#include "at91_pio.h"
/* /*
* undocumented TWI_SR flags, at least OVRE seems to be present on a sam7s256 * undocumented TWI_SR flags, at least OVRE seems to be present on a sam7s256
@ -61,8 +62,6 @@ static void twi_isr(void)
if (status & AT91C_TWI_NACK) { if (status & AT91C_TWI_NACK) {
*AT91C_TWI_IDR = AT91C_TWI_TXCOMP | AT91C_TWI_RXRDY | AT91C_TWI_TXRDY | AT91C_TWI_NACK; *AT91C_TWI_IDR = AT91C_TWI_TXCOMP | AT91C_TWI_RXRDY | AT91C_TWI_TXRDY | AT91C_TWI_NACK;
twi_state = TWI_ERROR; twi_state = TWI_ERROR;
// TODO: TWI_BLMC_UPDATE?
return; return;
} }
@ -95,19 +94,15 @@ static void twi_isr(void)
/* transfer really complete? */ /* transfer really complete? */
if (status & AT91C_TWI_TXCOMP) { if (status & AT91C_TWI_TXCOMP) {
uint32_t addr = (*AT91C_TWI_MMR >> 16) & 0x7F;
/* are we doing a blmc update? */ /* are we doing a blmc update? */
if (twi_state == TWI_BLMC_UPDATE) { if (twi_state == TWI_BLMC_UPDATE && addr != TWI_ADDR_BL4) {
uint32_t addr = (*AT91C_TWI_MMR >> 16) & 0x7F; /* increase address */
if (addr != TWI_ADDR_BL4) { *AT91C_TWI_MMR += (1<<16);
/* increase address */
*AT91C_TWI_MMR += (1<<16);
/* send next value to next blmc */ /* send next value to next blmc */
*AT91C_TWI_THR = *twi_data++; *AT91C_TWI_THR = *twi_data++;
} else {
// TODO:
}
} else { } else {
*AT91C_TWI_IDR = AT91C_TWI_TXCOMP | AT91C_TWI_RXRDY | AT91C_TWI_TXRDY | AT91C_TWI_NACK; *AT91C_TWI_IDR = AT91C_TWI_TXCOMP | AT91C_TWI_RXRDY | AT91C_TWI_TXRDY | AT91C_TWI_NACK;
@ -118,11 +113,14 @@ static void twi_isr(void)
uint32_t twi_setpwm(uint8_t *values) uint32_t twi_setpwm(uint8_t *values)
{ {
if (twi_state == TWI_ERROR)
twi_state = TWI_IDLE;
if (twi_state != TWI_IDLE) if (twi_state != TWI_IDLE)
return 1; return 1;
twi_state = TWI_BLMC_UPDATE; twi_state = TWI_BLMC_UPDATE;
twi_data = values; /* data is not copied! */ twi_data = values;
twi_size = 0; twi_size = 0;
twi_count = 0; twi_count = 0;
@ -135,8 +133,11 @@ uint32_t twi_setpwm(uint8_t *values)
return 0; return 0;
} }
uint32_t twi_cmd(uint8_t addr, struct blmc_cmd *cmd) 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) if (twi_state != TWI_IDLE)
return 1; return 1;
@ -144,7 +145,7 @@ uint32_t twi_cmd(uint8_t addr, struct blmc_cmd *cmd)
twi_state = TWI_GENERIC_CMD; twi_state = TWI_GENERIC_CMD;
/* read transfer, or write transfer with payload */ /* read transfer, or write transfer with payload */
if (cmd->mode & BLMC_CMD_READ || cmd->size != 0) { if (cmd->mode & TWI_MODE_READ || cmd->size != 0) {
/* set address, direction, argument count and command bytes */ /* set address, direction, argument count and command bytes */
*AT91C_TWI_MMR = (addr << 16) | (cmd->mode & 0xFF) << 8; *AT91C_TWI_MMR = (addr << 16) | (cmd->mode & 0xFF) << 8;
*AT91C_TWI_IADR = cmd->cmd; *AT91C_TWI_IADR = cmd->cmd;
@ -162,7 +163,7 @@ uint32_t twi_cmd(uint8_t addr, struct blmc_cmd *cmd)
twi_size = cmd->size; twi_size = cmd->size;
twi_count = 0; twi_count = 0;
if (cmd->mode & BLMC_CMD_READ) { if (cmd->mode & TWI_MODE_READ) {
*AT91C_TWI_CR = AT91C_TWI_START; *AT91C_TWI_CR = AT91C_TWI_START;
*AT91C_TWI_IER = AT91C_TWI_RXRDY | AT91C_TWI_NACK; *AT91C_TWI_IER = AT91C_TWI_RXRDY | AT91C_TWI_NACK;
@ -185,46 +186,113 @@ uint32_t twi_cmd(uint8_t addr, struct blmc_cmd *cmd)
return 0; 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) void at91_twi_test(void)
{ {
uint32_t i; uint32_t i;
for (i = TWI_ADDR_BL1; i <= TWI_ADDR_BL4; i++) { for (i = TWI_ADDR_BL1; i <= TWI_ADDR_BL4; i++) {
struct blmc_cmd cmd = { printf("twi[0x%02lx] ", i);
.cmd = CMD_BOOT_LOADER,
.mode = BLMC_CMD_WRITE | BLMC_CMD_0_ARG,
};
uint32_t ret = twi_cmd(i, &cmd);
printf("twi[0x%02lx](%ld) ", i, ret);
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; volatile uint32_t x;
for (x = 0; x < 200000; x++); for (x = 0; x < 200000; x++);
uint8_t buf[16]; uint8_t buf[16];
cmd.cmd = CMD_GET_INFO, buf[0] = '\0';
cmd.mode = BLMC_CMD_READ | BLMC_CMD_0_ARG, cmd.cmd = CMD_GET_INFO;
cmd.size = sizeof(buf), cmd.mode = TWI_MODE_READ | TWI_MODE_0_ARG;
cmd.data = buf, cmd.size = sizeof(buf);
ret = twi_cmd(i, &cmd); cmd.data = buf;
printf("boot(%ld):'%s' ", ret, 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.cmd = CMD_GET_SIGNATURE;
cmd.size = 4; cmd.size = 4;
ret = twi_cmd(i, &cmd); twi_cmd(i, &cmd);
printf("sig(%ld):0x%02x%02x%02x ", ret, buf[0], buf[1], buf[2]); printf("sig:0x%02x%02x%02x\n\r", buf[0], buf[1], buf[2]);
cmd.cmd = CMD_BOOT_APPLICATION; cmd.cmd = CMD_BOOT_APPLICATION;
cmd.mode = BLMC_CMD_WRITE | BLMC_CMD_0_ARG; cmd.mode = TWI_MODE_WRITE | TWI_MODE_0_ARG;
cmd.size = 0; cmd.size = 0;
ret = twi_cmd(i, &cmd); twi_cmd(i, &cmd);
/* TODO: sleep */
for (x = 0; x < 200000; x++); for (x = 0; x < 200000; x++);
cmd.cmd = CMD_GET_INFO, buf[0] = '\0';
cmd.mode = BLMC_CMD_READ | BLMC_CMD_0_ARG, cmd.cmd = CMD_GET_INFO;
cmd.size = sizeof(buf), cmd.mode = TWI_MODE_READ | TWI_MODE_0_ARG;
cmd.data = buf, cmd.size = sizeof(buf);
ret = twi_cmd(i, &cmd); cmd.data = buf;
printf("app(%ld):'%s'\n\r", ret, 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);
} }
} }

View File

@ -1,4 +1,6 @@
/*************************************************************************** /***************************************************************************
* sam7fc - USB Device Port with logical Serial Port *
* *
* Copyright (C) 01/2008 by Olaf Rempel * * Copyright (C) 01/2008 by Olaf Rempel *
* razzor@kopf-tisch.de * * razzor@kopf-tisch.de *
* * * *
@ -21,9 +23,11 @@
#include "at91_pio.h" #include "at91_pio.h"
#include "board.h" #include "board.h"
#include "fifo.h" #include "fifo.h"
#include "telemetrie.h"
#include "usb_ch9.h" #include "usb_ch9.h"
#include "usb_cdc.h" #include "usb_cdc.h"
#include "usb_dfu.h"
#define csr_clear_flags(csr, flags) \ #define csr_clear_flags(csr, flags) \
while ((csr) & (flags)) \ while ((csr) & (flags)) \
@ -65,6 +69,8 @@ static uint16_t current_address;
static uint16_t current_config; static uint16_t current_config;
static uint16_t current_interface; static uint16_t current_interface;
static struct comm_device usb_comm;
static const struct usb_device_descriptor dev_descriptor = { static const struct usb_device_descriptor dev_descriptor = {
.bLength = sizeof(struct usb_device_descriptor), .bLength = sizeof(struct usb_device_descriptor),
.bDescriptorType = USB_DT_DEVICE, .bDescriptorType = USB_DT_DEVICE,
@ -73,6 +79,7 @@ static const struct usb_device_descriptor dev_descriptor = {
.idVendor = USB_VENDOR_ID, .idVendor = USB_VENDOR_ID,
.idProduct = USB_PRODUCT_ID +1, .idProduct = USB_PRODUCT_ID +1,
.bcdDevice = 0x0001, .bcdDevice = 0x0001,
.iProduct = 0x01,
.bNumConfigurations = 1, .bNumConfigurations = 1,
}; };
@ -87,6 +94,8 @@ struct my_config {
struct usb_interface_descriptor data_iface; struct usb_interface_descriptor data_iface;
struct usb_endpoint_descriptor dataout_ep; struct usb_endpoint_descriptor dataout_ep;
struct usb_endpoint_descriptor datain_ep; struct usb_endpoint_descriptor datain_ep;
struct usb_interface_descriptor dfu_iface;
struct usb_dfu_descriptor dfu;
} __attribute__ ((packed)); } __attribute__ ((packed));
static const struct my_config cfg_descriptor = { static const struct my_config cfg_descriptor = {
@ -94,7 +103,7 @@ static const struct my_config cfg_descriptor = {
.bLength = sizeof(struct usb_config_descriptor), .bLength = sizeof(struct usb_config_descriptor),
.bDescriptorType = USB_DT_CONFIG, .bDescriptorType = USB_DT_CONFIG,
.wTotalLength = sizeof(struct my_config), .wTotalLength = sizeof(struct my_config),
.bNumInterfaces = 2, .bNumInterfaces = 3,
.bConfigurationValue = 1, .bConfigurationValue = 1,
.bmAttributes = USB_CONFIG_ATT_SELFPOWER | USB_CONFIG_ATT_WAKEUP, .bmAttributes = USB_CONFIG_ATT_SELFPOWER | USB_CONFIG_ATT_WAKEUP,
.bMaxPower = 50, .bMaxPower = 50,
@ -163,9 +172,59 @@ static const struct my_config cfg_descriptor = {
.bmAttributes = USB_ENDPOINT_XFER_BULK, .bmAttributes = USB_ENDPOINT_XFER_BULK,
.wMaxPacketSize = 64, .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,
},
}; };
static void ep_transfer_send(uint32_t ep, char *data, uint32_t length, /* 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)) void (*complete_cb)(void))
{ {
struct ep_ctx *ctx = &ep_ctx[ep]; struct ep_ctx *ctx = &ep_ctx[ep];
@ -192,7 +251,7 @@ static void ep_transfer_send(uint32_t ep, char *data, uint32_t length,
AT91C_UDP_CSR[ep] |= AT91C_UDP_TXPKTRDY; AT91C_UDP_CSR[ep] |= AT91C_UDP_TXPKTRDY;
} }
static void ep_transfer_receive(uint32_t ep, char *data, uint32_t length, void ep_transfer_receive(uint32_t ep, char *data, uint32_t length,
void (*complete_cb)(void)) void (*complete_cb)(void))
{ {
struct ep_ctx *ctx = &ep_ctx[ep]; struct ep_ctx *ctx = &ep_ctx[ep];
@ -213,7 +272,7 @@ static void ep_transfer_receive(uint32_t ep, char *data, uint32_t length,
/* stalls the endpoint */ /* stalls the endpoint */
static void ep_send_stall(uint32_t ep) static void ep_send_stall(uint32_t ep)
{ {
printf("stall\n\r"); printf("usb stall\n\r");
AT91C_UDP_CSR[ep] |= AT91C_UDP_FORCESTALL; AT91C_UDP_CSR[ep] |= AT91C_UDP_FORCESTALL;
} }
@ -236,6 +295,12 @@ static void udp_configure_ep(const struct usb_endpoint_descriptor *desc)
*AT91C_UDP_IER = (1 << ep); *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 * set local address
* (USB_REQ_SET_ADDRESS callback) * (USB_REQ_SET_ADDRESS callback)
@ -244,6 +309,7 @@ static void udp_txcb_setaddress(void)
{ {
*AT91C_UDP_FADDR = (AT91C_UDP_FEN | current_address); *AT91C_UDP_FADDR = (AT91C_UDP_FEN | current_address);
*AT91C_UDP_GLBSTATE = AT91C_UDP_FADDEN; *AT91C_UDP_GLBSTATE = AT91C_UDP_FADDEN;
udp_print_config();
} }
/* /*
@ -256,19 +322,21 @@ static void udp_txcb_setconfig(void)
udp_configure_ep(&cfg_descriptor.datain_ep); udp_configure_ep(&cfg_descriptor.datain_ep);
udp_configure_ep(&cfg_descriptor.dataout_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" */ /* set UDP to "configured" */
*AT91C_UDP_GLBSTATE = AT91C_UDP_CONFG; *AT91C_UDP_GLBSTATE = AT91C_UDP_CONFG;
} udp_print_config();
static void udp_txcb_setinterface(void)
{
printf("claim interface %d\n\r", current_interface);
} }
static void ep_handle_ctrlrequest(struct usb_ctrlrequest *req) 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", // 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); // req->bRequestType, req->bRequest, req->wValue, req->wIndex, req->wLength);
switch (req->bRequestType & (USB_TYPE_MASK | USB_RECIP_MASK)) { switch (req->bRequestType & (USB_TYPE_MASK | USB_RECIP_MASK)) {
case (USB_TYPE_STANDARD | USB_RECIP_DEVICE): /* 0x00/0x80 */ case (USB_TYPE_STANDARD | USB_RECIP_DEVICE): /* 0x00/0x80 */
@ -292,6 +360,24 @@ static void ep_handle_ctrlrequest(struct usb_ctrlrequest *req)
NULL); NULL);
break; 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: default:
ep_send_stall(0); ep_send_stall(0);
break; break;
@ -310,11 +396,10 @@ static void ep_handle_ctrlrequest(struct usb_ctrlrequest *req)
break; break;
case (USB_TYPE_STANDARD | USB_RECIP_INTERFACE): /* 0x01/0x81 */ case (USB_TYPE_STANDARD | USB_RECIP_INTERFACE): /* 0x01/0x81 */
// TODO: follow current_interface
switch (req->bRequest) { switch (req->bRequest) {
case USB_REQ_SET_INTERFACE: /* 0x0b */ case USB_REQ_SET_INTERFACE: /* 0x0b */
current_interface = req->wValue; current_interface = req->wValue;
ep_transfer_send(0, NULL, 0, udp_txcb_setinterface); ep_transfer_send(0, NULL, 0, udp_print_config);
break; break;
default: default:
@ -326,8 +411,21 @@ static void ep_handle_ctrlrequest(struct usb_ctrlrequest *req)
case (USB_TYPE_CLASS | USB_RECIP_INTERFACE): /* 0x21/0xA1 */ case (USB_TYPE_CLASS | USB_RECIP_INTERFACE): /* 0x21/0xA1 */
// TODO: follow current_interface // TODO: follow current_interface
switch (req->bRequest) { 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 */ case USB_CDC_REQ_SET_LINE_CODING: /* 0x20 */
/* read 7 bytes */ 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; break;
case USB_CDC_REQ_SET_CONTROL_LINE_STATE: /* 0x22 */ case USB_CDC_REQ_SET_CONTROL_LINE_STATE: /* 0x22 */
@ -353,6 +451,10 @@ static void udp_handle_ep(uint32_t ep)
if (!(*csr & AT91C_UDP_EPEDS)) if (!(*csr & AT91C_UDP_EPEDS))
return; return;
/* clear STALLSENT interrupt */
if (*csr & AT91C_UDP_STALLSENT)
csr_clear_flags(*csr, (AT91C_UDP_STALLSENT | AT91C_UDP_FORCESTALL));
/* ctrl request packet? */ /* ctrl request packet? */
if (*csr & AT91C_UDP_RXSETUP) { if (*csr & AT91C_UDP_RXSETUP) {
struct usb_ctrlrequest req; struct usb_ctrlrequest req;
@ -360,6 +462,10 @@ static void udp_handle_ep(uint32_t ep)
for (p = (uint8_t *)&req; p < (uint8_t *)(&req +1); p++) for (p = (uint8_t *)&req; p < (uint8_t *)(&req +1); p++)
*p = AT91C_UDP_FDR[ep]; *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 */ /* set data phase transfer direction */
if (req.bRequestType & USB_DIR_IN) if (req.bRequestType & USB_DIR_IN)
*csr |= AT91C_UDP_DIR; *csr |= AT91C_UDP_DIR;
@ -370,15 +476,21 @@ static void udp_handle_ep(uint32_t ep)
ep_handle_ctrlrequest(&req); ep_handle_ctrlrequest(&req);
} }
void (* transfer_cb)(void) = NULL;
/* transmit complete? */ /* transmit complete? */
if (*csr & AT91C_UDP_TXCOMP) { if (*csr & AT91C_UDP_TXCOMP) {
struct ep_ctx *ctx = &ep_ctx[ep]; struct ep_ctx *ctx = &ep_ctx[ep];
if (ctx->flags & CTX_FIFO) { if (ctx->flags & CTX_FIFO) {
/* get data from fifo */ /* get data from fifo */
if (fifo_txudp(ctx->fifo, ep, ctx->maxpktsize)) if (fifo_txudp(ctx->fifo, ep, ctx->maxpktsize)) {
AT91C_UDP_CSR[ep] |= AT91C_UDP_TXPKTRDY; AT91C_UDP_CSR[ep] |= AT91C_UDP_TXPKTRDY;
} else {
ctx->flags &= ~CTX_IN;
}
} else if ((ctx->flags & (CTX_TRANSFER | CTX_IN)) == (CTX_TRANSFER | CTX_IN)) { } else if ((ctx->flags & (CTX_TRANSFER | CTX_IN)) == (CTX_TRANSFER | CTX_IN)) {
/* transfer not complete */ /* transfer not complete */
struct ep_transfer *transfer = ctx->transfer; struct ep_transfer *transfer = ctx->transfer;
@ -395,9 +507,7 @@ static void udp_handle_ep(uint32_t ep)
/* transfer complete, execute callback */ /* transfer complete, execute callback */
} else { } else {
ctx->flags &= ~CTX_IN; ctx->flags &= ~CTX_IN;
transfer_cb = transfer->complete_cb;
if (transfer->complete_cb)
transfer->complete_cb();
} }
} }
@ -405,14 +515,9 @@ static void udp_handle_ep(uint32_t ep)
*csr &= ~(AT91C_UDP_TXCOMP); *csr &= ~(AT91C_UDP_TXCOMP);
} }
/* clear STALLSENT interrupt */
if (*csr & AT91C_UDP_STALLSENT)
csr_clear_flags(*csr, (AT91C_UDP_STALLSENT | AT91C_UDP_FORCESTALL));
/* data ready to read? */ /* data ready to read? */
if (*csr & (AT91C_UDP_RX_DATA_BK0 | AT91C_UDP_RX_DATA_BK1)) { if (*csr & (AT91C_UDP_RX_DATA_BK0 | AT91C_UDP_RX_DATA_BK1)) {
struct ep_ctx *ctx = &ep_ctx[ep]; struct ep_ctx *ctx = &ep_ctx[ep];
uint16_t len = (*csr & AT91C_UDP_RXBYTECNT) >> 16; uint16_t len = (*csr & AT91C_UDP_RXBYTECNT) >> 16;
// TODO: only ep0 status OUT? // TODO: only ep0 status OUT?
@ -438,9 +543,7 @@ static void udp_handle_ep(uint32_t ep)
/* test again */ /* test again */
if (transfer->length == transfer->curpos) { if (transfer->length == transfer->curpos) {
ctx->flags &= ~CTX_OUT; ctx->flags &= ~CTX_OUT;
transfer_cb = transfer->complete_cb;
if (transfer->complete_cb)
transfer->complete_cb();
} }
} }
@ -459,6 +562,9 @@ static void udp_handle_ep(uint32_t ep)
ctx->flags = (ctx->flags & ~CTX_RXBANK1) | CTX_RXBANK0; ctx->flags = (ctx->flags & ~CTX_RXBANK1) | CTX_RXBANK0;
} }
} }
if (transfer_cb)
transfer_cb();
} }
static void udp_isr(void) static void udp_isr(void)
@ -488,6 +594,12 @@ static void udp_isr(void)
udp->UDP_IDR = AT91C_UDP_EPINT1 | AT91C_UDP_EPINT2 | AT91C_UDP_EPINT3 | udp->UDP_IDR = AT91C_UDP_EPINT1 | AT91C_UDP_EPINT2 | AT91C_UDP_EPINT3 |
AT91C_UDP_RXSUSP | AT91C_UDP_RXRSM | AT91C_UDP_SOFINT | AT91C_UDP_RXSUSP | AT91C_UDP_RXRSM | AT91C_UDP_SOFINT |
AT91C_UDP_WAKEUP; AT91C_UDP_WAKEUP;
if (dfu_status.bStatus == DFU_STATE_appDETACH) {
void (* bootloader)(void) = (void *)0x13c000;
bootloader();
while (1);
}
} }
/* Handle Endpoint Interrupts */ /* Handle Endpoint Interrupts */
@ -502,6 +614,21 @@ static void udp_isr(void)
AT91C_UDP_ENDBUSRES | AT91C_UDP_WAKEUP); 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) void at91_udp_init(void)
{ {
/* configure & disable Pullup, disable Pullup von VBUS */ /* configure & disable Pullup, disable Pullup von VBUS */
@ -509,8 +636,6 @@ void at91_udp_init(void)
pio->PIO_CODR = UDP_PULLUP; pio->PIO_CODR = UDP_PULLUP;
pio->PIO_PER = UDP_PULLUP; pio->PIO_PER = UDP_PULLUP;
pio->PIO_OER = UDP_PULLUP; pio->PIO_OER = UDP_PULLUP;
// TODO: needed?
pio->PIO_PPUDR = UDP_VBUS_MON;
/* UDPCK (48MHz) = PLLCK / 2 */ /* UDPCK (48MHz) = PLLCK / 2 */
*AT91C_CKGR_PLLR |= AT91C_CKGR_USBDIV_1; *AT91C_CKGR_PLLR |= AT91C_CKGR_USBDIV_1;
@ -536,6 +661,12 @@ void at91_udp_init(void)
aic->AIC_SVR[AT91C_ID_UDP] = (uint32_t)udp_isr; aic->AIC_SVR[AT91C_ID_UDP] = (uint32_t)udp_isr;
aic->AIC_IECR = (1 << AT91C_ID_UDP); 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); pio_trigger_isr(UDP_VBUS_MON);
} }
@ -543,10 +674,10 @@ static void udp_vbus_monitor(uint32_t status, uint32_t input)
{ {
if (input & UDP_VBUS_MON) if (input & UDP_VBUS_MON)
/* usb connected -> enable pullup */ /* usb connected -> enable pullup */
*AT91C_PIOA_SODR = UDP_PULLUP; *AT91C_PIOA_CODR = UDP_PULLUP;
else else
/* usb got diconnected -> disable pullup */ /* usb got diconnected -> disable pullup */
*AT91C_PIOA_CODR = UDP_PULLUP; *AT91C_PIOA_SODR = UDP_PULLUP;
} }
PIO_PINCHANGE_ISR(UDP_VBUS_MON, udp_vbus_monitor); PIO_PINCHANGE_ISR(UDP_VBUS_MON, udp_vbus_monitor);

View File

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

264
src/flightctrl.c Normal file
View File

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

View File

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

44
src/pidctrl.c Normal file
View File

@ -0,0 +1,44 @@
/***************************************************************************
* 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);
}

View File

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