rotorcraft: handle_periodic_tasks to check all sys_time timer

- nps: use new systime - nps: renamed nps_autopilot_booz to nps_autopilot_rotorcraft
paparazzi · Feb 13, 2012 · ef76c8a · ef76c8a
1 parent cc07ac8
commit ef76c8a
Show file tree

Hide file tree

Showing 10 changed files with 39 additions and 32 deletions.
diff --git a/conf/Makefile.sim b/conf/Makefile.sim
@@ -56,16 +56,11 @@ CFLAGS += $(INCLUDES)
 CFLAGS += $($(TARGET).CFLAGS)
 CFLAGS += $(LOCAL_CFLAGS)
 CFLAGS += -O2
-CFLAGS += -std=gnu99
 
-# meschach prototypes trigger numerous warnings
-ifneq ($(SIM_TYPE),BOOZ)
 ifneq ($(SIM_TYPE),JSBSIM)
-CFLAGS	+= -Wstrict-prototypes
-endif
+CFLAGS += -std=gnu99
 endif
 
-
 LDFLAGS		=	-lm
 
 ifeq ($(SIM_TYPE),BOOZ)

diff --git a/conf/autopilot/rotorcraft.makefile b/conf/autopilot/rotorcraft.makefile
@@ -50,7 +50,7 @@ ap.srcs   += $(SRC_ARCH)/mcu_arch.c
 #
 # Math functions
 #
-$(TARGET).srcs += math/pprz_geodetic_int.c math/pprz_geodetic_float.c math/pprz_geodetic_double.c math/pprz_trig_int.c
+ap.srcs += math/pprz_geodetic_int.c math/pprz_geodetic_float.c math/pprz_geodetic_double.c math/pprz_trig_int.c
 
 ifeq ($(ARCH), stm32)
 ap.srcs += lisa/plug_sys.c
@@ -77,7 +77,7 @@ endif
 ifndef PERIODIC_FREQUENCY
 PERIODIC_FREQUENCY = 512
 endif
-$(TARGET).CFLAGS += -DPERIODIC_FREQUENCY=$(PERIODIC_FREQUENCY)
+ap.CFLAGS += -DPERIODIC_FREQUENCY=$(PERIODIC_FREQUENCY)
 #
 # Systime
 #

diff --git a/conf/autopilot/subsystems/rotorcraft/fdm_nps.makefile b/conf/autopilot/subsystems/rotorcraft/fdm_nps.makefile
@@ -51,7 +51,7 @@ sim.srcs += $(NPSDIR)/nps_main.c                      \
        $(NPSDIR)/nps_radio_control.c             \
        $(NPSDIR)/nps_radio_control_joystick.c    \
        $(NPSDIR)/nps_radio_control_spektrum.c    \
-       $(NPSDIR)/nps_autopilot_booz.c            \
+       $(NPSDIR)/nps_autopilot_rotorcraft.c            \
        $(NPSDIR)/nps_ivy.c                       \
        $(NPSDIR)/nps_flightgear.c                \
 
@@ -68,7 +68,7 @@ ifeq ($(TARGET), sim)
 endif
 
 
-sim.CFLAGS += -DPERIODIC_FREQUENCY='512.'
+sim.CFLAGS += -DPERIODIC_FREQUENCY=512
 #sim.CFLAGS += -DUSE_LED
 sim.srcs += mcu_periph/sys_time.c $(SRC_ARCH)/mcu_periph/sys_time_arch.c
 
@@ -85,9 +85,9 @@ sim.srcs   += $(SRC_FIRMWARE)/commands.c
 sim.srcs += $(SRC_FIRMWARE)/datalink.c
 
 #
+# Math functions
 #
-#
-
+sim.srcs += math/pprz_geodetic_int.c math/pprz_geodetic_float.c math/pprz_geodetic_double.c math/pprz_trig_int.c
 
 sim.CFLAGS += -DROTORCRAFT_BARO_LED=2
 sim.srcs += $(SRC_BOARD)/baro_board.c

diff --git a/conf/settings/settings_booz2_sim.xml b/conf/settings/settings_booz2_sim.xml
@@ -4,7 +4,7 @@
   <dl_settings>
 
     <dl_settings NAME="Sim">
-       <dl_setting var="nps_bypass_ahrs" min="0" step="1" max="1" module="nps/nps_autopilot_booz" shortname="bypass_ahrs" values="No|Yes"/>
+       <dl_setting var="nps_bypass_ahrs" min="0" step="1" max="1" module="nps/nps_autopilot_rotorcraft" shortname="bypass_ahrs" values="No|Yes"/>
     </dl_settings>
 
   </dl_settings>

diff --git a/sw/airborne/firmwares/rotorcraft/main.c b/sw/airborne/firmwares/rotorcraft/main.c
@@ -58,7 +58,7 @@
 #include "firmwares/rotorcraft/main.h"
 
 #ifdef SITL
-#include "nps_autopilot_booz.h"
+#include "nps_autopilot_rotorcraft.h"
 #endif
 
 #include "generated/modules.h"
@@ -70,7 +70,6 @@ static inline void on_baro_dif_event( void );
 static inline void on_gps_event( void );
 static inline void on_mag_event( void );
 
-#ifndef SITL
 
 tid_t main_periodic_tid; ///< id for main_periodic() timer
 tid_t failsafe_tid;      ///< id for failsafe_check() timer
@@ -79,22 +78,12 @@ tid_t electrical_tid;    ///< id for electrical_periodic() timer
 tid_t baro_tid;          ///< id for baro_periodic() timer
 tid_t telemetry_tid;     ///< id for telemetry_periodic() timer
 
+#ifndef SITL
 int main( void ) {
   main_init();
 
   while(1) {
-    if (sys_time_check_and_ack_timer(main_periodic_tid))
-      main_periodic();
-    if (sys_time_check_and_ack_timer(radio_control_tid))
-      radio_control_periodic_task();
-    if (sys_time_check_and_ack_timer(failsafe_tid))
-      failsafe_check();
-    if (sys_time_check_and_ack_timer(electrical_tid))
-      electrical_periodic();
-    if (sys_time_check_and_ack_timer(baro_tid))
-      baro_periodic();
-    if (sys_time_check_and_ack_timer(telemetry_tid))
-      telemetry_periodic();
+    handle_periodic_tasks();
     main_event();
   }
   return 0;
@@ -146,6 +135,20 @@ STATIC_INLINE void main_init( void ) {
   telemetry_tid = sys_time_register_timer((1./60.), NULL);
 }
 
+STATIC_INLINE void handle_periodic_tasks( void ) {
+  if (sys_time_check_and_ack_timer(main_periodic_tid))
+    main_periodic();
+  if (sys_time_check_and_ack_timer(radio_control_tid))
+    radio_control_periodic_task();
+  if (sys_time_check_and_ack_timer(failsafe_tid))
+    failsafe_check();
+  if (sys_time_check_and_ack_timer(electrical_tid))
+    electrical_periodic();
+  if (sys_time_check_and_ack_timer(baro_tid))
+    baro_periodic();
+  if (sys_time_check_and_ack_timer(telemetry_tid))
+    telemetry_periodic();
+}
 
 STATIC_INLINE void main_periodic( void ) {
 

diff --git a/sw/airborne/firmwares/rotorcraft/main.h b/sw/airborne/firmwares/rotorcraft/main.h
@@ -31,9 +31,10 @@
 #endif
 
 STATIC_INLINE void main_init( void );
-STATIC_INLINE void main_periodic( void );
 STATIC_INLINE void main_event( void );
+STATIC_INLINE void handle_periodic_tasks( void );
 
+STATIC_INLINE void main_periodic( void );
 STATIC_INLINE void telemetry_periodic(void);
 STATIC_INLINE void failsafe_check( void );
 

diff --git a/sw/simulator/nps/nps_autopilot.h b/sw/simulator/nps/nps_autopilot.h
@@ -13,6 +13,7 @@ extern struct NpsAutopilot autopilot;
 
 extern void nps_autopilot_init(enum NpsRadioControlType type, int num_script, char* js_dev);
 extern void nps_autopilot_run_step(double time);
+extern void nps_autopilot_run_systime_step(void);
 
 #endif /* NPS_AUTOPILOT_H */
 

diff --git a/sw/simulator/nps/nps_autopilot_booz.c → sw/simulator/nps/nps_autopilot_rotorcraft.c b/sw/simulator/nps/nps_autopilot_booz.c → sw/simulator/nps/nps_autopilot_rotorcraft.c
@@ -1,4 +1,4 @@
-#include "nps_autopilot_booz.h"
+#include "nps_autopilot_rotorcraft.h"
 
 #include "firmwares/rotorcraft/main.h"
 #include "nps_sensors.h"
@@ -8,6 +8,7 @@
 #include "subsystems/sensors/baro.h"
 #include "baro_board.h"
 #include "subsystems/electrical.h"
+#include "mcu_periph/sys_time.h"
 
 #include "actuators/supervision.h"
 
@@ -32,6 +33,10 @@ void nps_autopilot_init(enum NpsRadioControlType type_rc, int num_rc_script, cha
 
 }
 
+void nps_autopilot_run_systime_step( void ) {
+  sys_tick_handler();
+}
+
 #include <stdio.h>
 #include "subsystems/gps.h"
 
@@ -66,7 +71,7 @@ void nps_autopilot_run_step(double time __attribute__ ((unused))) {
     sim_overwrite_ahrs();
   }
 
-  main_periodic();
+  handle_periodic_tasks();
 
   if (time < 8) { /* start with a little bit of hovering */
     int32_t init_cmd[4];

diff --git a/sw/simulator/nps/nps_autopilot_booz.h → sw/simulator/nps/nps_autopilot_rotorcraft.h b/sw/simulator/nps/nps_autopilot_booz.h → sw/simulator/nps/nps_autopilot_rotorcraft.h
diff --git a/sw/simulator/nps/nps_main.c b/sw/simulator/nps/nps_main.c
@@ -54,7 +54,7 @@ void cont_hdl (int n __attribute__ ((unused))) {
    printf("Press <enter> to continue.\n");
  }
 
-double time_to_double(timeval *t) {
+double time_to_double(struct timeval *t) {
     return ((double)t->tv_sec + (double)(t->tv_usec * 1e-6));
 }
 
@@ -80,7 +80,7 @@ static void nps_main_init(void) {
 
   nps_main.sim_time = 0.;
   nps_main.display_time = 0.;
-  timeval t;
+  struct timeval t;
   gettimeofday (&t, NULL);
   nps_main.scaled_initial_time = time_to_double(&t);
   nps_main.host_time_factor = HOST_TIME_FACTOR;
@@ -114,6 +114,8 @@ static void nps_main_init(void) {
 static void nps_main_run_sim_step(void) {
   //  printf("sim at %f\n", nps_main.sim_time);
 
+  nps_autopilot_run_systime_step();
+
   nps_fdm_run_step(autopilot.commands);
 
   nps_sensors_run_step(nps_main.sim_time);