[Top][All Lists]
[Date Prev][Date Next][Thread Prev][Thread Next][Date Index][Thread Index]
[paparazzi-commits] [5178] Add onboard_transport for logging telemetry t
From: |
Allen Ibara |
Subject: |
[paparazzi-commits] [5178] Add onboard_transport for logging telemetry to onboard flash in text format |
Date: |
Tue, 27 Jul 2010 21:43:34 +0000 |
Revision: 5178
http://svn.sv.gnu.org/viewvc/?view=rev&root=paparazzi&revision=5178
Author: aibara
Date: 2010-07-27 21:43:34 +0000 (Tue, 27 Jul 2010)
Log Message:
-----------
Add onboard_transport for logging telemetry to onboard flash in text format
Added Paths:
-----------
paparazzi3/trunk/sw/airborne/fms/onboard_transport.c
paparazzi3/trunk/sw/airborne/fms/onboard_transport.h
Added: paparazzi3/trunk/sw/airborne/fms/onboard_transport.c
===================================================================
--- paparazzi3/trunk/sw/airborne/fms/onboard_transport.c
(rev 0)
+++ paparazzi3/trunk/sw/airborne/fms/onboard_transport.c 2010-07-27
21:43:34 UTC (rev 5178)
@@ -0,0 +1,216 @@
+#include <stdlib.h>
+#include <sys/types.h>
+#include <sys/stat.h>
+#include <fcntl.h>
+#include <stdio.h>
+#include <unistd.h>
+#include <time.h>
+#include <string.h>
+#include "onboard_transport.h"
+#include "downlink_transport.h"
+
+#define LOG_BUFLEN 512
+#define PATH_LEN 256
+#define FILENAME_LEN 64
+#define TIMESTAMP_SCALE 10000
+
+static void put_bytes(void *impl, enum DownlinkDataType data_type, uint8_t len
__attribute__((unused)), const uint8_t *bytes)
+{
+ struct onboard_transport *onboard = (struct onboard_transport *) impl;
+ uint32_t length = 0;
+
+ if (data_type == DL_TYPE_ARRAY_LENGTH) {
+ onboard->array_length = (uint8_t) *bytes;
+ return;
+ }
+
+ while (length++ <= onboard->array_length) {
+ if (onboard->array_length > 0 && length > 1) {
+ onboard->buffer_idx += snprintf(onboard->buffer + onboard->buffer_idx,
ONBOARD_BUFFER_LEN - onboard->buffer_idx, ",");
+ } else {
+ onboard->buffer_idx += snprintf(onboard->buffer + onboard->buffer_idx,
ONBOARD_BUFFER_LEN - onboard->buffer_idx, " ");
+ }
+ switch (data_type) {
+ case DL_TYPE_UINT8:
+ onboard->buffer_idx += snprintf(onboard->buffer + onboard->buffer_idx,
ONBOARD_BUFFER_LEN - onboard->buffer_idx, "%hhu", *bytes);
+ bytes = bytes + 1;
+ break;
+ case DL_TYPE_UINT16:
+ onboard->buffer_idx += snprintf(onboard->buffer + onboard->buffer_idx,
ONBOARD_BUFFER_LEN - onboard->buffer_idx, "%hu", * (const uint16_t *)bytes);
+ bytes = bytes + 2;
+ break;
+ case DL_TYPE_UINT32:
+ onboard->buffer_idx += snprintf(onboard->buffer + onboard->buffer_idx,
ONBOARD_BUFFER_LEN - onboard->buffer_idx, "%u", * (const uint32_t *)bytes);
+ bytes = bytes + 4;
+ break;
+ case DL_TYPE_UINT64:
+ onboard->buffer_idx += snprintf(onboard->buffer + onboard->buffer_idx,
ONBOARD_BUFFER_LEN - onboard->buffer_idx, "%llu", *(const uint64_t *)bytes);
+ bytes = bytes + 8;
+ break;
+ case DL_TYPE_INT8:
+ onboard->buffer_idx += snprintf(onboard->buffer + onboard->buffer_idx,
ONBOARD_BUFFER_LEN - onboard->buffer_idx, "%hhi", * (const int8_t *)bytes);
+ bytes = bytes + 1;
+ break;
+ case DL_TYPE_INT16:
+ onboard->buffer_idx += snprintf(onboard->buffer + onboard->buffer_idx,
ONBOARD_BUFFER_LEN - onboard->buffer_idx, "%hi", * (const int16_t *)bytes);
+ bytes = bytes + 2;
+ break;
+ case DL_TYPE_INT32:
+ onboard->buffer_idx += snprintf(onboard->buffer + onboard->buffer_idx,
ONBOARD_BUFFER_LEN - onboard->buffer_idx, "%i", * (const int32_t *)bytes);
+ bytes = bytes + 4;
+ break;
+ case DL_TYPE_INT64:
+ onboard->buffer_idx += snprintf(onboard->buffer + onboard->buffer_idx,
ONBOARD_BUFFER_LEN - onboard->buffer_idx, "%lli", *(const int64_t *)bytes);
+ bytes = bytes + 8;
+ break;
+ case DL_TYPE_FLOAT:
+ onboard->buffer_idx += snprintf(onboard->buffer + onboard->buffer_idx,
ONBOARD_BUFFER_LEN - onboard->buffer_idx, "%#f", *(const float *)bytes);
+ bytes = bytes + 4;
+ break;
+ case DL_TYPE_DOUBLE:
+ onboard->buffer_idx += snprintf(onboard->buffer + onboard->buffer_idx,
ONBOARD_BUFFER_LEN - onboard->buffer_idx, "%#f", *(const double *)bytes);
+ bytes = bytes + 8;
+ break;
+ case DL_TYPE_TIMESTAMP:
+ onboard->buffer_idx += snprintf(onboard->buffer + onboard->buffer_idx,
ONBOARD_BUFFER_LEN - onboard->buffer_idx, "%u.%04u", (*(const uint32_t *)bytes)
/ TIMESTAMP_SCALE,(*(const uint32_t *)bytes) % TIMESTAMP_SCALE);
+ bytes = bytes + 4;
+ break;
+ case DL_TYPE_ARRAY_LENGTH:
+ break;
+ }
+ }
+}
+
+static void start_message(void *impl, char *name, uint8_t msg_id, uint8_t
payload_len __attribute__((unused)))
+{
+ uint8_t ac_id = AC_ID;
+ struct onboard_transport *onboard = (struct onboard_transport *) impl;
+ onboard->buffer_idx = 0;
+ onboard->array_length = 0;
+
+ put_bytes(onboard, DL_TYPE_TIMESTAMP, 4, (uint8_t *) onboard->timestamp);
+ put_bytes(onboard, DL_TYPE_UINT8, 1, (uint8_t *) &ac_id);
+ onboard->buffer_idx += snprintf(onboard->buffer + onboard->buffer_idx,
ONBOARD_BUFFER_LEN - onboard->buffer_idx, " %s", name);
+ put_bytes(onboard, DL_TYPE_UINT8, 1, (uint8_t *) &msg_id);
+}
+
+static void end_message(void *impl)
+{
+ struct onboard_transport *onboard = (struct onboard_transport *) impl;
+ onboard->buffer_idx += snprintf(onboard->buffer + onboard->buffer_idx,
ONBOARD_BUFFER_LEN - onboard->buffer_idx, "\n");
+ if (write(onboard->fd, onboard->buffer, onboard->buffer_idx) < 0) {
+ onboard->overrun++;
+ }
+}
+
+static void overrun(void *impl)
+{
+ struct onboard_transport *onboard = (struct onboard_transport *) impl;
+ onboard->overrun++;
+}
+
+static void count_bytes(void *onboard __attribute__((unused)), uint8_t bytes
__attribute__((unused)))
+{
+
+}
+
+static int check_free_space(void *onboard __attribute__((unused)), uint8_t
bytes __attribute__((unused)))
+{
+ return TRUE;
+}
+
+static uint8_t size_of(void *onboard __attribute__((unused)), uint8_t len)
+{
+ return len + 2;
+}
+
+static int open_piped(char *filepath)
+{
+ int fd;
+ int pipe_fd[2];
+ int flags;
+ ssize_t count;
+ char buffer[LOG_BUFLEN];
+
+ if (pipe(pipe_fd) == -1) {
+ perror("onboard transport: pipe");
+ }
+
+ fd = open(filepath, O_CREAT | O_RDWR, 0644);
+ if (fd < 0) {
+ perror("onboard log: open");
+ }
+
+ if (fork() == 0) {
+ // This is the child, close the write side of the pipe
+ close(pipe_fd[1]);
+ int retval;
+
+ // copy from the read side of the pipe to the log
+ while (1) {
+ count = read(pipe_fd[0], buffer, LOG_BUFLEN);
+ retval = write(fd, buffer, count);
+ }
+ } else {
+ // This is the parent, close the read side of the pipe
+ close(pipe_fd[0]);
+
+ // Close the log file
+ close(fd);
+
+ // set non blocking on the write side of the pipe
+ flags = fcntl( pipe_fd[1], F_GETFL );
+ fcntl(pipe_fd[1], F_SETFL, flags | O_NONBLOCK);
+
+ // return the write side of the pipe
+ fd = pipe_fd[1];
+ }
+
+ return fd;
+}
+
+static void make_filename(char *filename)
+{
+ time_t t;
+ t = time(NULL);
+ struct tm *tmp;
+
+ tmp = localtime(&t);
+ if (tmp == NULL) {
+ perror("localtime");
+ }
+
+ // format DM_HHMM_SS
+ if (strftime(filename, FILENAME_LEN, "log_%d_%H%M_%S.data", tmp) == 0) {
+ fprintf(stderr, "strftime returned 0");
+ }
+}
+
+struct DownlinkTransport *onboard_transport_new(char *filepath, uint32_t
*timestamp)
+{
+ struct DownlinkTransport *tp = calloc(1, sizeof(struct DownlinkTransport));
+ struct onboard_transport *onboard = calloc(1, sizeof(struct
onboard_transport));
+
+ char full_filename[PATH_LEN];
+ char filename[FILENAME_LEN];
+
+ strncpy(full_filename, filepath, PATH_LEN);
+ make_filename(filename);
+ strncat(full_filename, filename, FILENAME_LEN);
+ int fd = open_piped(full_filename);
+
+ tp->impl = onboard;
+ onboard->fd = fd;
+ onboard->timestamp = timestamp;
+
+ tp->StartMessage = start_message;
+ tp->EndMessage = end_message;
+ tp->PutBytes = put_bytes;
+
+ tp->Overrun = overrun;
+ tp->CountBytes = count_bytes;
+ tp->SizeOf = size_of;
+ tp->CheckFreeSpace = check_free_space;
+
+ return tp;
+}
Added: paparazzi3/trunk/sw/airborne/fms/onboard_transport.h
===================================================================
--- paparazzi3/trunk/sw/airborne/fms/onboard_transport.h
(rev 0)
+++ paparazzi3/trunk/sw/airborne/fms/onboard_transport.h 2010-07-27
21:43:34 UTC (rev 5178)
@@ -0,0 +1,25 @@
+#ifndef ONBOARD_TRANSPORT_H
+#define ONBOARD_TRANSPORT_H
+
+#include "std.h"
+#include "airframe.h"
+
+#ifndef MSG_TIMESTAMP
+#define MSG_TIMESTAMP 0
+#endif
+
+#define ONBOARD_BUFFER_LEN 1500
+
+struct DownlinkTransport *onboard_transport_new(char *path, uint32_t
*timestamp);
+
+struct onboard_transport {
+ int fd;
+ char buffer[ONBOARD_BUFFER_LEN];
+ uint32_t buffer_idx;
+ uint32_t array_length;
+ uint32_t *timestamp;
+ uint32_t overrun;
+};
+
+#endif /* ONBOARD_TRANSPORT_H */
+
[Prev in Thread] |
Current Thread |
[Next in Thread] |
- [paparazzi-commits] [5178] Add onboard_transport for logging telemetry to onboard flash in text format,
Allen Ibara <=