From ef76c8a866b7fd148f5a02fd2d85d971b0fb196a Mon Sep 17 00:00:00 2001 From: Felix Ruess Date: Mon, 13 Feb 2012 20:40:22 +0100 Subject: [PATCH] rotorcraft: handle_periodic_tasks to check all sys_time timer - nps: use new systime - nps: renamed nps_autopilot_booz to nps_autopilot_rotorcraft --- conf/Makefile.sim | 7 +---- conf/autopilot/rotorcraft.makefile | 4 +-- .../subsystems/rotorcraft/fdm_nps.makefile | 8 ++--- conf/settings/settings_booz2_sim.xml | 2 +- sw/airborne/firmwares/rotorcraft/main.c | 31 ++++++++++--------- sw/airborne/firmwares/rotorcraft/main.h | 3 +- sw/simulator/nps/nps_autopilot.h | 1 + ...ilot_booz.c => nps_autopilot_rotorcraft.c} | 9 ++++-- ...ilot_booz.h => nps_autopilot_rotorcraft.h} | 0 sw/simulator/nps/nps_main.c | 6 ++-- 10 files changed, 39 insertions(+), 32 deletions(-) rename sw/simulator/nps/{nps_autopilot_booz.c => nps_autopilot_rotorcraft.c} (92%) rename sw/simulator/nps/{nps_autopilot_booz.h => nps_autopilot_rotorcraft.h} (100%) diff --git a/conf/Makefile.sim b/conf/Makefile.sim index 34db63a13de..8097438bdfd 100644 --- 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 index 78b4e567515..052a38d8cb1 100644 --- 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 index a40b082b65a..4ae56636423 100644 --- 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 index 4c49b248155..d4b2b93c89b 100644 --- a/conf/settings/settings_booz2_sim.xml +++ b/conf/settings/settings_booz2_sim.xml @@ -4,7 +4,7 @@ - + diff --git a/sw/airborne/firmwares/rotorcraft/main.c b/sw/airborne/firmwares/rotorcraft/main.c index a65562effb7..f3278c871c4 100644 --- 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 index 63f5e068e1a..c548af8a58c 100644 --- 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 index 9e6c5b8438e..730c46cce04 100644 --- 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 b/sw/simulator/nps/nps_autopilot_rotorcraft.c similarity index 92% rename from sw/simulator/nps/nps_autopilot_booz.c rename to sw/simulator/nps/nps_autopilot_rotorcraft.c index 0d085d547ab..240e50d75b3 100644 --- a/sw/simulator/nps/nps_autopilot_booz.c +++ b/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 #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 b/sw/simulator/nps/nps_autopilot_rotorcraft.h similarity index 100% rename from sw/simulator/nps/nps_autopilot_booz.h rename to sw/simulator/nps/nps_autopilot_rotorcraft.h diff --git a/sw/simulator/nps/nps_main.c b/sw/simulator/nps/nps_main.c index ab04e02f34b..900d7ad2d89 100644 --- 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 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);