From fe9b6689a6b1c2ee5d5302dfe46858ccd5dd226c Mon Sep 17 00:00:00 2001 From: Antoine Drouin Date: Tue, 23 Jan 2007 00:02:32 +0000 Subject: [PATCH] *** empty log message *** --- conf/airframes/imu.xml | 6 ++- conf/airframes/motor_bench.xml | 41 +++++++++++++++++++++ conf/autopilot/motor_bench.h | 22 +++++++++++ conf/autopilot/pprz_imu.h | 4 ++ sw/airborne/arm7/icp_scale.h | 31 ++++++++++++++++ sw/airborne/arm7/sys_time_hw.c | 23 ++++++++++++ sw/airborne/arm7/tacho_mb.c | 17 +++++++++ sw/airborne/arm7/tacho_mb.h | 18 +++++++++ sw/airborne/downlink.h | 4 +- sw/airborne/main_motor_bench.c | 67 ++++++++++++++++++++++++++++++++++ sw/airborne/print.h | 8 ++++ 11 files changed, 237 insertions(+), 4 deletions(-) create mode 100644 conf/airframes/motor_bench.xml create mode 100644 conf/autopilot/motor_bench.h create mode 100644 sw/airborne/arm7/icp_scale.h create mode 100644 sw/airborne/arm7/tacho_mb.c create mode 100644 sw/airborne/arm7/tacho_mb.h create mode 100644 sw/airborne/main_motor_bench.c diff --git a/conf/airframes/imu.xml b/conf/airframes/imu.xml index 73b290a9d2..6a3f55f240 100644 --- a/conf/airframes/imu.xml +++ b/conf/airframes/imu.xml @@ -17,10 +17,12 @@ FLASH_MODE = IAP imu.CFLAGS += -DIMU -DCONFIG=\"pprz_imu.h\" imu.srcs = main_imu.c sys_time.c $(SRC_ARCH)/sys_time_hw.c $(SRC_ARCH)/armVIC.c -#imu.CFLAGS += -DUSE_UART0 -DUART0_BAUD=B38400 +imu.CFLAGS += -DLED + +imu.CFLAGS += -DUSE_UART1 -DUART1_BAUD=B38400 imu.srcs += $(SRC_ARCH)/uart_hw.c -imu.CFLAGS += -DUSE_UART1 -DUART1_BAUD=B38400 -DDOWNLINK -DDOWNLINK_TRANSPORT=PprzTransport -DDOWNLINK_DEVICE=Uart1 +imu.CFLAGS += -DDOWNLINK -DDOWNLINK_TRANSPORT=PprzTransport -DDOWNLINK_DEVICE=Uart1 imu.srcs += downlink.c pprz_transport.c imu.CFLAGS += -DADC -DUSE_AD0 -DUSE_AD0_1 -DUSE_AD0_2 -DUSE_AD0_3 -DUSE_AD0_4 diff --git a/conf/airframes/motor_bench.xml b/conf/airframes/motor_bench.xml new file mode 100644 index 0000000000..43dd2762f5 --- /dev/null +++ b/conf/airframes/motor_bench.xml @@ -0,0 +1,41 @@ + + + + +ARCHI=arm7 +mb.ARCHDIR = $(ARCHI) +mb.ARCH = arm7tdmi +mb.TARGET = motor_bench +mb.TARGETDIR = motor_bench + +LPC21ISP_BAUD = 38400 +LPC21ISP_XTAL = 12000 + +#FLASH_MODE = IAP +FLASH_MODE = ISP + +mb.CFLAGS += -DCONFIG=\"motor_bench.h\" +mb.srcs = main_motor_bench.c sys_time.c $(SRC_ARCH)/sys_time_hw.c $(SRC_ARCH)/armVIC.c + +mb.CFLAGS += -DLED + +mb.CFLAGS += -DUSE_UART0 -DUART0_BAUD=B38400 +mb.srcs += $(SRC_ARCH)/uart_hw.c + +mb.CFLAGS += -DDOWNLINK -DDOWNLINK_TRANSPORT=PprzTransport -DDOWNLINK_DEVICE=Uart1 +mb.srcs += downlink.c pprz_transport.c + +mb.CFLAGS += -DADC -DUSE_AD0 -DUSE_AD0_1 -DUSE_AD0_2 -DUSE_AD0_3 -DUSE_AD0_4 +mb.srcs += $(SRC_ARCH)/adc_hw.c + +mb.CFLAGS += -DICP_SCALE +#mb.srcs += $(SRC_ARCH)/icp_scale.c + +mb.CFLAGS += -DTACHO_MB +mb.srcs += $(SRC_ARCH)/tacho_mb.c + + + + + + diff --git a/conf/autopilot/motor_bench.h b/conf/autopilot/motor_bench.h new file mode 100644 index 0000000000..ae704f914e --- /dev/null +++ b/conf/autopilot/motor_bench.h @@ -0,0 +1,22 @@ +#ifndef CONFIG_MOTOR_BENCH_H +#define CONFIG_MOTOR_BENCH_H + +/* Master oscillator freq. */ +#define FOSC (12000000) +/* PLL multiplier */ +#define PLL_MUL (5) +/* CPU clock freq. */ +#define CCLK (FOSC * PLL_MUL) +/* Peripheral bus speed mask 0x00->4, 0x01-> 1, 0x02 -> 2 */ +#define PBSD_BITS 0x00 +#define PBSD_VAL 4 +/* Peripheral bus clock freq. */ +#define PCLK (CCLK / PBSD_VAL) + +#define LED_1_BANK 0 +#define LED_1_PIN 12 + +#define LED_2_BANK 0 +#define LED_2_PIN 13 + +#endif /* CONFIG_MOTOR_BENCH_H */ diff --git a/conf/autopilot/pprz_imu.h b/conf/autopilot/pprz_imu.h index 3fefb15f10..9a4f46d02d 100644 --- a/conf/autopilot/pprz_imu.h +++ b/conf/autopilot/pprz_imu.h @@ -18,7 +18,11 @@ #define ADC_CHANNEL_AZ 3 #define ADC_CHANNEL_BAT 4 +#define LED_1_BANK 0 +#define LED_1_PIN 2 +#define LED_2_BANK 0 +#define LED_2_PIN 3 diff --git a/sw/airborne/arm7/icp_scale.h b/sw/airborne/arm7/icp_scale.h new file mode 100644 index 0000000000..18c10de965 --- /dev/null +++ b/sw/airborne/arm7/icp_scale.h @@ -0,0 +1,31 @@ +#ifndef ICP_SCALE_H +#define ICP_SCALE_H + +/* INPUT CAPTURE on P0.6*/ +#define ICP_PINSEL PINSEL0 +#define ICP_PINSEL_VAL 0x02 +#define ICP_PINSEL_BIT 12 + +#include "led.h" + +volatile uint32_t pulse_len; + +static inline void icp_scale_init ( void ) { + /* select pin for capture */ + ICP_PINSEL |= ICP_PINSEL_VAL << ICP_PINSEL_BIT; + /* enable capture 0.2 on falling edge + trigger interrupt */ + T0CCR |= TCCR_CR2_F | TCCR_CR2_I; +} + + +#define ICP_ISR() { \ + static uint32_t last; \ + uint32_t now = T0CR2; \ + pulse_len = now - last; \ + last = now; \ + LED_TOGGLE(2); \ + } + + + +#endif /* ICP_SCALE_H */ diff --git a/sw/airborne/arm7/sys_time_hw.c b/sw/airborne/arm7/sys_time_hw.c index 0b74835d09..2112f4d96e 100644 --- a/sw/airborne/arm7/sys_time_hw.c +++ b/sw/airborne/arm7/sys_time_hw.c @@ -7,8 +7,17 @@ #include "ppm.h" +#ifdef ICP_SCALE +#include "icp_scale.h" +#endif /* ICP_SCALE */ +#ifdef TACHO_MB +#include "tacho_mb.h" +#define TIMER0_IT_MASK (TIR_CR2I | TIR_MR1I | TIR_CR0I) +#else #define TIMER0_IT_MASK (TIR_CR2I | TIR_MR1I) +#endif /* TACHO_MB */ + void TIMER0_ISR ( void ) { ISR_ENTRY(); @@ -35,6 +44,20 @@ void TIMER0_ISR ( void ) { T0IR = TIR_MR1I; } #endif +#ifdef ICP_SCALE + if (T0IR&TIR_CR2I) { + ICP_ISR(); + /* clear interrupt */ + T0IR = TIR_CR2I; + } +#endif +#ifdef TACHO_MB + if (T0IR&TIR_CR0I) { + TACHO_MB_ISR(); + /* clear interrupt */ + T0IR = TIR_CR0I; + } +#endif /* TACHO_MB */ } VICVectAddr = 0x00000000; diff --git a/sw/airborne/arm7/tacho_mb.c b/sw/airborne/arm7/tacho_mb.c new file mode 100644 index 0000000000..32ea4e65b8 --- /dev/null +++ b/sw/airborne/arm7/tacho_mb.c @@ -0,0 +1,17 @@ +#include "tacho_mb.h" + +#include "LPC21xx.h" + +uint32_t t_duration; + +/* INPUT CAPTURE CAP0.0 on P0.22*/ +#define TACHO_MB_PINSEL PINSEL1 +#define TACHO_MB_PINSEL_VAL 0x02 +#define TACHO_MB_PINSEL_BIT 12 + +void tacho_mb_init ( void ) { + /* select pin for capture */ + TACHO_MB_PINSEL |= TACHO_MB_PINSEL_VAL << TACHO_MB_PINSEL_BIT; + /* enable capture 0.2 on falling edge + trigger interrupt */ + T0CCR |= TCCR_CR0_F | TCCR_CR0_I; +} diff --git a/sw/airborne/arm7/tacho_mb.h b/sw/airborne/arm7/tacho_mb.h new file mode 100644 index 0000000000..3aac45653f --- /dev/null +++ b/sw/airborne/arm7/tacho_mb.h @@ -0,0 +1,18 @@ +#ifndef TACHO_MB_H +#define TACHO_MB_H + +#include "std.h" + +extern void tacho_mb_init ( void ); + +extern uint32_t t_duration; + +#define TACHO_MB_ISR() { \ + static uint32_t tmb_last; \ + uint32_t t_now = T0CR0; \ + t_duration = t_now - tmb_last; \ + tmb_last = t_now; \ + LED_TOGGLE(1); \ + } + +#endif /* TACHO_MB_H */ diff --git a/sw/airborne/downlink.h b/sw/airborne/downlink.h index 6527301b66..4df1ee4166 100644 --- a/sw/airborne/downlink.h +++ b/sw/airborne/downlink.h @@ -50,12 +50,12 @@ #endif /** !SITL */ #ifdef AP -/** Telemetry mode for FBW process: index in the telemetry.xml file */ +/** Telemetry mode for AP process: index in the telemetry.xml file */ extern uint8_t telemetry_mode_Ap; #endif #ifdef FBW -/** Telemetry mode for AP process: index in the telemetry.xml file */ +/** Telemetry mode for FBW process: index in the telemetry.xml file */ extern uint8_t telemetry_mode_Fbw; #endif diff --git a/sw/airborne/main_motor_bench.c b/sw/airborne/main_motor_bench.c new file mode 100644 index 0000000000..c1ac854c3e --- /dev/null +++ b/sw/airborne/main_motor_bench.c @@ -0,0 +1,67 @@ + +#include "std.h" +#include "sys_time.h" +#include "init_hw.h" +#include "interrupt_hw.h" + +#include "uart.h" +#include "print.h" + +#include "adc.h" + +#include "icp_scale.h" + +#include "tacho_mb.h" + +#include "messages.h" +#include "downlink.h" + +static inline void main_init( void ); +static inline void main_periodic_task( void ); +static inline void main_event_task( void); + +int main( void ) { + main_init(); + LED_ON(1); + LED_ON(2); + while (1) { + if (sys_time_periodic()) + main_periodic_task(); + main_event_task(); + } + return 0; +} + +static inline void main_init( void ) { + hw_init(); + sys_time_init(); + led_init(); + uart0_init_tx(); + adc_init(); + icp_scale_init(); + tacho_mb_init(); + int_enable(); +} + +static uint32_t t0, t1; + +static inline void main_event_task( void ) { + +} + +static inline void main_periodic_task( void ) { + t0 = T0TC; + static uint8_t cnt; + cnt++; + if (!(cnt%16)) { + // LED_TOGGLE(1); + uart0_transmit('#'); + Uart0PrintHex32(pulse_len); + uart0_transmit(' '); + Uart0PrintHex32(t_duration); + uart0_transmit('\n'); + } +} + + + diff --git a/sw/airborne/print.h b/sw/airborne/print.h index c02790ff84..a7c2482952 100644 --- a/sw/airborne/print.h +++ b/sw/airborne/print.h @@ -51,8 +51,16 @@ _PrintHex(out_fun, low16); \ } +#define _PrintHex32(out_fun, c ) { \ + uint16_t high32 = (uint16_t)(c>>16); \ + uint16_t low32 = (uint16_t)(c); \ + _PrintHex16(out_fun, high32); \ + _PrintHex16(out_fun, low32); \ +} + #define Uart0PrintHex(c) _PrintHex(uart0_transmit, c) #define Uart0PrintHex16(c) _PrintHex16(uart0_transmit, c) +#define Uart0PrintHex32(c) _PrintHex32(uart0_transmit, c) #define Uart0PrintString(s) _PrintString(uart0_transmit, s) #define Uart1PrintHex(c) _PrintHex(uart1_transmit, c)