diff --git a/conf/autopilot/lisa_m_test_progs.makefile b/conf/autopilot/lisa_m_test_progs.makefile index 2d73842e6d..5148b16039 100644 --- a/conf/autopilot/lisa_m_test_progs.makefile +++ b/conf/autopilot/lisa_m_test_progs.makefile @@ -73,6 +73,22 @@ test_led.srcs += $(SRC_AIRBORNE)/mcu.c \ test_led.CFLAGS += -DUSE_LED test_led.srcs += $(SRC_ARCH)/led_hw.c +# +# test sys_time +# +test_sys_time.ARCHDIR = $(ARCH) +test_sys_time.CFLAGS += -I$(SRC_LISA) -I$(ARCH) -DPERIPHERALS_AUTO_INIT +test_sys_time.CFLAGS += -DBOARD_CONFIG=$(BOARD_CFG) +test_sys_time.srcs += $(SRC_AIRBORNE)/mcu.c \ + $(SRC_ARCH)/mcu_arch.c \ + test/mcu_periph/test_sys_time.c \ + mcu_periph/sys_time.c \ + $(SRC_ARCH)/mcu_periph/sys_time_arch.c \ + $(SRC_ARCH)/stm32_exceptions.c \ + $(SRC_ARCH)/stm32_vector_table.c +test_sys_time.CFLAGS += -DUSE_LED -DUSE_SYS_TIME -DSYS_TIME_LED=1 -DLED_RED=2 -DLED_BLUE=3 +test_sys_time.srcs += $(SRC_ARCH)/led_hw.c + # # test uart # diff --git a/sw/airborne/arch/stm32/mcu_periph/sys_time_arch.c b/sw/airborne/arch/stm32/mcu_periph/sys_time_arch.c index 323c5bade9..4ced7e6cbe 100644 --- a/sw/airborne/arch/stm32/mcu_periph/sys_time_arch.c +++ b/sw/airborne/arch/stm32/mcu_periph/sys_time_arch.c @@ -25,19 +25,24 @@ #include "stm32_vector_table.h" #ifdef SYS_TIME_LED -#include "subsystems/led.h" +#include "led.h" #endif void sys_time_init( void ) { - /* Generate SysTick interrupt every SYS_TIME_RESOLUTION AHB_CLK */ - (void)SysTick_Config(SYS_TIME_RESOLUTION); - /* Set SysTick handler priority */ + /* Generate SysTick interrupt every SYS_TIME_RESOLUTION_CPU_TICKS + * The timer interrupt is activated on the transition from 1 to 0, + * therefore it activates every n+1 clock ticks. + */ + if (SysTick_Config(SYS_TIME_RESOLUTION_CPU_TICKS-1)) + while(1); /* if reload of value is impossible, go into endless loop */ + + /* Set SysTick handler priority */ NVIC_SetPriority(SysTick_IRQn, 0x0); sys_time.nb_sec = 0; sys_time.nb_sec_rem = 0; - sys_time.nb_tic = 0; + sys_time.nb_tick = 0; for (unsigned int i=0; i= SYS_TIME_TICS_PER_SEC) { - sys_time.nb_sec_rem -= SYS_TIME_TICS_PER_SEC; + sys_time.nb_tick++; + sys_time.nb_sec_rem += SYS_TIME_RESOLUTION_CPU_TICKS; + if (sys_time.nb_sec_rem >= CPU_TICKS_PER_SEC) { + sys_time.nb_sec_rem -= CPU_TICKS_PER_SEC; sys_time.nb_sec++; #ifdef SYS_TIME_LED LED_TOGGLE(SYS_TIME_LED); @@ -68,7 +73,7 @@ void sys_tick_irq_handler(void) { } for (unsigned int i=0; i= sys_time.timer[i].end_time) { + sys_time.nb_tick >= sys_time.timer[i].end_time) { sys_time.timer[i].end_time += sys_time.timer[i].duration; sys_time.timer[i].elapsed = TRUE; if (sys_time.timer[i].cb) sys_time.timer[i].cb(i); diff --git a/sw/airborne/arch/stm32/mcu_periph/sys_time_arch.h b/sw/airborne/arch/stm32/mcu_periph/sys_time_arch.h index aad8e7f812..3e51901f57 100644 --- a/sw/airborne/arch/stm32/mcu_periph/sys_time_arch.h +++ b/sw/airborne/arch/stm32/mcu_periph/sys_time_arch.h @@ -1,5 +1,4 @@ /* - * Paparazzi $Id$ * * Copyright (C) 2009-2010 The Paparazzi Team * @@ -27,8 +26,8 @@ * */ -#ifndef SYS_TIME_HW_H -#define SYS_TIME_HW_H +#ifndef SYS_TIME_ARCH_H +#define SYS_TIME_ARCH_H #include "mcu_periph/sys_time.h" @@ -36,14 +35,22 @@ #include #include "std.h" -#define InitSysTimePeriodic() +extern void sys_tick_irq_handler(void); -#define SYS_TIME_TICS_OF_SEC(s) (uint32_t)((s) * AHB_CLK + 0.5) -#define SYS_TIME_SIGNED_TICS_OF_SEC(s) (int32_t)((s) * AHB_CLK + 0.5) +#define CPU_TICKS_OF_SEC(s) (uint32_t)((s) * AHB_CLK + 0.5) +#define CPU_SIGNED_TICKS_OF_SEC(s) (int32_t)((s) * AHB_CLK + 0.5) -#define SysTimeTimerStart(_t) { _t = sys_time.nb_tic; } -#define SysTimeTimer(_t) (sys_time.nb_tic - (_t))) -#define SysTimeTimerStop(_t) { _t = (sys_time.nb_tic - (_t)); } +#define SEC_OF_CPU_TICKS(t) ((t) / AHB_CLK) +#define MSEC_OF_CPU_TICKS(t) ((t) / (AHB_CLK/1000)) +#define USEC_OF_CPU_TICKS(t) ((t) / (AHB_CLK/1000000)) + +#define GET_CUR_TIME_USEC() (sys_time.nb_sec * 1000000 + \ + USEC_OF_CPU_TICKS(sys_time.nb_sec_rem) + \ + USEC_OF_CPU_TICKS(SysTick->LOAD - SysTick->VAL)) + +#define SysTimeTimerStart(_t) { _t = GET_CUR_TIME_USEC(); } +#define SysTimeTimer(_t) ( GET_CUR_TIME_USEC() - (_t)) +#define SysTimeTimerStop(_t) { _t = ( GET_CUR_TIME_USEC() - (_t)); } /** Busy wait, in microseconds */ @@ -52,4 +59,4 @@ static inline void sys_time_usleep(uint32_t us) { } -#endif /* SYS_TIME_HW_H */ +#endif /* SYS_TIME_ARCH_H */ diff --git a/sw/airborne/firmwares/rotorcraft/main.c b/sw/airborne/firmwares/rotorcraft/main.c index b2a048c290..1244242216 100644 --- a/sw/airborne/firmwares/rotorcraft/main.c +++ b/sw/airborne/firmwares/rotorcraft/main.c @@ -75,7 +75,7 @@ int main( void ) { main_init(); while(1) { - if (sys_time_periodic()) + if (sys_time_check_and_ack_timer(0)) main_periodic(); main_event(); } @@ -87,7 +87,8 @@ STATIC_INLINE void main_init( void ) { mcu_init(); - sys_time_init(); + sys_time_register_timer(SYS_TIME_TIMER_S(1./512.), NULL); + electrical_init(); actuators_init(); diff --git a/sw/airborne/mcu.c b/sw/airborne/mcu.c index 1e8fee07d7..4e2b0890c6 100644 --- a/sw/airborne/mcu.c +++ b/sw/airborne/mcu.c @@ -25,6 +25,7 @@ #include "mcu.h" #ifdef PERIPHERALS_AUTO_INIT +#include "mcu_periph/sys_time.h" #ifdef USE_LED #include "led.h" #endif @@ -58,6 +59,7 @@ void mcu_init(void) { mcu_arch_init(); #ifdef PERIPHERALS_AUTO_INIT + sys_time_init(); #ifdef USE_LED led_init(); #endif diff --git a/sw/airborne/mcu_periph/sys_time.c b/sw/airborne/mcu_periph/sys_time.c index 06ef866d1b..7d9dbda495 100644 --- a/sw/airborne/mcu_periph/sys_time.c +++ b/sw/airborne/mcu_periph/sys_time.c @@ -7,7 +7,7 @@ struct sys_time sys_time; uint8_t sys_time_register_timer(uint32_t duration, sys_time_cb cb) { - uint32_t start_time = sys_time.nb_tic; + uint32_t start_time = sys_time.nb_tick; for (int i = 0; i< SYS_TIME_NB_TIMER; i++) { if (!sys_time.timer[i].in_use) { sys_time.timer[i].cb = cb; diff --git a/sw/airborne/mcu_periph/sys_time.h b/sw/airborne/mcu_periph/sys_time.h index a22c4b0a47..61bddc4ded 100644 --- a/sw/airborne/mcu_periph/sys_time.h +++ b/sw/airborne/mcu_periph/sys_time.h @@ -31,12 +31,13 @@ #define SYS_TIME_H #include +#include #include "std.h" #include BOARD_CONFIG #ifndef SYS_TIME_NB_TIMER -#define SYS_TIME_NB_TIMER 5 +#define SYS_TIME_NB_TIMER 5 #endif typedef void (*sys_time_cb) (uint8_t id); @@ -45,19 +46,23 @@ struct sys_time_timer { bool_t in_use; sys_time_cb cb; volatile bool_t elapsed; - uint32_t end_time; - uint32_t duration; + uint32_t end_time; ///< in SYS_TICKS + uint32_t duration; ///< in SYS_TICKS }; struct sys_time { - uint32_t nb_sec; - uint32_t nb_sec_rem; - uint32_t nb_tic; + uint32_t nb_sec; ///< full seconds since startup + uint32_t nb_sec_rem; ///< remainder of second in CPU_TICKS + uint32_t nb_tick; ///< in SYS_TICKS with SYS_TIME_RESOLUTION struct sys_time_timer timer[SYS_TIME_NB_TIMER]; }; extern struct sys_time sys_time; +//FIXME temporary hack +#define cpu_time_sec sys_time.nb_sec +#define cpu_time_ticks sys_time.nb_sec_rem + extern void sys_time_init(void); extern uint8_t sys_time_register_timer(uint32_t duration, sys_time_cb cb); extern void sys_time_cancel_timer(uint8_t id); @@ -71,18 +76,36 @@ static inline bool_t sys_time_check_and_ack_timer( uint8_t id ) { return FALSE; } +#define GET_CUR_TIME_FLOAT() ((float)sys_time.nb_sec + SEC_OF_CPU_TICKS((float)sys_time.nb_sec_rem)) + + +/* CPU clock */ +#define CPU_TICKS_OF_USEC(us) CPU_TICKS_OF_SEC((us) * 1e-6) +#define CPU_TICKS_OF_NSEC(ns) CPU_TICKS_OF_SEC((ns) * 1e-9) +#define CPU_SIGNED_TICKS_OF_USEC(us) CPU_SIGNED_TICKS_OF_SEC((us) * 1e-6) +#define CPU_SIGNED_TICKS_OF_NSEC(us) CPU_SIGNED_TICKS_OF_SEC((us) * 1e-9) + +#define CPU_TICKS_PER_SEC CPU_TICKS_OF_SEC( 1.) + + +/* paparazzi sys_time timers */ #ifndef SYS_TIME_RESOLUTION -#define SYS_TIME_RESOLUTION SYS_TIME_TICS_OF_SEC( 1./1048576.) +#define SYS_TIME_RESOLUTION ( 1./1024. ) #endif +#define SYS_TIME_RESOLUTION_CPU_TICKS CPU_TICKS_OF_SEC(SYS_TIME_RESOLUTION) -#define SYS_TIME_TIMER_S(_s) (SYS_TIME_TICS_OF_SEC(_s)/SYS_TIME_RESOLUTION) +#define SYS_TIME_TIMER_S(_s) ((_s)/SYS_TIME_RESOLUTION) -#define SYS_TIME_TICS_OF_USEC(us) SYS_TIME_TICS_OF_SEC((us) * 1e-6) -#define SYS_TIME_TICS_OF_NSEC(ns) SYS_TIME_TICS_OF_SEC((ns) * 1e-9) -#define SYS_TIME_SIGNED_TICS_OF_USEC(us) SYS_TIME_SIGNED_TICS_OF_SEC((us) * 1e-6) -#define SYS_TIME_SIGNED_TICS_OF_NSEC(us) SYS_TIME_SIGNED_TICS_OF_SEC((us) * 1e-9) +#define SYS_TIME_TICKS_OF_SEC(s) (uint32_t)((s) / SYS_TIME_RESOLUTION + 0.5) +#define SYS_TIME_TICKS_OF_USEC(us) SYS_TIME_TICKS_OF_SEC((us) * 1e-6) +#define SYS_TIME_TICKS_OF_NSEC(ns) SYS_TIME_TICKS_OF_SEC((ns) * 1e-9) + +#define SEC_OF_SYS_TIME_TICKS(t) ((t) * SYS_TIME_RESOLUTION) +#define USEC_OF_SYS_TIME_TICKS(t) ((t) * SYS_TIME_RESOLUTION / 1e-6) +#define NSEC_OF_SYS_TIME_TICKS(t) ((t) * SYS_TIME_RESOLUTION / 1e-9) + +#define USEC_OF_SEC(sec) ((sec) * 1e6) -#define SYS_TIME_TICS_PER_SEC SYS_TIME_TICS_OF_SEC( 1.) #include "mcu_periph/sys_time_arch.h" diff --git a/sw/airborne/test/mcu_periph/test_systime.c b/sw/airborne/test/mcu_periph/test_sys_time.c similarity index 84% rename from sw/airborne/test/mcu_periph/test_systime.c rename to sw/airborne/test/mcu_periph/test_sys_time.c index 922b282072..ec1b48d8bc 100644 --- a/sw/airborne/test/mcu_periph/test_systime.c +++ b/sw/airborne/test/mcu_periph/test_sys_time.c @@ -1,6 +1,7 @@ -#include "std_pprz.h" +#include "std.h" #include "mcu.h" -#include "subsystems/led.h" +#include "led.h" +#include "mcu_periph/sys_time.h" static inline void main_periodic_02( void ); static inline void main_periodic_03( void ); @@ -8,9 +9,8 @@ static inline void main_periodic_05( uint8_t id ); static inline void main_event( void ); int main(void) { - + mcu_init(); - led_init(); unsigned int tmr_02 = sys_time_register_timer(SYS_TIME_TIMER_S(0.2), NULL); unsigned int tmr_03 = sys_time_register_timer(SYS_TIME_TIMER_S(0.3), NULL); sys_time_register_timer(SYS_TIME_TIMER_S(0.5), main_periodic_05); @@ -26,25 +26,30 @@ int main(void) { return 0; } -/* - Called from main loop polling +/* + Called from main loop polling */ static inline void main_periodic_02( void ) { +#ifdef LED_GREEN LED_TOGGLE(LED_GREEN); +#endif } static inline void main_periodic_03( void ) { +#ifdef LED_BLUE LED_TOGGLE(LED_BLUE); +#endif } -/* +/* Called from the systime interrupt handler */ static inline void main_periodic_05( uint8_t id ) { +#ifdef LED_RED LED_TOGGLE(LED_RED); +#endif } static inline void main_event( void ) { } -