Mirrors_Framework/paparazzi
1
0
Fork 0
mirror of https://github.com/paparazzi/paparazzi.git synced 2026-03-23 23:48:07 +08:00
Code Issues Actions 2 Packages Projects Releases Wiki Activity

[logger] adapt logger_control_effectiveness for linux based OS

Browse Source
This commit is contained in:
Gautier Hattenberger
2026-03-20 23:41:26 +01:00
parent 41572358f3
commit 618355b9b7
3 changed files with 100 additions and 18 deletions
Download Patch File Download Diff File Expand all files Collapse all files

2
conf/modules/logger_control_effectiveness.xml
View File

@@ -18,7 +18,7 @@
<header>
<file name="logger_control_effectiveness.h" />
</header>
<periodic fun="logger_control_effectiveness_periodic()" start="logger_control_effectiveness_start()" autorun="FALSE" />
<periodic fun="logger_control_effectiveness_periodic()" start="logger_control_effectiveness_start()" stop="logger_control_effectiveness_stop()" autorun="FALSE" />
<makefile target="ap">
<file name="logger_control_effectiveness.c"/>
</makefile>

115
sw/airborne/modules/loggers/logger_control_effectiveness.c
View File

@@ -24,10 +24,41 @@
*/
#include "modules/loggers/logger_control_effectiveness.h"
#include "modules/loggers/sdlog_chibios.h"
#include "mcu_periph/sys_time.h"
#include "state.h"
#if USE_CHIBIOS_RTOS
#include "modules/loggers/sdlog_chibios.h"
#define LogWrite sdLogWriteLog
#define LogFileIsOpen() (pprzLogFile != -1)
#define LogOpen(_file) {}
#define LogClose(_file) {}
#define LogFormatHeader "%.5f,%ld,%ld,%ld,%ld,%ld,%ld"
#define LogFormatVect3 ",%ld,%ld,%ld"
#else // assume Linux based OS
#include <stdio.h>
#include <time.h>
#include <unistd.h>
static void open_log(FILE* file);
#define LogWrite fprintf
#define LogFileIsOpen() (pprzLogFile != NULL)
#define LogOpen(_file) open_log(_file)
#define LogClose(_file) { \
fclose(_file); \
_file = NULL; \
}
#define LogFormatHeader "%.5f,%d,%d,%d,%d,%d,%d"
#define LogFormatVect3 ",%d,%d,%d"
static FILE* pprzLogFile = NULL;
/* Set the default log path to bebop storage */
#ifndef LOGGER_CONTROL_EFFECTIVENESS_FILE_PATH
#define LOGGER_CONTROL_EFFECTIVENESS_FILE_PATH /data/ftp/internal_000
#endif
#endif
// define parameters logged by default
#ifndef LOGGER_CONTROL_EFFECTIVENESS_COMMANDS
@@ -63,36 +94,44 @@
/** Write the log header line according to the enabled parts */
void logger_control_effectiveness_start(void)
{
if (pprzLogFile != -1) {
sdLogWriteLog(pprzLogFile, "time,gyro_p,gyro_q,gyro_r,ax,ay,az");
LogOpen(pprzLogFile);
if (LogFileIsOpen()) {
LogWrite(pprzLogFile, "time,gyro_p,gyro_q,gyro_r,ax,ay,az");
#if LOGGER_CONTROL_EFFECTIVENESS_COMMANDS
for (unsigned int i = 0; i < COMMANDS_NB; i++) {
sdLogWriteLog(pprzLogFile, ",cmd_%d", i);
LogWrite(pprzLogFile, ",cmd_%d", i);
}
#endif
#if LOGGER_CONTROL_EFFECTIVENESS_ACTUATORS
for (unsigned int i = 0; i < ACTUATORS_NB; i++) {
sdLogWriteLog(pprzLogFile, ",act_%d", i);
LogWrite(pprzLogFile, ",act_%d", i);
}
#endif
#if LOGGER_CONTROL_EFFECTIVENESS_POS
sdLogWriteLog(pprzLogFile, ",pos_x,pos_y,pos_z");
LogWrite(pprzLogFile, ",pos_x,pos_y,pos_z");
#endif
#if LOGGER_CONTROL_EFFECTIVENESS_SPEED
sdLogWriteLog(pprzLogFile, ",speed_x,speed_y,speed_z");
LogWrite(pprzLogFile, ",speed_x,speed_y,speed_z");
#endif
#if LOGGER_CONTROL_EFFECTIVENESS_AIRSPEED
sdLogWriteLog(pprzLogFile, ",eas");
LogWrite(pprzLogFile, ",eas");
#endif
sdLogWriteLog(pprzLogFile,"\n");
LogWrite(pprzLogFile,"\n");
}
}
void logger_control_effectiveness_stop(void)
{
if (LogFileIsOpen()) {
LogClose(pprzLogFile);
}
}
/** Log the values to file */
void logger_control_effectiveness_periodic(void)
{
if (pprzLogFile == -1) {
if (LogFileIsOpen()) {
return;
}
@@ -100,7 +139,7 @@ void logger_control_effectiveness_periodic(void)
struct Int32Vect3 *accel_body = stateGetAccelBody_i();
// log time, rate and accel
sdLogWriteLog(pprzLogFile, "%.5f,%ld,%ld,%ld,%ld,%ld,%ld",
LogWrite(pprzLogFile, LogFormatHeader,
get_sys_time_float(),
rates->p,
rates->q,
@@ -112,35 +151,77 @@ void logger_control_effectiveness_periodic(void)
// log commands
#if LOGGER_CONTROL_EFFECTIVENESS_COMMANDS
for (unsigned int i = 0; i < COMMANDS_NB; i++) {
sdLogWriteLog(pprzLogFile, ",%d", commands[i]);
LogWrite(pprzLogFile, ",%d", commands[i]);
}
#endif
// log actuators
#if LOGGER_CONTROL_EFFECTIVENESS_ACTUATORS
for (unsigned int i = 0; i < ACTUATORS_NB; i++) {
sdLogWriteLog(pprzLogFile, ",%d", actuators[i].pprz_val);
LogWrite(pprzLogFile, ",%d", actuators[i].pprz_val);
}
#endif
// log position
#if LOGGER_CONTROL_EFFECTIVENESS_POS
struct EnuCoor_i *pos = stateGetPositionEnu_i();
sdLogWriteLog(pprzLogFile, ",%ld,%ld,%ld", pos->x, pos->y, pos->z);
LogWrite(pprzLogFile, LogFormatVect3, pos->x, pos->y, pos->z);
#endif
// log speed
#if LOGGER_CONTROL_EFFECTIVENESS_SPEED
struct EnuCoor_i *speed = stateGetSpeedEnu_i();
sdLogWriteLog(pprzLogFile, ",%ld,%ld,%ld", speed->x, speed->y, speed->z);
LogWrite(pprzLogFile, LogFormatVect3, speed->x, speed->y, speed->z);
#endif
// log airspeed
#if LOGGER_CONTROL_EFFECTIVENESS_AIRSPEED
sdLogWriteLog(pprzLogFile, ",%.2f", stateGetAirspeed_f());
LogWrite(pprzLogFile, ",%.2f", stateGetAirspeed_f());
#endif
// end line
sdLogWriteLog(pprzLogFile,"\n");
LogWrite(pprzLogFile,"\n");
}
#if !USE_CHIBIOS_RTOS
static void open_log(FILE *file)
{
// Create output folder if necessary
if (access(STRINGIFY(LOGGER_CONTROL_EFFECTIVENESS_FILE_PATH), F_OK)) {
char save_dir_cmd[256];
sprintf(save_dir_cmd, "mkdir -p %s", STRINGIFY(LOGGER_CONTROL_EFFECTIVENESS_FILE_PATH));
if (system(save_dir_cmd) != 0) {
printf("[logger] Could not create log file directory %s.\n", STRINGIFY(LOGGER_CONTROL_EFFECTIVENESS_FILE_PATH));
return;
}
}
// Get current date/time for filename
char date_time[80];
time_t now = time(0);
struct tm tstruct;
tstruct = *localtime(&now);
strftime(date_time, sizeof(date_time), "%Y%m%d-%H%M%S", &tstruct);
uint32_t counter = 0;
char filename[512];
// Check for available files
sprintf(filename, "%s/%s.csv", STRINGIFY(LOGGER_CONTROL_EFFECTIVENESS_FILE_PATH), date_time);
while ((file = fopen(filename, "r"))) {
fclose(file);
sprintf(filename, "%s/%s_%05d.csv", STRINGIFY(LOGGER_FILE_PATH), date_time, counter);
counter++;
}
file = fopen(filename, "w");
if(!file) {
printf("[logger] ERROR opening log file %s!\n", filename);
return;
}
printf("[logger] Start logging to %s...\n", filename);
}
#endif

1
sw/airborne/modules/loggers/logger_control_effectiveness.h
View File

@@ -27,6 +27,7 @@
#define LOGGER_CONTROL_EFFECTIVENESS_H
extern void logger_control_effectiveness_start(void);
extern void logger_control_effectiveness_stop(void);
extern void logger_control_effectiveness_periodic(void);
#endif /* LOGGER_CONTROL_EFFECTIVENESS_H */