diff --git a/conf/autopilot/subsystems/fixedwing/autopilot.makefile b/conf/autopilot/subsystems/fixedwing/autopilot.makefile index d7b61c5485..bc64d1058c 100644 --- a/conf/autopilot/subsystems/fixedwing/autopilot.makefile +++ b/conf/autopilot/subsystems/fixedwing/autopilot.makefile @@ -56,8 +56,9 @@ $(TARGET).CFLAGS += -DTRAFFIC_INFO # # LEDs # - -$(TARGET).CFLAGS += -DUSE_LED +ifneq ($(ARCH), jsbsim) + $(TARGET).CFLAGS += -DUSE_LED +endif ifneq ($(ARCH), lpc21) ifneq ($(ARCH), jsbsim) $(TARGET).srcs += $(SRC_ARCH)/led_hw.c @@ -71,7 +72,8 @@ ifndef PERIODIC_FREQUENCY PERIODIC_FREQUENCY = 60 endif $(TARGET).CFLAGS += -DPERIODIC_FREQUENCY=$(PERIODIC_FREQUENCY) -$(TARGET).srcs += mcu_periph/sys_time.c $(SRC_ARCH)/mcu_periph/sys_time_arch.c +$(TARGET).srcs += mcu_periph/sys_time.c $(SRC_ARCH)/mcu_periph/sys_time_arch.c +$(TARGET).CFLAGS += -DUSE_SYS_TIME -DSYS_TIME_RESOLUTION='(1./$(PERIODIC_FREQUENCY).)' # # InterMCU & Commands @@ -123,11 +125,6 @@ ifneq ($(SYS_TIME_LED),none) ns_CFLAGS += -DSYS_TIME_LED=$(SYS_TIME_LED) endif -# -# Sys-time -# -ns_CFLAGS += -DUSE_SYS_TIME -DSYS_TIME_RESOLUTION='(1./$(PERIODIC_FREQUENCY).)' - # # UARTS diff --git a/sw/airborne/arch/sim/mcu_periph/sys_time_arch.c b/sw/airborne/arch/sim/mcu_periph/sys_time_arch.c index d516657fed..83ccbdfe27 100644 --- a/sw/airborne/arch/sim/mcu_periph/sys_time_arch.c +++ b/sw/airborne/arch/sim/mcu_periph/sys_time_arch.c @@ -26,3 +26,21 @@ void sys_time_arch_init( void ) { } + +void sys_tick_handler( void ) { + + 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++; + } + for (unsigned int i=0; i= 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/sim/mcu_periph/sys_time_arch.h b/sw/airborne/arch/sim/mcu_periph/sys_time_arch.h index a47b23f9da..da431f45bb 100644 --- a/sw/airborne/arch/sim/mcu_periph/sys_time_arch.h +++ b/sw/airborne/arch/sim/mcu_periph/sys_time_arch.h @@ -21,22 +21,24 @@ * */ -/* - *\brief simulator dummy timing functions - * +/** @file arch/sim/mcu_periph/sys_time_arch.h + * Simulator timing functions */ #ifndef SYS_TIME_ARCH_H #define SYS_TIME_ARCH_H -#include +#include "std.h" -#define CPU_TICKS_OF_SEC(x) (x) -#define SIGNED_CPU_TICKS_OF_SEC(x) (x) +extern void sys_tick_handler(void); -#define SEC_OF_CPU_TICKS(st) (st) -#define MSEC_OF_CPU_TICKS(st) (st) -#define USEC_OF_CPU_TICKS(st) (st) +// simulate 1us cpu ticks +#define CPU_TICKS_OF_SEC(s) (uint32_t)((s) * 1e6 + 0.5) +#define SIGNED_CPU_TICKS_OF_SEC(s) (int32_t)((s) * 1e6 + 0.5) + +#define SEC_OF_CPU_TICKS(t) ((t) / 1e6) +#define MSEC_OF_CPU_TICKS(t) ((t) / 1e3) +#define USEC_OF_CPU_TICKS(t) (t) #define SysTimeTimerStart(_t) { } #define SysTimeTimer(_t) (_t) diff --git a/sw/simulator/sim_ac_fw.c b/sw/simulator/sim_ac_fw.c index 8892722e75..6b3a5bda51 100644 --- a/sw/simulator/sim_ac_fw.c +++ b/sw/simulator/sim_ac_fw.c @@ -78,8 +78,8 @@ void autopilot_init(void) { } void autopilot_periodic_task(void) { - periodic_task_ap(); - periodic_task_fbw(); + handle_periodic_tasks_ap(); + handle_periodic_tasks_fbw(); } void autopilot_event_task(void) { diff --git a/sw/simulator/sim_ac_jsbsim.c b/sw/simulator/sim_ac_jsbsim.c index ee0a565943..57db201604 100644 --- a/sw/simulator/sim_ac_jsbsim.c +++ b/sw/simulator/sim_ac_jsbsim.c @@ -80,6 +80,9 @@ static void sim_init(void) { // main AP init (feed the sensors once before ?) autopilot_init(); + printf("sys_time resolution: %f\n", SYS_TIME_RESOLUTION); + printf("sys_time period in msec: %d\n", SYSTIME_PERIOD); + } static gboolean sim_periodic(gpointer data __attribute__ ((unused))) { @@ -116,6 +119,11 @@ static gboolean sim_periodic(gpointer data __attribute__ ((unused))) { return result; } +static gboolean systime_periodic(gpointer data __attribute__ ((unused))) { + sys_tick_handler(); + return true; +} + int main ( int argc, char** argv) { @@ -129,6 +137,7 @@ int main ( int argc, char** argv) { GMainLoop *ml = g_main_loop_new(NULL, FALSE); g_timeout_add(JSBSIM_PERIOD, sim_periodic, NULL); + g_timeout_add(SYSTIME_PERIOD, systime_periodic, NULL); g_main_loop_run(ml); diff --git a/sw/simulator/sim_ac_jsbsim.h b/sw/simulator/sim_ac_jsbsim.h index 44f3a8c7f5..05baa3087d 100644 --- a/sw/simulator/sim_ac_jsbsim.h +++ b/sw/simulator/sim_ac_jsbsim.h @@ -32,6 +32,7 @@ #include "std.h" #include "generated/airframe.h" #include "generated/flight_plan.h" +#include "mcu_periph/sys_time.h" #include @@ -43,6 +44,8 @@ #endif #define DT (JSBSIM_PERIOD*1e-3) ///< JSBSim timestep in seconds +#define SYSTIME_PERIOD ((uint32_t)(SYS_TIME_RESOLUTION * 1000)) ///< in msec + #define RAD2DEG 57.29578 #define FT2M 0.3048