diff --git a/conf/airframes/booz2_flixr.xml b/conf/airframes/booz2_flixr.xml index 6b2d6b02be..a737fe1728 100644 --- a/conf/airframes/booz2_flixr.xml +++ b/conf/airframes/booz2_flixr.xml @@ -207,6 +207,7 @@ #set GPS lag for horizontal filter ap.CFLAGS += -DGPS_LAG=0.8 -DUSE_GPS_ACC4R + ap.CFLAGS += -DTELEMETRY_STARTUP_DELAY=3 diff --git a/sw/airborne/booz/booz2_main.c b/sw/airborne/booz/booz2_main.c index fae7f4ef38..289fc983e8 100644 --- a/sw/airborne/booz/booz2_main.c +++ b/sw/airborne/booz/booz2_main.c @@ -119,28 +119,29 @@ STATIC_INLINE void booz2_main_periodic( void ) { booz2_autopilot_periodic(); /* set actuators */ actuators_set(booz2_autopilot_motors_on); - PeriodicPrescaleBy10( \ - { \ + PeriodicPrescaleBy10( \ + { \ radio_control_periodic(); \ - if (radio_control.status != RADIO_CONTROL_OK && booz2_autopilot_mode != BOOZ2_AP_MODE_KILL)\ - booz2_autopilot_set_mode(BOOZ2_AP_MODE_FAILSAFE); \ - }, \ - { \ - Booz2TelemetryPeriodic(); \ - }, \ - { \ - booz_fms_periodic(); \ - }, \ - { \ - /*BoozControlSurfacesSetFromCommands();*/ \ - }, \ - {}, \ - {}, \ - {}, \ - {}, \ - {}, \ - {} \ - ); \ + if (radio_control.status != RADIO_CONTROL_OK && booz2_autopilot_mode != BOOZ2_AP_MODE_KILL) \ + booz2_autopilot_set_mode(BOOZ2_AP_MODE_FAILSAFE);\ + }, \ + { \ + if (cpu_time_sec > TELEMETRY_STARTUP_DELAY) \ + Booz2TelemetryPeriodic(); \ + }, \ + { \ + booz_fms_periodic(); \ + }, \ + { \ + /*BoozControlSurfacesSetFromCommands();*/ \ + }, \ + {}, \ + {}, \ + {}, \ + {}, \ + {}, \ + {} \ + ); \ // t1 = T0TC; // diff = t1 - t0; diff --git a/sw/airborne/booz/booz2_telemetry.h b/sw/airborne/booz/booz2_telemetry.h index dd57466b68..c98a7c18d5 100644 --- a/sw/airborne/booz/booz2_telemetry.h +++ b/sw/airborne/booz/booz2_telemetry.h @@ -36,6 +36,10 @@ #include "actuators.h" +#ifndef TELEMETRY_STARTUP_DELAY +#define TELEMETRY_STARTUP_DELAY 0 +#endif + #define PERIODIC_SEND_ALIVE(_chan) DOWNLINK_SEND_ALIVE(_chan, 16, MD5SUM) #include "booz2_battery.h"