fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-24 11:13:34 +08:00
Code Issues Packages Projects Releases Wiki Activity

Rewrote SD logging app, simpler, but effective. Pending testing

Browse Source
This commit is contained in:
Lorenz Meier
2012-09-10 23:04:31 +02:00
parent 0019f65b10
commit e440fc4027
11 changed files with 267 additions and 964 deletions
Download Patch File Download Diff File Expand all files Collapse all files
apps/sdlog/sdlog.c
+251 -330
View File
@@ -1,8 +1,7 @@
/****************************************************************************
* examples/hello/main.c
*
* Copyright (C) 2008, 2011 Gregory Nutt. All rights reserved.
* Author: Gregory Nutt <spudmonkey@racsa.co.cr>
* Copyright (C) 2012 PX4 Development Team. All rights reserved.
* Author: @author Example User <mail@example.com>
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
@@ -14,7 +13,7 @@
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name NuttX nor the names of its contributors may be
* 3. Neither the name PX4 nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
@@ -33,152 +32,121 @@
*
****************************************************************************/
/**
* @file sdlog.c
* Simple SD logger for flight data
*/
#include <time.h>
#include <nuttx/config.h>
#include <sys/types.h>
#include <sys/stat.h>
#include <fcntl.h>
#include <errno.h>
#include <unistd.h>
#include <stdio.h>
#include <stdint.h>
#include <poll.h>
#include <stdlib.h>
#include <string.h>
#include <float.h>
#include <stdbool.h>
#include <sys/mount.h>
#include <sys/stat.h>
#include <time.h>
#include <pthread.h>
#include <fcntl.h>
#include <sys/prctl.h>
#include <errno.h>
#include <systemlib/err.h>
#include "sdlog.h"
#include "sdlog_generated.h"
#include <uORB/uORB.h>
#include <uORB/topics/sensor_combined.h>
#include <uORB/topics/vehicle_attitude.h>
#include <uORB/topics/vehicle_attitude_setpoint.h>
#include <uORB/topics/actuator_outputs.h>
#include <uORB/topics/actuator_controls.h>
/****************************************************************************
* Definitions
****************************************************************************/
static bool thread_should_exit = false; /**< Deamon exit flag */
static bool thread_running = false; /**< Deamon status flag */
static int deamon_task; /**< Handle of deamon task / thread */
static const int MAX_NO_LOGFOLDER = 999; /**< Maximum number of log folders */
static const char *mountpoint = "/fs/microsd";
/**
* Deamon management function.
*/
__EXPORT int sdlog_main(int argc, char *argv[]);
const char *src = "/dev/mmcsd0";
const char *trgt = "/fs/microsd";
const char *type = "vfat";
const char *logfile_end = ".px4log";
const char *mfile_end = ".m";
char folder_path[64];
/**
* Mainloop of deamon.
*/
int sdlog_thread_main(int argc, char *argv[]);
#define SDLOG_LED_PRIORITY LED_REQUEST_PRIORITY_MAX
#define BUFFER_BYTES 1000 // length of buffer
#define SAVE_EVERY_BYTES 2000
#define MAX_MOUNT_TRIES 5
/**
* Print the correct usage.
*/
static void usage(const char *reason);
static void sdlog_sig_handler(int signo, siginfo_t *info, void *ucontext); // is executed when SIGUSR1 is received
bool sdlog_sigusr1_rcvd; // if this is set to true through SIGUSR1, sdlog will terminate
static int file_exist(const char *filename);
static pthread_t logbuffer_thread; // thread to copy log values to the buffer
static void *logbuffer_loop(void *arg);
/**
* Create folder for current logging session.
*/
static int create_logfolder(char* folder_path);
uint8_t *buffer_start; // always points to the very beginning
uint8_t *buffer_end; // always points to the very end
uint8_t *buffer_cursor_start; // points to the start of the current buffer
uint8_t *buffer_cursor_end; // points to the end of the current buffer
size_t buffer_bytes_used; // amount stored in the buffer at the moment
uint32_t bytes_recv; // to count bytes received and written to the sdcard
uint32_t bytes_sent; // to count the bytes written to the buffer
/****************************************************************************
* user_start
****************************************************************************/
int file_exist(const char *filename)
static void
usage(const char *reason)
{
struct stat buffer;
return (stat(filename, &buffer) == 0);
if (reason)
fprintf(stderr, "%s\n", reason);
errx(1, "usage: sdlog {start|stop|status} [-p <additional params>]\n\n");
}
/**
* The deamon app only briefly exists to start
* the background job. The stack size assigned in the
* Makefile does only apply to this management task.
*
* The actual stack size should be set in the call
* to task_create().
*/
int sdlog_main(int argc, char *argv[])
{
// print text
printf("[sdlog] initialized\n");
usleep(10000);
if (argc < 1)
usage("missing command");
sdlog_sigusr1_rcvd = false;
if (!strcmp(argv[1], "start")) {
led_request_t amber_on_request = {.led = LED_AMBER, .priority = SDLOG_LED_PRIORITY, .request_type = LED_REQUEST_ON};
led_request_t amber_off_request = {.led = LED_AMBER, .priority = SDLOG_LED_PRIORITY, .request_type = LED_REQUEST_OFF};
amber_on_request.pid = getpid();
amber_off_request.pid = getpid();
/* signal handler to abort when low voltage occurs */
struct sigaction act;
struct sigaction oact;
act.sa_sigaction = sdlog_sig_handler;
(void)sigemptyset(&act.sa_mask);
(void)sigaddset(&act.sa_mask, SIGUSR1);
(void)sigaction(SIGUSR1, &act, &oact);
uint8_t mount_counter = 0;
commander_state_machine_t current_system_state;
global_data_send_led_request(&amber_on_request);
if (file_exist(trgt) == 1) {
printf("[sdlog] Mount already available at %s\n", trgt);
global_data_send_led_request(&amber_off_request);
} else {
printf("[sdlog] Mount not available yet, trying to mount...\n");
/* mostly the first mount fails, sometimes it helps to press the card onto the board! */
while (mount(src, trgt, type, 0, "") != 0) {
/* abort if kill signal is received */
if (sdlog_sigusr1_rcvd == true) {
return 0;
}
//make sure were not airborne
global_data_trylock(&global_data_sys_status->access_conf);
current_system_state = global_data_sys_status->state_machine;
global_data_unlock(&global_data_sys_status->access_conf);
if (current_system_state == SYSTEM_STATE_STANDBY || current_system_state == SYSTEM_STATE_PREFLIGHT || current_system_state == SYSTEM_STATE_GROUND_ERROR) {
usleep(1000000);
printf("[sdlog] ERROR: Failed to mount SD card (attempt %d of %d), reason: %s\n", mount_counter + 1, MAX_MOUNT_TRIES, strerror((int)*get_errno_ptr()));
global_data_send_led_request(&amber_off_request);
mount_counter++;
} else {
printf("[sdlog] WARNING: Not mounting SD card in flight!\n");
printf("[sdlog] ending now...\n");
fflush(stdout);
return 0;
}
if (mount_counter >= MAX_MOUNT_TRIES) {
printf("[sdlog] ERROR: SD card could not be mounted!\n");
printf("[sdlog] ending now...\n");
fflush(stdout);
return 0;
}
if (thread_running) {
printf("sdlog already running\n");
/* this is not an error */
exit(0);
}
printf("[sdlog] Mount created at %s...\n", trgt);
global_data_send_led_request(&amber_off_request);
thread_should_exit = false;
deamon_task = task_create("sdlog", SCHED_PRIORITY_DEFAULT - 10, 4096, sdlog_thread_main, (argv) ? (const char **)&argv[2] : (const char **)NULL);
thread_running = true;
exit(0);
}
if (!strcmp(argv[1], "stop")) {
thread_should_exit = true;
exit(0);
}
if (!strcmp(argv[1], "status")) {
if (thread_running) {
printf("\tsdlog is running\n");
} else {
printf("\tsdlog not started\n");
}
exit(0);
}
usage("unrecognized command");
exit(1);
}
int create_logfolder(char* folder_path) {
/* make folder on sdcard */
uint16_t foldernumber = 1; // start with folder 0001
int mkdir_ret;
/* look for the next folder that does not exist */
while (foldernumber < MAX_NO_LOGFOLDER) {
/* set up file path: e.g. /mnt/sdcard/logfile0001.txt */
sprintf(folder_path, "%s/session%04u", trgt, foldernumber);
/* set up file path: e.g. /mnt/sdcard/sensorfile0001.txt */
sprintf(folder_path, "%s/session%04u", mountpoint, foldernumber);
mkdir_ret = mkdir(folder_path, S_IRWXU | S_IRWXG | S_IRWXO);
/* the result is -1 if the folder exists */
@@ -192,251 +160,204 @@ int sdlog_main(int argc, char *argv[])
continue;
} else {
printf("[sdlog] ERROR: Failed creating new folder: %s\n", strerror((int)*get_errno_ptr()));
warn("failed creating new folder");
return -1;
}
}
if (foldernumber >= MAX_NO_LOGFOLDER) {
/* we should not end up here, either we have more than MAX_NO_LOGFOLDER on the SD card, or another problem */
printf("[sdlog] ERROR: all %d possible folders exist already\n", MAX_NO_LOGFOLDER);
warn("all %d possible folders exist already", MAX_NO_LOGFOLDER);
return -1;
}
return 0;
}
/* write m-file for evaluation in Matlab */
FILE *mfile;
char mfile_path[64] = ""; // string to hold the path to the mfile
const char *mfilename = "sdlog_eval";
int sdlog_thread_main(int argc, char *argv[]) {
sprintf(mfile_path, "%s/%s%s", folder_path, mfilename, mfile_end);
printf("[sdlog] starting\n");
if (NULL == (mfile = fopen(mfile_path, "w"))) {
printf("[sdlog] ERROR: opening %s failed: %s\n", mfile_path, strerror((int)*get_errno_ptr()));
if (file_exist(mountpoint) != OK) {
errx(1, "logging mount point %s not present, exiting.", mountpoint);
}
/* write file contents generated using updatesdlog.sh and mfile.template in nuttx/configs/px4fmu/include */
fwrite(MFILE_STRING, sizeof(MFILE_STRING), 1, mfile);
fprintf(mfile, "\n");
fclose(mfile);
printf("[sdlog] m-file written to sdcard\n");
char folder_path[64];
if (create_logfolder(folder_path))
errx(1, "unable to create logging folder, exiting");
/* create sensorfile */
int sensorfile;
FILE *gpsfile;
// FILE *vehiclefile;
/* create the ringbuffer */
uint8_t buffer[BUFFER_BYTES];
buffer_start = buffer;
buffer_cursor_start = buffer;
buffer_cursor_end = buffer;
buffer_end = buffer + BUFFER_BYTES;
char path_buf[64] = ""; // string to hold the path to the sensorfile
/* create loop to write log data in a ringbuffer */
pthread_attr_t logbuffer_attr;
pthread_attr_init(&logbuffer_attr);
pthread_attr_setstacksize(&logbuffer_attr, 2400);
pthread_create(&logbuffer_thread, &logbuffer_attr, logbuffer_loop, NULL);
/* create logfile */
FILE *logfile;
char logfile_path[64] = ""; // string to hold the path to the logfile
const char *logfilename = "all";
/* set up file path: e.g. /mnt/sdcard/session0001/gpslog.txt */
sprintf(logfile_path, "%s/%s%s", folder_path, logfilename, logfile_end);
if (NULL == (logfile = fopen(logfile_path, "wb"))) {
printf("[sdlog] opening %s failed: %s\n", logfile_path, strerror((int)*get_errno_ptr()));
/* set up file path: e.g. /mnt/sdcard/session0001/sensors_combined.bin */
sprintf(path_buf, "%s/%s.bin", folder_path, "sensors_combined");
if (0 == (sensorfile = open(path_buf, O_CREAT | O_WRONLY | O_DSYNC))) {
errx(1, "opening %s failed.\n", path_buf);
}
// else
// {
// printf("[sdlog] opening %s was successful\n",logfile_path);
// }
/* set up file path: e.g. /mnt/sdcard/session0001/gps.txt */
sprintf(path_buf, "%s/%s.txt", folder_path, "gps");
if (NULL == (gpsfile = fopen(path_buf, "w"))) {
errx(1, "opening %s failed.\n", path_buf);
}
int gpsfile_no = fileno(gpsfile);
int logfile_no = fileno(logfile);
bytes_recv = 0; /**< count all bytes that were received and written to the sdcard */
size_t ret_write; /**< last amount written to sdcard */
size_t target_write; /**< desired amount to write to sdcard */
int ret_fsync; /**< return value of fsync() system call */
int error_count = 0; /**< number of continous errors (one successful write resets it) */
/* Start logging */
while (1) {
/* write as soon as content is in the buffer */
while (buffer_bytes_used > 1) {
/* case where the data is not wrapped in the buffer */
if (buffer_cursor_start < buffer_cursor_end) {
/* write all available data */
target_write = (size_t)(buffer_cursor_end - buffer_cursor_start);
if (0 <= (ret_write = fwrite(buffer_cursor_start, 1, target_write, logfile))) {
/* decrease the amount stored in the buffer, normally to 0 */
buffer_bytes_used -= ret_write;
/* set new cursor position: wrap it in case it falls out of the buffer */
buffer_cursor_start = buffer_start + (buffer_cursor_start - buffer_start + ret_write) % BUFFER_BYTES;
bytes_recv += ret_write;
error_count = 0;
} else {
error_count++;
printf("[sdlog] ERROR: Write fail: %d of %d, %s\n", ret_write, target_write, strerror((int)*get_errno_ptr()));
}
}
/* case where the content is wrapped around the buffer */
else if (buffer_cursor_start > buffer_cursor_end) {
/* write data until the end of the buffer */
target_write = (size_t)(buffer_end - buffer_cursor_start);
if (0 <= (ret_write = fwrite(buffer_cursor_start, 1, target_write, logfile))) {
/* decrease the amount stored in the buffer */
buffer_bytes_used -= ret_write;
/* set new cursor position: wrap it in case it falls out of the buffer */
buffer_cursor_start = buffer_start + (buffer_cursor_start - buffer_start + ret_write) % BUFFER_BYTES;
bytes_recv += ret_write;
error_count = 0;
} else {
error_count++;
printf("[sdlog] ERROR: Write fail: %d of %d, %s\n", ret_write, target_write, strerror((int)*get_errno_ptr()));
}
/* write the rest of the data at the beginning of the buffer */
target_write = (size_t)(buffer_cursor_end - buffer_start);
if (0 <= (ret_write = fwrite(buffer_start, 1, target_write, logfile))) {
/* decrease the amount stored in the buffer, now normally to 0 */
buffer_bytes_used -= ret_write;
/* set new cursor position: wrap it in case it falls out of the buffer */
buffer_cursor_start = buffer_start + (buffer_cursor_start - buffer_start + ret_write) % BUFFER_BYTES;
bytes_recv += ret_write;
error_count = 0;
} else {
error_count++;
printf("[sdlog] ERROR: write fail: %d of %d, %s\n", ret_write, target_write, strerror((int)*get_errno_ptr()));
}
} else {
error_count++;
printf("[sdlog] ERROR: dropped data\n");
}
// if(bytes_recv>500000)
// {
// goto finished;
// }
}
/* --- IMPORTANT: DEFINE NUMBER OF ORB STRUCTS TO WAIT FOR HERE --- */
/* number of messages */
const ssize_t fdsc = 25;
/* Sanity check variable and index */
ssize_t fdsc_count = 0;
/* file descriptors to wait for */
struct pollfd fds[fdsc];
/* save and exit if we received signal 1 or have a permanent error */
if (sdlog_sigusr1_rcvd == true || error_count > 100) {
fclose(logfile);
umount(trgt);
union {
struct sensor_combined_s raw;
struct vehicle_attitude_s att;
struct vehicle_attitude_setpoint_s att_sp;
struct actuator_outputs_s act_outputs;
struct actuator_controls_s actuators;
} buf;
if (error_count > 100) {
return ERROR;
struct {
int sensor_sub;
int att_sub;
int spa_sub;
int act_0_sub;
int actuators_sub;
} subs;
} else {
return OK;
}
}
/* --- SENSORS RAW VALUE --- */
/* subscribe to ORB for sensors raw */
subs.sensor_sub = orb_subscribe(ORB_ID(sensor_combined));
fds[fdsc_count].fd = subs.sensor_sub;
fds[fdsc_count].events = POLLIN;
fdsc_count++;
/* save file from time to time */
else if ((bytes_recv > 0 && bytes_recv % SAVE_EVERY_BYTES == 0)) {
if ((ret_fsync = fsync(logfile_no)) != OK) {
printf("[sdlog] ERROR: sync fail: #%d, %s\n", ret_fsync, strerror((int)*get_errno_ptr()));
}
}
/* --- ATTITUDE VALUE --- */
/* subscribe to ORB for attitude */
subs.att_sub = orb_subscribe(ORB_ID(vehicle_attitude));
orb_set_interval(subs.att_sub, 100);
fds[fdsc_count].fd = subs.att_sub;
fds[fdsc_count].events = POLLIN;
fdsc_count++;
// else if(bytes_recv > 2*SAVE_EVERY_BYTES)
// {
// goto finished;
// }
/* sleep, not to block everybody else */
usleep(1000);
/* --- ATTITUDE SETPOINT VALUE --- */
/* subscribe to ORB for attitude setpoint */
/* struct already allocated */
subs.spa_sub = orb_subscribe(ORB_ID(vehicle_attitude_setpoint));
orb_set_interval(subs.spa_sub, 2000); /* 0.5 Hz updates */
fds[fdsc_count].fd = subs.spa_sub;
fds[fdsc_count].events = POLLIN;
fdsc_count++;
/** --- ACTUATOR OUTPUTS --- */
subs.act_0_sub = orb_subscribe(ORB_ID(actuator_outputs_0));
fds[fdsc_count].fd = subs.act_0_sub;
fds[fdsc_count].events = POLLIN;
fdsc_count++;
/* --- ACTUATOR CONTROL VALUE --- */
/* subscribe to ORB for actuator control */
subs.actuators_sub = orb_subscribe(ORB_ID_VEHICLE_ATTITUDE_CONTROLS);
fds[fdsc_count].fd = subs.actuators_sub;
fds[fdsc_count].events = POLLIN;
fdsc_count++;
/* WARNING: If you get the error message below,
* then the number of registered messages (fdsc)
* differs from the number of messages in the above list.
*/
if (fdsc_count > fdsc) {
warn("WARNING: Not enough space for poll fds allocated. Check %s:%d.\n", __FILE__, __LINE__);
fdsc_count = fdsc;
}
//finished:
//
// /* at the moment we should not end here */
// fclose(logfile);
// printf("[sdlog] logfile saved\n");
// umount(trgt);
// printf("[sdlog] ending...\n");
// fflush(stdout);
/*
* set up poll to block for new data,
* wait for a maximum of 1000 ms (1 second)
*/
const int timeout = 1000;
thread_running = true;
while (!thread_should_exit) {
int poll_ret = poll(fds, fdsc_count, timeout);
/* handle the poll result */
if (poll_ret == 0) {
/* XXX this means none of our providers is giving us data - might be an error? */
} else if (poll_ret < 0) {
/* XXX this is seriously bad - should be an emergency */
} else {
int ifds = 0;
/* --- SENSORS RAW VALUE --- */
if (fds[ifds++].revents & POLLIN) {
/* copy sensors raw data into local buffer */
orb_copy(ORB_ID(sensor_combined), subs.sensor_sub, &buf.raw);
/* write out */
write(sensorfile, (const char*)&buf.raw, sizeof(buf.raw));
}
/* --- ATTITUDE VALUE --- */
if (fds[ifds++].revents & POLLIN) {
/* copy attitude data into local buffer */
orb_copy(ORB_ID(vehicle_attitude), subs.att_sub, &buf.att);
}
/* --- VEHICLE ATTITUDE SETPOINT --- */
if (fds[ifds++].revents & POLLIN) {
/* copy local position data into local buffer */
orb_copy(ORB_ID(vehicle_attitude_setpoint), subs.spa_sub, &buf.att_sp);
}
/* --- ACTUATOR OUTPUTS 0 --- */
if (fds[ifds++].revents & POLLIN) {
/* copy actuator data into local buffer */
orb_copy(ORB_ID(actuator_outputs_0), subs.act_0_sub, &buf.act_outputs);
}
/* --- ACTUATOR CONTROL --- */
if (fds[ifds++].revents & POLLIN) {
orb_copy(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, subs.actuators_sub, &buf.actuators);
}
/* enforce write to disk */
fsync(sensorfile);
fsync(gpsfile_no);
}
}
warn("exiting.");
close(sensorfile);
fclose(gpsfile);
thread_running = false;
return 0;
}
/* is executed when SIGUSR1 is raised (to terminate the app) */
static void sdlog_sig_handler(int signo, siginfo_t *info, void *ucontext)
/**
* @return 0 if file exists
*/
int file_exist(const char *filename)
{
sdlog_sigusr1_rcvd = true;
struct stat buffer;
return stat(filename, &buffer);
}
static void *logbuffer_loop(void *arg)
{
/* set name for this pthread */
prctl(PR_SET_NAME, "sdlog logbuffer", getpid());
bytes_sent = 0; // count all bytes written to the buffer
//size_t block_length = sizeof(global_data_sensors_raw_t) - 4*sizeof(access_conf_t);
size_t first_block_length;
size_t second_block_length;
buffer_bytes_used = 0;
log_block_t log_block = {.check = {'$', '$', '$', '$'}};
size_t block_length = sizeof(log_block_t);
// printf("Block length is: %u",(uint16_t)block_length);
// uint16_t test_counter = 0;
/* start copying data to buffer */
while (1) {
copy_block(&log_block);
/* no more free space in buffer */
if (buffer_bytes_used + (uint16_t)block_length > BUFFER_BYTES) {
printf("[sdlog] buf full, skipping\n");
}
/* data needs to be wrapped around ringbuffer*/
else if (buffer_cursor_end + block_length >= buffer_end) {
/* first block length is from cursor until the end of the buffer */
first_block_length = (size_t)(buffer_end - buffer_cursor_end);
/* second block length is the rest */
second_block_length = block_length - first_block_length;
/* copy the data, not the pointer conversion */
memcpy(buffer_cursor_end, ((uint8_t *) & (log_block)), first_block_length);
memcpy(buffer_start, ((uint8_t *) & (log_block)) + first_block_length, second_block_length);
/* set the end position of the cursor */
buffer_cursor_end = buffer_start + second_block_length;
/* increase the amount stored in the buffer */
buffer_bytes_used += block_length;
bytes_sent += block_length;
}
/* data does not need to be wrapped around */
else {
/* copy the whole block in one step */
memcpy(buffer_cursor_end, ((uint8_t *) & (log_block)), block_length);
/* set the cursor to 0 of the buffer instead of the end */
buffer_cursor_end = buffer_start + (buffer_cursor_end - buffer_start + block_length) % BUFFER_BYTES;
/* increase the amount stored in the buffer */
buffer_bytes_used += block_length;
bytes_sent += block_length;
}
/* also end the pthread when we receive a SIGUSR1 */
if (sdlog_sigusr1_rcvd == true) {
break;
}
}
return NULL;
}