From 2263aab958811b66e741bd1fe74384a52765fece Mon Sep 17 00:00:00 2001 From: Antoine Drouin Date: Sat, 27 Jan 2007 00:16:54 +0000 Subject: [PATCH] *** empty log message *** --- conf/airframes/motor_bench.xml | 4 +-- conf/messages.xml | 7 +++++ sw/airborne/arm7/sys_time_hw.c | 3 ++ sw/airborne/arm7/sys_time_hw.h | 18 +++++++++-- sw/airborne/main_motor_bench.c | 56 +++++++++++++++++++++++++++------- sw/airborne/sys_time.c | 2 +- sw/airborne/sys_time.h | 2 +- sw/logalizer/Makefile | 3 ++ sw/logalizer/motor_bench.c | 34 +++++++++++++++++++++ 9 files changed, 111 insertions(+), 18 deletions(-) create mode 100644 sw/logalizer/motor_bench.c diff --git a/conf/airframes/motor_bench.xml b/conf/airframes/motor_bench.xml index f8abf8ef31..892449134a 100644 --- a/conf/airframes/motor_bench.xml +++ b/conf/airframes/motor_bench.xml @@ -14,7 +14,7 @@ LPC21ISP_XTAL = 12000 #FLASH_MODE = IAP FLASH_MODE = ISP -mb.CFLAGS += -DCONFIG=\"motor_bench.h\" +mb.CFLAGS += -DCONFIG=\"motor_bench.h\" -DPERIODIC_TASK_PERIOD='SYS_TICS_OF_SEC(10e-3)' mb.srcs = main_motor_bench.c sys_time.c $(SRC_ARCH)/sys_time_hw.c $(SRC_ARCH)/armVIC.c mb.CFLAGS += -DLED @@ -22,7 +22,7 @@ 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.CFLAGS += -DDOWNLINK -DDOWNLINK_TRANSPORT=PprzTransport -DDOWNLINK_DEVICE=Uart0 mb.srcs += downlink.c pprz_transport.c mb.CFLAGS += -DADC -DUSE_AD0 -DUSE_AD0_1 -DUSE_AD0_2 -DUSE_AD0_3 -DUSE_AD0_4 diff --git a/conf/messages.xml b/conf/messages.xml index 4860f4d2c1..6ff69811ff 100644 --- a/conf/messages.xml +++ b/conf/messages.xml @@ -363,6 +363,13 @@ + + + + + + + diff --git a/sw/airborne/arm7/sys_time_hw.c b/sw/airborne/arm7/sys_time_hw.c index 2112f4d96e..727eb99d7e 100644 --- a/sw/airborne/arm7/sys_time_hw.c +++ b/sw/airborne/arm7/sys_time_hw.c @@ -1,6 +1,9 @@ #include "armVIC.h" #include "sys_time.h" + +uint32_t cpu_time_ticks; + #if defined ACTUATORS #include ACTUATORS #endif /* ACTUATORS */ diff --git a/sw/airborne/arm7/sys_time_hw.h b/sw/airborne/arm7/sys_time_hw.h index b796dc8c3f..264b7f9a3f 100644 --- a/sw/airborne/arm7/sys_time_hw.h +++ b/sw/airborne/arm7/sys_time_hw.h @@ -35,6 +35,7 @@ #include CONFIG #include "led.h" +extern uint32_t cpu_time_ticks; static uint32_t last_periodic_event; void TIMER0_ISR ( void ) __attribute__((naked)); @@ -58,7 +59,8 @@ static inline void sys_time_init( void ) { /* enable timer 0 */ T0TCR = TCR_ENABLE; - cpu_time = 0; + cpu_time_sec = 0; + cpu_time_ticks = 0; /* select TIMER0 as IRQ */ VICIntSelect &= ~VIC_BIT(VIC_TIMER0); @@ -78,15 +80,25 @@ static inline void sys_time_init( void ) { #define FIFTY_MS SYS_TICS_OF_SEC( 50e-3 ) #define AVR_PERIOD_MS SYS_TICS_OF_SEC( 15.625e-3 ) +#ifndef PERIODIC_TASK_PERIOD #define PERIODIC_TASK_PERIOD AVR_PERIOD_MS +#endif + +#define TIME_TICKS_PER_SEC SYS_TICS_OF_SEC(1) static inline bool_t sys_time_periodic( void ) { uint32_t now = T0TC; - if (now - last_periodic_event >= PERIODIC_TASK_PERIOD) { + uint32_t dif = now - last_periodic_event; + if ( dif >= PERIODIC_TASK_PERIOD) { last_periodic_event += PERIODIC_TASK_PERIOD; + cpu_time_ticks += PERIODIC_TASK_PERIOD; + if (cpu_time_ticks > TIME_TICKS_PER_SEC) { + cpu_time_ticks -= TIME_TICKS_PER_SEC; + cpu_time_sec++; #ifdef TIME_LED - LED_TOGGLE(TIME_LED) + LED_TOGGLE(TIME_LED) #endif + } return TRUE; } return FALSE; diff --git a/sw/airborne/main_motor_bench.c b/sw/airborne/main_motor_bench.c index c1ac854c3e..1b54072bc4 100644 --- a/sw/airborne/main_motor_bench.c +++ b/sw/airborne/main_motor_bench.c @@ -16,14 +16,25 @@ #include "messages.h" #include "downlink.h" +#include "paparazzi.h" + static inline void main_init( void ); static inline void main_periodic_task( void ); static inline void main_event_task( void); +static inline void bench_periodic ( void ); +static inline bool_t prbs_10( void ) ; + +#define MODE_MANUAL 0 +#define MODE_RAMP 1 +#define MODE_STEP 2 +#define MODE_PRBS 3 + +uint8_t mode; +uint16_t throttle; + int main( void ) { main_init(); - LED_ON(1); - LED_ON(2); while (1) { if (sys_time_periodic()) main_periodic_task(); @@ -40,28 +51,51 @@ static inline void main_init( void ) { adc_init(); icp_scale_init(); tacho_mb_init(); + mode = MODE_PRBS; int_enable(); } -static uint32_t t0, t1; - static inline void main_event_task( void ) { } static inline void main_periodic_task( void ) { - t0 = T0TC; + bench_periodic(); + DOWNLINK_SEND_MOTOR_BENCH_STATUS(&cpu_time_ticks, &cpu_time_sec, &throttle, &mode); + static uint8_t cnt; cnt++; if (!(cnt%16)) { - // LED_TOGGLE(1); - uart0_transmit('#'); - Uart0PrintHex32(pulse_len); - uart0_transmit(' '); - Uart0PrintHex32(t_duration); - uart0_transmit('\n'); + LED_TOGGLE(1); + // DOWNLINK_SEND_MOTOR_BENCH_STATUS(&cpu_time_ticks, &cpu_time_sec, &throttle, &mode); } } +#define PRBS_VAL1 (0.55*MAX_PPRZ) +#define PRBS_VAL2 (0.65*MAX_PPRZ) + +static inline void bench_periodic ( void ) { + switch (mode) { + case MODE_MANUAL: + throttle = 0; + break; + case MODE_PRBS: + throttle = prbs_10() ? PRBS_VAL1 : PRBS_VAL2; + break; + } +} + +static inline bool_t prbs_10( void ) { + static uint16_t lsr = 0xFFFF; + uint8_t feed = + bit_is_set(lsr,6) ^ + bit_is_set(lsr,9); + uint8_t val = bit_is_set(lsr,9); + lsr = (lsr << 1) | feed; + return val; +} + + + diff --git a/sw/airborne/sys_time.c b/sw/airborne/sys_time.c index dbedf0cc90..7867019a7f 100644 --- a/sw/airborne/sys_time.c +++ b/sw/airborne/sys_time.c @@ -1,3 +1,3 @@ #include "sys_time.h" -uint16_t cpu_time; +uint16_t cpu_time_sec; diff --git a/sw/airborne/sys_time.h b/sw/airborne/sys_time.h index 6f9c5dca07..c446f217cd 100644 --- a/sw/airborne/sys_time.h +++ b/sw/airborne/sys_time.h @@ -4,7 +4,7 @@ #include #include CONFIG -extern uint16_t cpu_time; +extern uint16_t cpu_time_sec; #include "sys_time_hw.h" diff --git a/sw/logalizer/Makefile b/sw/logalizer/Makefile index e7cbd7ab99..7969ca99ec 100644 --- a/sw/logalizer/Makefile +++ b/sw/logalizer/Makefile @@ -38,6 +38,9 @@ CC = gcc CFLAGS=-g -O2 -Wall `pkg-config gtk+-2.0 --cflags` LDFLAGS=`pkg-config gtk+-2.0 --libs` -s -lgtkdatabox `pcre-config --libs` -lglibivy +motor_bench : motor_bench.c + $(CC) $(CFLAGS) -g -o $@ $^ $(LDFLAGS) + ahrsview : ahrsview.c sliding_plot.c $(CC) $(CFLAGS) -g -o $@ $^ $(LDFLAGS) diff --git a/sw/logalizer/motor_bench.c b/sw/logalizer/motor_bench.c new file mode 100644 index 0000000000..23131bb602 --- /dev/null +++ b/sw/logalizer/motor_bench.c @@ -0,0 +1,34 @@ +#include +#include +#include +#include + +#include +#include + +#define TICK_PER_SEC 15000000. +void on_MOTOR_BENCH_STATUS(IvyClientPtr app, void *user_data, int argc, char *argv[]){ + guint time_tick = atoi(argv[0]); + guint time_sec = atoi(argv[1]); + guint throttle = atoi(argv[2]); + guint mode = atoi(argv[3]); + + float time = (float)time_sec + (float)time_tick / TICK_PER_SEC; + printf("%f %d\n", time, throttle); + +} + + +int main ( int argc, char** argv) { + + // g_timeout_add(16, timeout_callback, chan); + + GMainLoop *ml = g_main_loop_new(NULL, FALSE); + + IvyInit ("MotorBench", "MotorBench READY", NULL, NULL, NULL, NULL); + IvyBindMsg(on_MOTOR_BENCH_STATUS, NULL, "^\\S* MOTOR_BENCH_STATUS (\\S*) (\\S*) (\\S*) (\\S*)"); + IvyStart("127.255.255.255"); + + g_main_loop_run(ml); + return 0; +}