fix jsbsim simulation with new sys_time stuff

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

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_NB_TIMER; i++) {
+    if (sys_time.timer[i].in_use &&
+        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);
+    }
+  }
+}

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 <unistd.h>
+#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)

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) {

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);
 

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 <Ivy/ivy.h>
 
@@ -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