Merge pull request #287 from DrTon/sdlog2

sdlog2 - new APM compatible logger

src/modules/sdlog2/logbuffer.c

@@ -0,0 +1,133 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2013 PX4 Development Team. All rights reserved.
+ *   Author: Anton Babushkin <rk3dov@gmail.com>
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 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.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file logbuffer.c
+ *
+ * Ring FIFO buffer for binary log data.
+ *
+ * @author Anton Babushkin <rk3dov@gmail.com>
+ */
+
+#include <string.h>
+#include <stdlib.h>
+
+#include "logbuffer.h"
+
+void logbuffer_init(struct logbuffer_s *lb, int size)
+{
+	lb->size  = size;
+	lb->write_ptr = 0;
+	lb->read_ptr = 0;
+	lb->data = malloc(lb->size);
+}
+
+int logbuffer_count(struct logbuffer_s *lb)
+{
+	int n = lb->write_ptr - lb->read_ptr;
+
+	if (n < 0) {
+		n += lb->size;
+	}
+
+	return n;
+}
+
+int logbuffer_is_empty(struct logbuffer_s *lb)
+{
+	return lb->read_ptr == lb->write_ptr;
+}
+
+bool logbuffer_write(struct logbuffer_s *lb, void *ptr, int size)
+{
+	// bytes available to write
+	int available = lb->read_ptr - lb->write_ptr - 1;
+
+	if (available < 0)
+		available += lb->size;
+
+	if (size > available) {
+		// buffer overflow
+		return false;
+	}
+
+	char *c = (char *) ptr;
+	int n = lb->size - lb->write_ptr;	// bytes to end of the buffer
+
+	if (n < size) {
+		// message goes over end of the buffer
+		memcpy(&(lb->data[lb->write_ptr]), c, n);
+		lb->write_ptr = 0;
+
+	} else {
+		n = 0;
+	}
+
+	// now: n = bytes already written
+	int p = size - n;	// number of bytes to write
+	memcpy(&(lb->data[lb->write_ptr]), &(c[n]), p);
+	lb->write_ptr = (lb->write_ptr + p) % lb->size;
+	return true;
+}
+
+int logbuffer_get_ptr(struct logbuffer_s *lb, void **ptr, bool *is_part)
+{
+	// bytes available to read
+	int available = lb->write_ptr - lb->read_ptr;
+
+	if (available == 0) {
+		return 0;	// buffer is empty
+	}
+
+	int n = 0;
+
+	if (available > 0) {
+		// read pointer is before write pointer, all available bytes can be read
+		n = available;
+		*is_part = false;
+
+	} else {
+		// read pointer is after write pointer, read bytes from read_ptr to end of the buffer
+		n = lb->size - lb->read_ptr;
+		*is_part = true;
+	}
+
+	*ptr = &(lb->data[lb->read_ptr]);
+	return n;
+}
+
+void logbuffer_mark_read(struct logbuffer_s *lb, int n)
+{
+	lb->read_ptr = (lb->read_ptr + n) % lb->size;
+}

src/modules/sdlog2/logbuffer.h

@@ -0,0 +1,68 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2013 PX4 Development Team. All rights reserved.
+ *   Author: Anton Babushkin <rk3dov@gmail.com>
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 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.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file logbuffer.h
+ *
+ * Ring FIFO buffer for binary log data.
+ *
+ * @author Anton Babushkin <rk3dov@gmail.com>
+ */
+
+#ifndef SDLOG2_RINGBUFFER_H_
+#define SDLOG2_RINGBUFFER_H_
+
+#include <stdbool.h>
+
+struct logbuffer_s {
+	// all pointers are in bytes
+	int write_ptr;
+	int read_ptr;
+	int size;
+	char *data;
+};
+
+void logbuffer_init(struct logbuffer_s *lb, int size);
+
+int logbuffer_count(struct logbuffer_s *lb);
+
+int logbuffer_is_empty(struct logbuffer_s *lb);
+
+bool logbuffer_write(struct logbuffer_s *lb, void *ptr, int size);
+
+int logbuffer_get_ptr(struct logbuffer_s *lb, void **ptr, bool *is_part);
+
+void logbuffer_mark_read(struct logbuffer_s *lb, int n);
+
+#endif

src/modules/sdlog2/module.mk

@@ -0,0 +1,43 @@
+############################################################################
+#
+#   Copyright (c) 2013 PX4 Development Team. All rights reserved.
+#
+# Redistribution and use in source and binary forms, with or without
+# modification, are permitted provided that the following conditions
+# are met:
+#
+# 1. Redistributions of source code must retain the above copyright
+#    notice, this list of conditions and the following disclaimer.
+# 2. Redistributions in binary form must reproduce the above copyright
+#    notice, this list of conditions and the following disclaimer in
+#    the documentation and/or other materials provided with the
+#    distribution.
+# 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.
+#
+# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+# POSSIBILITY OF SUCH DAMAGE.
+#
+############################################################################
+
+#
+# sdlog2 Application
+#
+
+MODULE_COMMAND  = sdlog2
+# The main thread only buffers to RAM, needs a high priority
+MODULE_PRIORITY = "SCHED_PRIORITY_MAX-30"
+
+SRCS = sdlog2.c \
+       logbuffer.c

src/modules/sdlog2/sdlog2_format.h

@@ -0,0 +1,98 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2013 PX4 Development Team. All rights reserved.
+ *   Author: Anton Babushkin <rk3dov@gmail.com>
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 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.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file sdlog2_format.h
+ *
+ * General log format structures and macro.
+ *
+ * @author Anton Babushkin <rk3dov@gmail.com>
+ */
+
+/*
+Format characters in the format string for binary log messages
+  b   : int8_t
+  B   : uint8_t
+  h   : int16_t
+  H   : uint16_t
+  i   : int32_t
+  I   : uint32_t
+  f   : float
+  n   : char[4]
+  N   : char[16]
+  Z   : char[64]
+  c   : int16_t * 100
+  C   : uint16_t * 100
+  e   : int32_t * 100
+  E   : uint32_t * 100
+  L   : int32_t latitude/longitude
+  M   : uint8_t flight mode
+
+  q   : int64_t
+  Q   : uint64_t
+ */
+
+#ifndef SDLOG2_FORMAT_H_
+#define SDLOG2_FORMAT_H_
+
+#define LOG_PACKET_HEADER_LEN	   3
+#define LOG_PACKET_HEADER	       uint8_t head1, head2, msg_type;
+#define LOG_PACKET_HEADER_INIT(id) .head1 = HEAD_BYTE1, .head2 = HEAD_BYTE2, .msg_type = id
+
+// once the logging code is all converted we will remove these from
+// this header
+#define HEAD_BYTE1  0xA3    // Decimal 163
+#define HEAD_BYTE2  0x95    // Decimal 149
+
+struct log_format_s {
+	uint8_t type;
+	uint8_t length;		// full packet length including header
+	char name[4];
+	char format[16];
+	char labels[64];
+};
+
+#define LOG_FORMAT(_name, _format, _labels) { \
+		.type = LOG_##_name##_MSG, \
+		.length = sizeof(struct log_##_name##_s) + LOG_PACKET_HEADER_LEN, \
+		.name = #_name, \
+		.format = _format, \
+		.labels = _labels \
+	}
+
+#define LOG_FORMAT_MSG	  0x80
+
+#define LOG_PACKET_SIZE(_name)	LOG_PACKET_HEADER_LEN + sizeof(struct log_##_name##_s)
+
+#endif /* SDLOG2_FORMAT_H_ */

src/modules/sdlog2/sdlog2_messages.h

@@ -0,0 +1,152 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2013 PX4 Development Team. All rights reserved.
+ *   Author: Anton Babushkin <rk3dov@gmail.com>
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 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.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file sdlog2_messages.h
+ *
+ * Log messages and structures definition.
+ *
+ * @author Anton Babushkin <rk3dov@gmail.com>
+ */
+
+#ifndef SDLOG2_MESSAGES_H_
+#define SDLOG2_MESSAGES_H_
+
+#include "sdlog2_format.h"
+
+/* define message formats */
+
+#pragma pack(push, 1)
+/* --- TIME - TIME STAMP --- */
+#define LOG_TIME_MSG 1
+struct log_TIME_s {
+	uint64_t t;
+};
+
+/* --- ATT - ATTITUDE --- */
+#define LOG_ATT_MSG 2
+struct log_ATT_s {
+	float roll;
+	float pitch;
+	float yaw;
+};
+
+/* --- ATSP - ATTITUDE SET POINT --- */
+#define LOG_ATSP_MSG 3
+struct log_ATSP_s {
+	float roll_sp;
+	float pitch_sp;
+	float yaw_sp;
+};
+
+/* --- IMU - IMU SENSORS --- */
+#define LOG_IMU_MSG 4
+struct log_IMU_s {
+	float acc_x;
+	float acc_y;
+	float acc_z;
+	float gyro_x;
+	float gyro_y;
+	float gyro_z;
+	float mag_x;
+	float mag_y;
+	float mag_z;
+};
+
+/* --- SENS - OTHER SENSORS --- */
+#define LOG_SENS_MSG 5
+struct log_SENS_s {
+	float baro_pres;
+	float baro_alt;
+	float baro_temp;
+	float diff_pres;
+};
+
+/* --- LPOS - LOCAL POSITION --- */
+#define LOG_LPOS_MSG 6
+struct log_LPOS_s {
+	float x;
+	float y;
+	float z;
+	float vx;
+	float vy;
+	float vz;
+	float hdg;
+	int32_t home_lat;
+	int32_t home_lon;
+	float home_alt;
+};
+
+/* --- LPSP - LOCAL POSITION SETPOINT --- */
+#define LOG_LPSP_MSG 7
+struct log_LPSP_s {
+	float x;
+	float y;
+	float z;
+	float yaw;
+};
+
+/* --- GPS - GPS POSITION --- */
+#define LOG_GPS_MSG 8
+struct log_GPS_s {
+	uint64_t gps_time;
+	uint8_t fix_type;
+	float eph;
+	float epv;
+	int32_t lat;
+	int32_t lon;
+	float alt;
+	float vel_n;
+	float vel_e;
+	float vel_d;
+	float cog;
+};
+#pragma pack(pop)
+
+/* construct list of all message formats */
+
+static const struct log_format_s log_formats[] = {
+	LOG_FORMAT(TIME, "Q", "StartTime"),
+	LOG_FORMAT(ATT, "fff", "Roll,Pitch,Yaw"),
+	LOG_FORMAT(ATSP, "fff", "RollSP,PitchSP,YawSP"),
+	LOG_FORMAT(IMU, "fffffffff", "AccX,AccY,AccZ,GyroX,GyroY,GyroZ,MagX,MagY,MagZ"),
+	LOG_FORMAT(SENS, "ffff", "BaroPres,BaroAlt,BaroTemp,DiffPres"),
+	LOG_FORMAT(LPOS, "fffffffLLf", "X,Y,Z,VX,VY,VZ,Heading,HomeLat,HomeLon,HomeAlt"),
+	LOG_FORMAT(LPSP, "ffff", "X,Y,Z,Yaw"),
+	LOG_FORMAT(GPS, "QBffLLfffff", "GPSTime,FixType,EPH,EPV,Lat,Lon,Alt,VelN,VelE,VelD,Cog"),
+};
+
+static const int log_formats_num = sizeof(log_formats) / sizeof(struct log_format_s);
+
+#endif /* SDLOG2_MESSAGES_H_ */