Compare commits
13 Commits
Author | SHA1 | Date | |
---|---|---|---|
90385bca3d | |||
8583ff4f02 | |||
a8b04b8c9a | |||
24b566fc7e | |||
a8256718ab | |||
87edb0819d | |||
8414aeac15 | |||
580d8369fa | |||
c4227fbabb | |||
a81d25a84d | |||
7f54c86b7c | |||
773cc44a09 | |||
60cccae208 |
7
Makefile
7
Makefile
@ -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)
|
||||||
|
140
at91_init0.s
140
at91_init0.s
@ -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
|
|
||||||
|
95
at91_init1.c
95
at91_init1.c
@ -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
BIN
eagle/sam7fc-bot.png
Normal file
Binary file not shown.
After Width: | Height: | Size: 41 KiB |
BIN
eagle/sam7fc-sch.png
Normal file
BIN
eagle/sam7fc-sch.png
Normal file
Binary file not shown.
After Width: | Height: | Size: 203 KiB |
BIN
eagle/sam7fc-top.png
Normal file
BIN
eagle/sam7fc-top.png
Normal file
Binary file not shown.
After Width: | Height: | Size: 36 KiB |
32
include/at91_adc.h
Normal file
32
include/at91_adc.h
Normal 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_ */
|
@ -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_ */
|
||||||
|
@ -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);
|
||||||
|
@ -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);
|
||||||
|
|
||||||
|
@ -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_*/
|
||||||
|
@ -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);
|
||||||
|
@ -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_*/
|
||||||
|
@ -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
23
include/pidctrl.h
Normal 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
103
include/tdc_proto.h
Normal 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_ */
|
@ -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
63
main.c
@ -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();
|
||||||
|
}
|
||||||
}
|
}
|
||||||
|
184
src/at91_adc.c
184
src/at91_adc.c
@ -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);
|
||||||
}
|
}
|
||||||
|
@ -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;
|
||||||
}
|
}
|
||||||
|
@ -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"
|
||||||
|
);
|
||||||
|
}
|
||||||
|
@ -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 *
|
||||||
* *
|
* *
|
||||||
|
@ -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;
|
||||||
}
|
}
|
||||||
|
@ -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)
|
||||||
|
@ -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 *
|
||||||
* *
|
* *
|
||||||
|
@ -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);
|
|
||||||
|
146
src/at91_twi.c
146
src/at91_twi.c
@ -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 *)¶m;
|
||||||
|
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);
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
|
191
src/at91_udp.c
191
src/at91_udp.c
@ -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);
|
||||||
|
@ -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
264
src/flightctrl.c
Normal 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");
|
||||||
|
}
|
@ -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
44
src/pidctrl.c
Normal 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);
|
||||||
|
}
|
273
src/telemetrie.c
273
src/telemetrie.c
@ -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);
|
||||||
|
}
|
||||||
|
Loading…
Reference in New Issue
Block a user