diff --git a/conf/airframes/ENAC/fixed-wing/apogee.xml b/conf/airframes/ENAC/fixed-wing/apogee.xml index be0f44ee21..3ab7562012 100644 --- a/conf/airframes/ENAC/fixed-wing/apogee.xml +++ b/conf/airframes/ENAC/fixed-wing/apogee.xml @@ -11,21 +11,22 @@ - + + - - - + + + diff --git a/conf/conf_tests.xml b/conf/conf_tests.xml index b62f3750f7..97bcee9bc6 100644 --- a/conf/conf_tests.xml +++ b/conf/conf_tests.xml @@ -312,10 +312,10 @@ ac_id="14" airframe="airframes/ENAC/fixed-wing/apogee.xml" radio="radios/T10CG_SBUS.xml" - telemetry="telemetry/default_fixedwing_imu.xml" + telemetry="telemetry/fixedwing_flight_recorder.xml" flight_plan="flight_plans/basic.xml" settings="settings/fixedwing_basic.xml" - settings_modules="modules/meteo_france_DAQ.xml" + settings_modules="" gui_color="blue" /> + + + + + Record flight data according to your telemetry file. + Data are stored on a SD card with the pprzlog format. + Currently only supported on Apogee/Fixedwing using chibios-libopencm3 implementation. + The selected telemetry file should include a FlightRecorder process. + + + + +
+ +
+ + + + + + + + diff --git a/conf/telemetry/fixedwing_flight_recorder.xml b/conf/telemetry/fixedwing_flight_recorder.xml new file mode 100644 index 0000000000..cd89f5dc68 --- /dev/null +++ b/conf/telemetry/fixedwing_flight_recorder.xml @@ -0,0 +1,138 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/sw/airborne/firmwares/fixedwing/chibios-libopencm3/chibios_init.c b/sw/airborne/firmwares/fixedwing/chibios-libopencm3/chibios_init.c index ddd453852b..f23e3c84c6 100644 --- a/sw/airborne/firmwares/fixedwing/chibios-libopencm3/chibios_init.c +++ b/sw/airborne/firmwares/fixedwing/chibios-libopencm3/chibios_init.c @@ -58,9 +58,7 @@ static WORKING_AREA(wa_thd_heartbeat, 2048); void chibios_launch_heartbeat (void); bool_t sdOk = FALSE; -#if LOG_PROCESS_STATE -static int32_t get_stack_free (Thread *tp); -#endif + /* * Init ChibiOS HAL and Sys @@ -105,26 +103,6 @@ static __attribute__((noreturn)) msg_t thd_heartbeat(void *arg) chThdSleepMilliseconds (sdOk == TRUE ? 1000 : 200); static uint32_t timestamp = 0; -#if LOG_PROCESS_STATE - sdLogWriteLog (&processLogFile, " addr stack frestk prio refs state time name\r\n"); -#endif - - // chSysDisable (); - Thread *tp = chRegFirstThread(); - do { - -#if LOG_PROCESS_STATE - sdLogWriteLog (&processLogFile, "%.8lx %.8lx %6lu %4lu %4lu [S:%d] %5lu %s\r\n", - (uint32_t)tp, (uint32_t)tp->p_ctx.r13, - get_stack_free (tp), - (uint32_t)tp->p_prio, (uint32_t)(tp->p_refs - 1), - tp->p_state, (uint32_t)tp->p_time, - chRegGetThreadName(tp)); -#endif - - tp = chRegNextThread(tp); - } while (tp != NULL); - // chSysEnable (); // we sync gps time to rtc every 5 seconds if (chTimeNow() - timestamp > 5000) { @@ -138,20 +116,3 @@ static __attribute__((noreturn)) msg_t thd_heartbeat(void *arg) } -#if LOG_PROCESS_STATE -static int32_t get_stack_free (Thread *tp) -{ - int32_t index = 0; - const uint8_t *maxRamAddr = (uint8_t*) (0x20000000 + (128*1024)); - const int32_t internalStructSize = 80; - - unsigned long long *stkAdr = (unsigned long long *) ((uint8_t *) tp + internalStructSize); - //unsigned long long *stkAdr = (unsigned long long *) tp; - - while ((stkAdr[index] == 0x5555555555555555) && ( ((uint8_t *) &(stkAdr[index])) < maxRamAddr)) - index++; - - const int32_t freeBytes = index * sizeof(long long); - return MAX(0, freeBytes - internalStructSize); -} -#endif diff --git a/sw/airborne/firmwares/fixedwing/main_ap.c b/sw/airborne/firmwares/fixedwing/main_ap.c index 8b7381e5f8..8c5998d3e8 100644 --- a/sw/airborne/firmwares/fixedwing/main_ap.c +++ b/sw/airborne/firmwares/fixedwing/main_ap.c @@ -484,7 +484,7 @@ void reporting_task(void) else { //PeriodicSendAp(DefaultChannel, DefaultDevice); #if PERIODIC_TELEMETRY - periodic_telemetry_send_Ap(&(DefaultChannel).trans_tx, &(DefaultDevice).device); + periodic_telemetry_send_Ap(DefaultPeriodic, &(DefaultChannel).trans_tx, &(DefaultDevice).device); #endif } } diff --git a/sw/airborne/firmwares/fixedwing/main_fbw.c b/sw/airborne/firmwares/fixedwing/main_fbw.c index 97072979dc..822f73b7f5 100644 --- a/sw/airborne/firmwares/fixedwing/main_fbw.c +++ b/sw/airborne/firmwares/fixedwing/main_fbw.c @@ -152,13 +152,13 @@ void init_fbw(void) #endif #if PERIODIC_TELEMETRY - register_periodic_telemetry(&telemetry_Fbw, "FBW_STATUS", send_fbw_status); - register_periodic_telemetry(&telemetry_Fbw, "COMMANDS", send_commands); + register_periodic_telemetry(DefaultPeriodic, "FBW_STATUS", send_fbw_status); + register_periodic_telemetry(DefaultPeriodic, "COMMANDS", send_commands); #ifdef ACTUATORS - register_periodic_telemetry(&telemetry_Fbw, "ACTUATORS", send_actuators); + register_periodic_telemetry(DefaultPeriodic, "ACTUATORS", send_actuators); #endif #ifdef RADIO_CONTROL - register_periodic_telemetry(&telemetry_Fbw, "RC", send_rc); + register_periodic_telemetry(DefaultPeriodic, "RC", send_rc); #endif #endif @@ -337,7 +337,7 @@ void periodic_task_fbw(void) #endif #if PERIODIC_TELEMETRY - periodic_telemetry_send_Fbw(&(DefaultChannel).trans_tx, &(DefaultDevice).device); + periodic_telemetry_send_Fbw(DefaultPeriodic, &(DefaultChannel).trans_tx, &(DefaultDevice).device); #endif } diff --git a/sw/airborne/firmwares/rotorcraft/main.c b/sw/airborne/firmwares/rotorcraft/main.c index fc5362964a..a315c834e6 100644 --- a/sw/airborne/firmwares/rotorcraft/main.c +++ b/sw/airborne/firmwares/rotorcraft/main.c @@ -259,7 +259,7 @@ STATIC_INLINE void telemetry_periodic(void) /* then report periodicly */ else { #if PERIODIC_TELEMETRY - periodic_telemetry_send_Main(&(DefaultChannel).trans_tx, &(DefaultDevice).device); + periodic_telemetry_send_Main(DefaultPeriodic, &(DefaultChannel).trans_tx, &(DefaultDevice).device); #endif } } diff --git a/sw/airborne/modules/digital_cam/uart_cam_ctrl.c b/sw/airborne/modules/digital_cam/uart_cam_ctrl.c index 1d397f34c1..5d88e293bb 100644 --- a/sw/airborne/modules/digital_cam/uart_cam_ctrl.c +++ b/sw/airborne/modules/digital_cam/uart_cam_ctrl.c @@ -129,7 +129,7 @@ void digital_cam_uart_init(void) } } #if PERIODIC_TELEMETRY - register_periodic_telemetry(&telemetry_Ap, "PAYLOAD", send_thumbnails); + register_periodic_telemetry(DefaultPeriodic, "PAYLOAD", send_thumbnails); #endif #ifdef SITL diff --git a/sw/airborne/modules/loggers/flight_recorder.c b/sw/airborne/modules/loggers/flight_recorder.c new file mode 100644 index 0000000000..7c2c1c233a --- /dev/null +++ b/sw/airborne/modules/loggers/flight_recorder.c @@ -0,0 +1,72 @@ +/* + * Copyright (C) 2015 Gautier Hattenberger + * + * This file is part of paparazzi + * + * paparazzi is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 2, or (at your option) + * any later version. + * + * paparazzi is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with paparazzi; see the file COPYING. If not, see + * . + */ +/** + * @file "modules/loggers/flight_recorder.c" + * @author Gautier Hattenberger + * Record flight data according to your telemetry file + */ + +#define PERIODIC_C_FLIGHTRECORDER + +#include "modules/loggers/flight_recorder.h" + +#include "subsystems/datalink/telemetry.h" +#include "subsystems/datalink/pprzlog_transport.h" + +#if FLIGHTRECORDER_SDLOG + +#include "sdLog.h" +#include "subsystems/chibios-libopencm3/chibios_sdlog.h" +static struct chibios_sdlog flightrecorder_sdlog; +#ifndef FLIGHTRECORDER_DEVICE +#define FLIGHTRECORDER_DEVICE flightrecorder_sdlog +#else +#warning "SD log is activated, but FLIGHTRECORDER_DEVICE is alreay set (should not be defined)" +#endif + +#else +// include downlink for other devices +#include "subsystems/datalink/downlink.h" +#endif + +#ifndef TELEMETRY_PROCESS_FlightRecorder +#error "You need to use a telemetry xml file with FlightRecorder process!" +#endif + +void flight_recorder_init() +{ +#if FLIGHTRECORDER_SDLOG + chibios_sdlog_init(&flightrecorder_sdlog, &flightRecorderLogFile); +#endif +} + +void flight_recorder_periodic() +{ +#if FLIGHTRECORDER_SDLOG + // test if sd log is ready + if (flightRecorderLogFile.fs == NULL) return; +#endif + +#if PERIODIC_TELEMETRY + periodic_telemetry_send_FlightRecorder(DefaultPeriodic, &pprzlog_tp.trans_tx, &(FLIGHTRECORDER_DEVICE).device); +#endif +} + + diff --git a/sw/airborne/modules/loggers/flight_recorder.h b/sw/airborne/modules/loggers/flight_recorder.h new file mode 100644 index 0000000000..844cf3aec2 --- /dev/null +++ b/sw/airborne/modules/loggers/flight_recorder.h @@ -0,0 +1,40 @@ +/* + * Copyright (C) 2015 Gautier Hattenberger + * + * This file is part of paparazzi + * + * paparazzi is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 2, or (at your option) + * any later version. + * + * paparazzi is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with paparazzi; see the file COPYING. If not, see + * . + */ +/** + * @file "modules/loggers/flight_recorder.h" + * @author Gautier Hattenberger + * Record flight data according to your telemetry file + */ + +#ifndef FLIGHT_RECORDER_H +#define FLIGHT_RECORDER_H + +/** Init function + */ +extern void flight_recorder_init(void); + +/** Periodic function + * + * should be called at TELEMETRY_FREQUENCY + */ +extern void flight_recorder_periodic(void); + +#endif + diff --git a/sw/airborne/subsystems/chibios-libopencm3/chibios_sdlog.c b/sw/airborne/subsystems/chibios-libopencm3/chibios_sdlog.c index da43b645ba..06b8e1f70d 100644 --- a/sw/airborne/subsystems/chibios-libopencm3/chibios_sdlog.c +++ b/sw/airborne/subsystems/chibios-libopencm3/chibios_sdlog.c @@ -46,13 +46,14 @@ EventListener powerOutageListener; FIL pprzLogFile = {0}; -#if LOG_PROCESS_STATE -static const char PROCESS_LOG_NAME[] = "processLog_"; -FIL processLogFile = {0}; -#endif - struct chibios_sdlog chibios_sdlog; +#if FLIGHTRECORDER_SDLOG +static const char FLIGHTRECORDER_LOG_NAME[] = "fr_"; +static const char FR_LOG_DIR[] = "FLIGHT_RECORDER"; +FIL flightRecorderLogFile = {0}; +#endif + static WORKING_AREA(waThdBatterySurvey, 4096); static void launchBatterySurveyThread (void) { @@ -68,27 +69,36 @@ static int sdlog_check_free_space(struct chibios_sdlog* p __attribute__((unused) return TRUE; } -static void sdlog_transmit(struct chibios_sdlog* p __attribute__((unused)), uint8_t byte) +static void sdlog_transmit(struct chibios_sdlog* p, uint8_t byte) { - sdLogWriteByte(&pprzLogFile, byte); + sdLogWriteByte(p->file, byte); } static void sdlog_send(struct chibios_sdlog* p __attribute__((unused))) { } static int null_function(struct chibios_sdlog *p __attribute__((unused))) { return 0; } +void chibios_sdlog_init(struct chibios_sdlog *sdlog, FIL *file) +{ + // Store file descriptor + sdlog->file = file; + // Configure generic device + sdlog->device.periph = (void *)(sdlog); + sdlog->device.check_free_space = (check_free_space_t) sdlog_check_free_space; + sdlog->device.put_byte = (put_byte_t) sdlog_transmit; + sdlog->device.send_message = (send_message_t) sdlog_send; + sdlog->device.char_available = (char_available_t) null_function; // write only + sdlog->device.get_byte = (get_byte_t) null_function; // write only + +} + bool_t chibios_logInit(const bool_t binaryFile) { nvicSetSystemHandlerPriority(HANDLER_PENDSV, CORTEX_PRIORITY_MASK(15)); - // Configure generic device - chibios_sdlog.device.periph = (void *)(&chibios_sdlog); - chibios_sdlog.device.check_free_space = (check_free_space_t) sdlog_check_free_space; - chibios_sdlog.device.put_byte = (put_byte_t) sdlog_transmit; - chibios_sdlog.device.send_message = (send_message_t) sdlog_send; - chibios_sdlog.device.char_available = (char_available_t) null_function; // write only - chibios_sdlog.device.get_byte = (get_byte_t) null_function; // write only + // Init sdlog struct + chibios_sdlog_init(&chibios_sdlog, &pprzLogFile); if (sdLogInit (NULL) != SDLOG_OK) goto error; @@ -96,8 +106,8 @@ bool_t chibios_logInit(const bool_t binaryFile) if (sdLogOpenLog (&pprzLogFile, PPRZ_LOG_DIR, PPRZ_LOG_NAME) != SDLOG_OK) goto error; -#if LOG_PROCESS_STATE - if (sdLogOpenLog (&processLogFile, PROCESS_LOG_NAME) != SDLOG_OK) +#if FLIGHTRECORDER_SDLOG + if (sdLogOpenLog (&flightRecorderLogFile, FR_LOG_DIR, FLIGHTRECORDER_LOG_NAME) != SDLOG_OK) goto error; #endif @@ -120,8 +130,8 @@ void chibios_logFinish(void) if (pprzLogFile.fs != NULL) { sdLogStopThread (); sdLogCloseLog (&pprzLogFile); -#if LOG_PROCESS_STATE - sdLogCloseLog (&processLogFile); +#if FLIGHTRECORDER_SDLOG + sdLogCloseLog (&flightRecorderLogFile); #endif sdLogFinish (); pprzLogFile.fs = NULL; diff --git a/sw/airborne/subsystems/chibios-libopencm3/chibios_sdlog.h b/sw/airborne/subsystems/chibios-libopencm3/chibios_sdlog.h index c3b106af46..6fc5496bd9 100644 --- a/sw/airborne/subsystems/chibios-libopencm3/chibios_sdlog.h +++ b/sw/airborne/subsystems/chibios-libopencm3/chibios_sdlog.h @@ -40,19 +40,24 @@ extern FIL pprzLogFile; -#if LOG_PROCESS_STATE +#if FLIGHTRECORDER_SDLOG // if activated, will log all process states -extern FIL processLogFile; +extern FIL flightRecorderLogFile; #endif extern bool_t chibios_logInit(const bool_t binaryFile); extern void chibios_logFinish(void); struct chibios_sdlog { + FIL *file; /** Generic device interface */ struct link_device device; }; extern struct chibios_sdlog chibios_sdlog; +/** init chibios_sdlog structure + */ +extern void chibios_sdlog_init(struct chibios_sdlog *sdlog, FIL *file); + #endif diff --git a/sw/airborne/subsystems/chibios-libopencm3/ffconf.h b/sw/airborne/subsystems/chibios-libopencm3/ffconf.h index c530ca8337..5edef24c8c 100644 --- a/sw/airborne/subsystems/chibios-libopencm3/ffconf.h +++ b/sw/airborne/subsystems/chibios-libopencm3/ffconf.h @@ -185,7 +185,12 @@ / function must be added to the project. */ -#define _FS_SHARE 0 /* 0:Disable or >=1:Enable */ +/* 0:Disable or >=1:Enable */ +#if FLIGHTRECORDER_SDLOG +#define _FS_SHARE 2 // Open a second file if flight recorder is used +#else +#define _FS_SHARE 0 +#endif /* To enable file shareing feature, set _FS_SHARE to 1 or greater. The value defines how many files can be opened simultaneously. */ diff --git a/sw/airborne/subsystems/datalink/telemetry.c b/sw/airborne/subsystems/datalink/telemetry.c index d7f512af7d..729d161a0a 100644 --- a/sw/airborne/subsystems/datalink/telemetry.c +++ b/sw/airborne/subsystems/datalink/telemetry.c @@ -28,6 +28,13 @@ */ #include "subsystems/datalink/telemetry_common.h" +#include "generated/periodic_telemetry.h" + +/** Implement global structures from generated header + */ +struct telemetry_msg telemetry_msgs[TELEMETRY_NB_MSG] = TELEMETRY_STRUCT; +struct periodic_telemetry pprz_telemetry = { TELEMETRY_NB_MSG, telemetry_msgs }; + /** Register a telemetry callback function. * @param _pt periodic telemetry structure to register @@ -35,9 +42,9 @@ * @param _cb callback function, called according to telemetry mode and specified period * @return TRUE if message registered with success, FALSE otherwise */ -bool_t register_periodic_telemetry(struct pprz_telemetry *_pt, const char *_msg, telemetry_cb _cb) +bool_t register_periodic_telemetry(struct periodic_telemetry *_pt, const char *_msg, telemetry_cb _cb) { - // return FALSE if NULL is passed as pprz_telemetry + // return FALSE if NULL is passed as periodic_telemetry if (_pt == NULL) { return FALSE; } // look for message name uint8_t i; diff --git a/sw/airborne/subsystems/datalink/telemetry.h b/sw/airborne/subsystems/datalink/telemetry.h index 5e7903d147..70fe3d6fc9 100644 --- a/sw/airborne/subsystems/datalink/telemetry.h +++ b/sw/airborne/subsystems/datalink/telemetry.h @@ -54,4 +54,18 @@ #include "subsystems/datalink/downlink.h" #include "generated/periodic_telemetry.h" +/** Global telemetry structures + * + * Contains the list of message and register callbacks. + * Filled with generated structure from periodic_telemetry.h + */ +extern struct telemetry_msg telemetry_msgs[TELEMETRY_NB_MSG]; +extern struct periodic_telemetry pprz_telemetry; + +/** Set default periodic telemetry + */ +#ifndef DefaultPeriodic +#define DefaultPeriodic (&pprz_telemetry) +#endif + #endif /* TELEMETRY_H */ diff --git a/sw/airborne/subsystems/datalink/telemetry_common.h b/sw/airborne/subsystems/datalink/telemetry_common.h index c547d788c9..025ee84567 100644 --- a/sw/airborne/subsystems/datalink/telemetry_common.h +++ b/sw/airborne/subsystems/datalink/telemetry_common.h @@ -34,12 +34,6 @@ #include "mcu_periph/link_device.h" #include "subsystems/datalink/transport.h" -/** Set default periodic telemetry to NULL - */ -#ifndef DefaultPeriodic -#define DefaultPeriodic NULL -#endif - /** Telemetry callback definition */ typedef void (*telemetry_cb)(struct transport_tx *trans, struct link_device *dev); @@ -51,11 +45,11 @@ struct telemetry_msg { telemetry_cb cb; ///< callback funtion }; -/** Telemetry structure. +/** Periodic telemetry structure. * Contains the total number of messages (from generated telemetry file) * and the list of registered callbacks */ -struct pprz_telemetry { +struct periodic_telemetry { uint8_t nb; ///< number of messages struct telemetry_msg *msgs; ///< the list of (msg name, callbacks) }; @@ -68,9 +62,9 @@ struct pprz_telemetry { * @return TRUE if message registered with success, FALSE otherwise */ #if PERIODIC_TELEMETRY -extern bool_t register_periodic_telemetry(struct pprz_telemetry *_pt, const char *_msg, telemetry_cb _cb); +extern bool_t register_periodic_telemetry(struct periodic_telemetry *_pt, const char *_msg, telemetry_cb _cb); #else -static inline bool_t register_periodic_telemetry(struct pprz_telemetry *_pt __attribute__((unused)), +static inline bool_t register_periodic_telemetry(struct periodic_telemetry *_pt __attribute__((unused)), const char *_msg __attribute__((unused)), telemetry_cb _cb __attribute__((unused))) { return FALSE; } #endif diff --git a/sw/airborne/subsystems/radio_control/ppm.c b/sw/airborne/subsystems/radio_control/ppm.c index b03cc0267c..daf1ce7d8c 100644 --- a/sw/airborne/subsystems/radio_control/ppm.c +++ b/sw/airborne/subsystems/radio_control/ppm.c @@ -50,12 +50,6 @@ static bool_t ppm_data_valid; #if PERIODIC_TELEMETRY -#ifdef FBW -#define DOWNLINK_TELEMETRY &telemetry_Fbw -#else -#define DOWNLINK_TELEMETRY DefaultPeriodic -#endif - #include "subsystems/datalink/telemetry.h" static void send_ppm(struct transport_tx *trans, struct link_device *dev) @@ -79,7 +73,7 @@ void radio_control_impl_init(void) ppm_arch_init(); #if PERIODIC_TELEMETRY - register_periodic_telemetry(DOWNLINK_TELEMETRY, "PPM", send_ppm); + register_periodic_telemetry(DefaultPeriodic, "PPM", send_ppm); #endif } diff --git a/sw/airborne/subsystems/radio_control/sbus.c b/sw/airborne/subsystems/radio_control/sbus.c index adc9cbd724..410628914b 100644 --- a/sw/airborne/subsystems/radio_control/sbus.c +++ b/sw/airborne/subsystems/radio_control/sbus.c @@ -34,12 +34,6 @@ struct Sbus sbus; // Telemetry function #if PERIODIC_TELEMETRY -#ifdef FBW -#define DOWNLINK_TELEMETRY &telemetry_Fbw -#else -#define DOWNLINK_TELEMETRY DefaultPeriodic -#endif - #include "subsystems/datalink/telemetry.h" static void send_sbus(struct transport_tx *trans, struct link_device *dev) @@ -57,7 +51,7 @@ void radio_control_impl_init(void) // Register telemetry message #if PERIODIC_TELEMETRY - register_periodic_telemetry(DOWNLINK_TELEMETRY, "PPM", send_sbus); + register_periodic_telemetry(DefaultPeriodic, "PPM", send_sbus); #endif } diff --git a/sw/airborne/subsystems/radio_control/sbus_dual.c b/sw/airborne/subsystems/radio_control/sbus_dual.c index 4f2954176f..05d90e1642 100644 --- a/sw/airborne/subsystems/radio_control/sbus_dual.c +++ b/sw/airborne/subsystems/radio_control/sbus_dual.c @@ -37,12 +37,6 @@ struct Sbus sbus1, sbus2; // Telemetry function #if PERIODIC_TELEMETRY -#ifdef FBW -#define DOWNLINK_TELEMETRY &telemetry_Fbw -#else -#define DOWNLINK_TELEMETRY DefaultPeriodic -#endif - #include "subsystems/datalink/telemetry.h" static void send_sbus(struct transport_tx *trans, struct link_device *dev) @@ -61,7 +55,7 @@ void radio_control_impl_init(void) // Register telemetry message #if PERIODIC_TELEMETRY - register_periodic_telemetry(DOWNLINK_TELEMETRY, "PPM", send_sbus); + register_periodic_telemetry(DefaultPeriodic, "PPM", send_sbus); #endif } diff --git a/sw/airborne/test/subsystems/test_ahrs.c b/sw/airborne/test/subsystems/test_ahrs.c index ad9b0a8e1f..5608f62947 100644 --- a/sw/airborne/test/subsystems/test_ahrs.c +++ b/sw/airborne/test/subsystems/test_ahrs.c @@ -57,6 +57,8 @@ static inline void on_mag_event(void); #define _DefaultAhrsRegister(_x) __DefaultAhrsRegister(_x) #define DefaultAhrsRegister() _DefaultAhrsRegister(DefaultAhrsImpl) +uint16_t datalink_time = 0; + int main(void) { main_init(); @@ -91,6 +93,7 @@ static inline void main_periodic_task(void) imu_periodic(); } RunOnceEvery(10, { LED_PERIODIC();}); + RunOnceEvery(PERIODIC_FREQUENCY, { datalink_time++; }); main_report(); } @@ -142,5 +145,5 @@ static inline void main_report(void) { RunOnceEvery(512, DOWNLINK_SEND_ALIVE(DefaultChannel, DefaultDevice, 16, MD5SUM)); - periodic_telemetry_send_Main(&(DefaultChannel).trans_tx, &(DefaultDevice).device); + periodic_telemetry_send_Main(DefaultPeriodic, &(DefaultChannel).trans_tx, &(DefaultDevice).device); } diff --git a/sw/tools/generators/gen_periodic.ml b/sw/tools/generators/gen_periodic.ml index fbd7937d69..fe02e8433b 100644 --- a/sw/tools/generators/gen_periodic.ml +++ b/sw/tools/generators/gen_periodic.ml @@ -82,12 +82,12 @@ let output_modes = fun out_h process_name modes freq modules -> l := (p, !phase) :: !l; i := !i + freq/10; right (); - lprintf out_h "if (telemetry_%s.msgs[TELEMETRY_%s_MSG_%s_ID].cb != NULL)\n" process_name (String.uppercase process_name) message_name; + lprintf out_h "if (telemetry->msgs[TELEMETRY_MSG_%s_ID].cb != NULL)\n" message_name; right (); - lprintf out_h "telemetry_%s.msgs[TELEMETRY_%s_MSG_%s_ID].cb(trans, dev);\n" process_name (String.uppercase process_name) message_name; + lprintf out_h "telemetry->msgs[TELEMETRY_MSG_%s_ID].cb(trans, dev);\n" message_name; left (); fprintf out_h "#if USE_PERIODIC_TELEMETRY_REPORT\n"; - lprintf out_h "else periodic_telemetry_err_report(TELEMETRY_PROCESS_%s, telemetry_mode_%s, TELEMETRY_%s_MSG_%s_ID);\n" process_name process_name (String.uppercase process_name) message_name; + lprintf out_h "else periodic_telemetry_err_report(TELEMETRY_PROCESS_%s, telemetry_mode_%s, TELEMETRY_MSG_%s_ID);\n" process_name process_name message_name; fprintf out_h "#endif\n"; left (); lprintf out_h "}\n" @@ -129,6 +129,77 @@ let write_settings = fun xml_file out_set telemetry_xml -> end; fprintf out_set "\n" +let print_message_table = fun out_h xml -> + let messages = Hashtbl.create 15 in + fprintf out_h "/* Periodic telemetry messages */\n"; + (* For each process *) + List.iter (fun process -> + (** For each mode of this process *) + List.iter (fun mode -> + (** For each message in this mode *) + List.iter (fun msg -> + let n = ExtXml.attrib msg "name" in + (* Add message to the list if it doesn't exist *) + if not (Hashtbl.mem messages n) then Hashtbl.add messages n () + ) (Xml.children mode) + ) (Xml.children process) + ) (Xml.children xml); + (* Print ID *) + let nb = Hashtbl.fold (fun n _ i -> + Xml2h.define (sprintf "TELEMETRY_MSG_%s_ID" n) (sprintf "%d" i); + i+1 + ) messages 0 in + Xml2h.define "TELEMETRY_NB_MSG" (sprintf "%d" nb); + (* Structure initialization *) + fprintf out_h "#define TELEMETRY_STRUCT { \\\n"; + Hashtbl.iter (fun n _ -> fprintf out_h " { \"%s\", NULL }, \\\n" n) messages; + fprintf out_h "};\n\n" + +let print_process_send = fun out_h xml freq modules -> + (** For each process *) + List.iter + (fun process -> + let process_name = ExtXml.attrib process "name" in + let modes = Xml.children process in + + fprintf out_h "\n/* Periodic telemetry: %s process */\n" process_name; + let p_id = ref 0 in + Xml2h.define (sprintf "TELEMETRY_PROCESS_%s" process_name) (string_of_int !p_id); + incr p_id; + + (** For each mode of this process *) + let _ = List.fold_left (fun i mode -> + let name = ExtXml.attrib mode "name" in + Xml2h.define (sprintf "TELEMETRY_MODE_%s_%s" process_name name) (string_of_int i); + (* Output the periods of the messages *) + List.iter (fun x -> + let p = ExtXml.attrib x "period" + and n = ExtXml.attrib x "name" in + (* FIXME really needed ? *) + Xml2h.define (sprintf "PERIOD_%s_%s_%d" n process_name i) (sprintf "(%s)" p) + ) (Xml.children mode); + i + 1 + ) 0 modes in + + fprintf out_h "\n/* Functions for %s process */\n" process_name; + fprintf out_h "#ifdef PERIODIC_C_%s\n" (String.uppercase process_name); + fprintf out_h "#ifndef TELEMETRY_MODE_%s\n" (String.uppercase process_name); + fprintf out_h "#define TELEMETRY_MODE_%s 0\n" (String.uppercase process_name); + fprintf out_h "#endif\n"; + fprintf out_h "uint8_t telemetry_mode_%s = TELEMETRY_MODE_%s;\n" process_name (String.uppercase process_name); + fprintf out_h "#else /* PERIODIC_C_%s not defined (general header) */\n" (String.uppercase process_name); + fprintf out_h "extern uint8_t telemetry_mode_%s;\n" process_name; + fprintf out_h "#endif /* PERIODIC_C_%s */\n" (String.uppercase process_name); + + lprintf out_h "static inline void periodic_telemetry_send_%s(struct periodic_telemetry *telemetry, struct transport_tx *trans, struct link_device *dev) { /* %dHz */\n" process_name freq; + right (); + output_modes out_h process_name modes freq modules; + left (); + lprintf out_h "}\n" + ) + (Xml.children xml) + + let _ = if Array.length Sys.argv <> 6 then begin @@ -146,6 +217,7 @@ let _ = let out_h = stdout in + (** Print header *) fprintf out_h "/* This file has been generated by gen_periodic from %s and %s */\n" Sys.argv.(2) Sys.argv.(3); fprintf out_h "/* Please DO NOT EDIT */\n\n"; fprintf out_h "#ifndef _VAR_PERIODIC_H_\n"; @@ -155,70 +227,11 @@ let _ = fprintf out_h "#include \"subsystems/datalink/telemetry_common.h\"\n\n"; fprintf out_h "#define TELEMETRY_FREQUENCY %d\n\n" freq; - (** For each process *) - List.iter - (fun process -> - let process_name = ExtXml.attrib process "name" in + (** Print the telemetry table with ID *) + print_message_table out_h telemetry_xml; - let modes = Xml.children process in - let messages = Hashtbl.create 5 in - - fprintf out_h "\n/* Periodic telemetry: %s process */\n" process_name; - let p_id = ref 0 in - Xml2h.define (sprintf "TELEMETRY_PROCESS_%s" process_name) (string_of_int !p_id); - incr p_id; - - let i = ref 0 in - (** For each mode of this process *) - List.iter (fun mode -> - let name = ExtXml.attrib mode "name" in - Xml2h.define (sprintf "TELEMETRY_MODE_%s_%s" process_name name) (string_of_int !i); - (* Output the periods of the messages *) - List.iter - (fun x -> - let p = ExtXml.attrib x "period" - and n = ExtXml.attrib x "name" in - (* add message to the list if it exists *) - if not (Hashtbl.mem messages n) then Hashtbl.add messages n (); - Xml2h.define (sprintf "PERIOD_%s_%s_%d" n process_name !i) (sprintf "(%s)" p)) (* FIXME really needed ? *) - (Xml.children mode); - incr i) - modes; - - let i = ref 0 in - (* Print message ID and total number *) - Hashtbl.iter (fun n _ -> - Xml2h.define (sprintf "TELEMETRY_%s_MSG_%s_ID" (String.uppercase process_name) n) (sprintf "%d" !i); - incr i; - ) messages; - Xml2h.define (sprintf "TELEMETRY_%s_NB_MSG" (String.uppercase process_name)) (sprintf "%d" !i); - - (* Structure initialization *) - fprintf out_h "#define TELEMETRY_%s_STRUCT { \\\n" (String.uppercase process_name); - Hashtbl.iter (fun n _ -> fprintf out_h " { \"%s\", NULL }, \\\n" n) messages; - fprintf out_h "};\n"; - - fprintf out_h "\n/* Functions for %s process */\n" process_name; - fprintf out_h "#ifdef PERIODIC_C_%s\n" (String.uppercase process_name); - fprintf out_h "#ifndef TELEMETRY_MODE_%s\n" (String.uppercase process_name); - fprintf out_h "#define TELEMETRY_MODE_%s 0\n" (String.uppercase process_name); - fprintf out_h "#endif\n"; - fprintf out_h "uint8_t telemetry_mode_%s = TELEMETRY_MODE_%s;\n" process_name (String.uppercase process_name); - fprintf out_h "struct telemetry_msg telemetry_msg_%s[TELEMETRY_%s_NB_MSG] = TELEMETRY_%s_STRUCT\n" process_name (String.uppercase process_name) (String.uppercase process_name); - fprintf out_h "struct pprz_telemetry telemetry_%s = { TELEMETRY_%s_NB_MSG, telemetry_msg_%s };\n" process_name (String.uppercase process_name) process_name; - fprintf out_h "#else /* PERIODIC_C_%s not defined (general header) */\n" (String.uppercase process_name); - fprintf out_h "extern uint8_t telemetry_mode_%s;\n" process_name; - fprintf out_h "extern struct telemetry_msg telemetry_msg_%s[TELEMETRY_%s_NB_MSG];\n" process_name (String.uppercase process_name); - fprintf out_h "extern struct pprz_telemetry telemetry_%s;\n" process_name; - fprintf out_h "#endif /* PERIODIC_C_%s */\n" (String.uppercase process_name); - - lprintf out_h "static inline void periodic_telemetry_send_%s(struct transport_tx *trans __attribute__((unused)), struct link_device *dev __attribute__((unused))) { /* %dHz */\n" process_name freq; (*TODO pass transport+device with correct types *) - right (); - output_modes out_h process_name modes freq modules_name; - left (); - lprintf out_h "}\n" - ) - (Xml.children telemetry_xml); + (** Print process sending functions *) + print_process_send out_h telemetry_xml freq modules_name; (** Output XML settings file with telemetry modes *) let out_set = open_out Sys.argv.(5) in