Begin version control

I don't want to lose code again!

Makefile

@@ -0,0 +1,27 @@
+CXXFLAGS = -O2 -g
+CFLAGS = -O2 -g
+LIBS = -lboost_filesystem
+
+OBJS = $(patsubst %.c, %.o, $(wildcard *.c)) \
+	   $(patsubst %.cc, %.o, $(wildcard *.cc))
+C_DEPS = $(patsubst %.o, %.dep, $(wildcard *.c))
+CXX_DEPS = $(patsubst %.o, %.depxx, $(wildcard *.cc))
+
+tx01_data : $(OBJS)
+	$(CXX) $(LDFLAGS) -o $@ $^ $(LIBS)
+
+sinclude $(DEPS)
+
+%.dep : %.c
+	$(CC) -MM $(CFLAGS) $< > $@.$$$$; \
+	sed 's,\($*\)\.o[ :]*,\1.o $@:.g' < $@.$$$$ > $@; \
+	$(RM) $@.$$$$
+
+%.depxx : %.cc
+	$(CXX) -MM $(CXXFLAGS) $< > $@.$$$$; \
+	sed 's,\($*\)\.o[ :]*,\1.o $@:.g' < $@.$$$$ > $@; \
+	$(RM) $@.$$$$
+
+.PHONY : clean
+clean:
+	echo $(RM) -f tx01_data $(OBJS) $(C_DEPS) $(CXX_DEPS)

config.cc

@@ -0,0 +1,6 @@
+#include "config.hpp"
+
+double conf_aggr_ts = 5;
+double conf_aggr_tav = 15;
+bool conf_debug_dump_flight_data = true;
+bool conf_debug_dump_adc_raw = false;

config.hpp

@@ -0,0 +1,7 @@
+#ifndef _CONFIG_HPP_
+#define _CONFIG_HPP_
+
+extern double conf_aggr_ts, conf_aggr_tav;
+extern bool conf_debug_dump_flight_data, conf_debug_dump_adc_raw;
+
+#endif

config.ini

@@ -0,0 +1,3 @@
+[aggregation]
+ts = 5.00
+tav = 20.00

crc.c

@@ -0,0 +1,47 @@
+#include "crc.h"
+
+static const uint16_t crc_table[256] = {
+0x0000, 0x1021, 0x2042, 0x3063, 0x4084, 0x50a5, 0x60c6, 0x70e7, 0x8108,
+0x9129, 0xa14a, 0xb16b, 0xc18c, 0xd1ad, 0xe1ce, 0xf1ef, 0x1231, 0x0210,
+0x3273, 0x2252, 0x52b5, 0x4294, 0x72f7, 0x62d6, 0x9339, 0x8318, 0xb37b,
+0xa35a, 0xd3bd, 0xc39c, 0xf3ff, 0xe3de, 0x2462, 0x3443, 0x0420, 0x1401,
+0x64e6, 0x74c7, 0x44a4, 0x5485, 0xa56a, 0xb54b, 0x8528, 0x9509, 0xe5ee,
+0xf5cf, 0xc5ac, 0xd58d, 0x3653, 0x2672, 0x1611, 0x0630, 0x76d7, 0x66f6,
+0x5695, 0x46b4, 0xb75b, 0xa77a, 0x9719, 0x8738, 0xf7df, 0xe7fe, 0xd79d,
+0xc7bc, 0x48c4, 0x58e5, 0x6886, 0x78a7, 0x0840, 0x1861, 0x2802, 0x3823,
+0xc9cc, 0xd9ed, 0xe98e, 0xf9af, 0x8948, 0x9969, 0xa90a, 0xb92b, 0x5af5,
+0x4ad4, 0x7ab7, 0x6a96, 0x1a71, 0x0a50, 0x3a33, 0x2a12, 0xdbfd, 0xcbdc,
+0xfbbf, 0xeb9e, 0x9b79, 0x8b58, 0xbb3b, 0xab1a, 0x6ca6, 0x7c87, 0x4ce4,
+0x5cc5, 0x2c22, 0x3c03, 0x0c60, 0x1c41, 0xedae, 0xfd8f, 0xcdec, 0xddcd,
+0xad2a, 0xbd0b, 0x8d68, 0x9d49, 0x7e97, 0x6eb6, 0x5ed5, 0x4ef4, 0x3e13,
+0x2e32, 0x1e51, 0x0e70, 0xff9f, 0xefbe, 0xdfdd, 0xcffc, 0xbf1b, 0xaf3a,
+0x9f59, 0x8f78, 0x9188, 0x81a9, 0xb1ca, 0xa1eb, 0xd10c, 0xc12d, 0xf14e,
+0xe16f, 0x1080, 0x00a1, 0x30c2, 0x20e3, 0x5004, 0x4025, 0x7046, 0x6067,
+0x83b9, 0x9398, 0xa3fb, 0xb3da, 0xc33d, 0xd31c, 0xe37f, 0xf35e, 0x02b1,
+0x1290, 0x22f3, 0x32d2, 0x4235, 0x5214, 0x6277, 0x7256, 0xb5ea, 0xa5cb,
+0x95a8, 0x8589, 0xf56e, 0xe54f, 0xd52c, 0xc50d, 0x34e2, 0x24c3, 0x14a0,
+0x0481, 0x7466, 0x6447, 0x5424, 0x4405, 0xa7db, 0xb7fa, 0x8799, 0x97b8,
+0xe75f, 0xf77e, 0xc71d, 0xd73c, 0x26d3, 0x36f2, 0x0691, 0x16b0, 0x6657,
+0x7676, 0x4615, 0x5634, 0xd94c, 0xc96d, 0xf90e, 0xe92f, 0x99c8, 0x89e9,
+0xb98a, 0xa9ab, 0x5844, 0x4865, 0x7806, 0x6827, 0x18c0, 0x08e1, 0x3882,
+0x28a3, 0xcb7d, 0xdb5c, 0xeb3f, 0xfb1e, 0x8bf9, 0x9bd8, 0xabbb, 0xbb9a,
+0x4a75, 0x5a54, 0x6a37, 0x7a16, 0x0af1, 0x1ad0, 0x2ab3, 0x3a92, 0xfd2e,
+0xed0f, 0xdd6c, 0xcd4d, 0xbdaa, 0xad8b, 0x9de8, 0x8dc9, 0x7c26, 0x6c07,
+0x5c64, 0x4c45, 0x3ca2, 0x2c83, 0x1ce0, 0x0cc1, 0xef1f, 0xff3e, 0xcf5d,
+0xdf7c, 0xaf9b, 0xbfba, 0x8fd9, 0x9ff8, 0x6e17, 0x7e36, 0x4e55, 0x5e74,
+0x2e93, 0x3eb2, 0x0ed1, 0x1ef0};
+
+uint16_t crc16_ccitt(const char *data, int len)
+{
+    uint16_t crc16 = 0x0000;
+    uint16_t crc_h8, crc_l8;
+    
+    while( len-- ) {
+        crc_h8 = (crc16 >> 8);
+        crc_l8 = (crc16 << 8);
+        crc16 = crc_l8 ^ crc_table[crc_h8 ^ (unsigned char) *data];
+        data++;
+    }
+
+    return crc16;
+}

crc.h

@@ -0,0 +1,16 @@
+#ifndef _CRC_H_
+#define _CRC_H_ 1
+
+#include <stdint.h>
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+extern uint16_t crc16_ccitt(const char *buf, int len);
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif

fdr.hpp

@@ -0,0 +1,9 @@
+#include <map>
+
+struct flight_data
+{
+	double ils_height;
+	double gps_height;
+};
+
+std::map<uint32_t, flight_data> FDR;

main.cc

@@ -0,0 +1,319 @@
+#include <algorithm>
+#include <iostream>
+#include <string>
+#include <vector>
+#include <map>
+
+#include <boost/interprocess/file_mapping.hpp>
+#include <boost/interprocess/mapped_region.hpp>
+#include <boost/filesystem.hpp>
+
+#include "tx01_msg.hpp"
+#include "tx01_data_frame.hpp"
+#include "tx01_telemetry_frame.hpp"
+#include "config.hpp"
+#include "crc.h"
+
+template <class T>
+inline static T load(const char *addr)
+{
+	T t;
+	memcpy(&t, addr, sizeof(T));
+	return t;
+}
+
+struct adc_sample
+{
+	uint32_t fl_time;
+	int v;
+	bool fired;
+};
+
+static std::vector<adc_sample> rm100_data[2];
+
+// dump raw data file for gnuplot, enabled by config file.
+static void dump_rm100_raw_data(int k)
+{
+	std::ofstream out(std::string("rm100_raw_") + "01"[k] + ".dat");
+	for (size_t i = 0; i < rm100_data[k].size(); i++)
+		out << i << ' ' << rm100_data[k][i].v << '\n';
+}
+
+struct particle
+{
+	size_t k;
+	uint32_t fl_time;
+	bool fired;
+};
+
+static void dump_particles(const std::vector<particle> particles, int id)
+{
+	std::ofstream out(std::string("rm100_particles_") + "01"[id] + ".csv");
+	out << "k,fl_time,fired\n";
+	for (particle p: particles)
+		out << p.k << ',' << p.fl_time << p.fired << '\n';
+}
+
+static std::vector<particle>
+adc_data_to_particles(const std::vector<adc_sample> &sample)
+{
+	std::vector<particle> ret;
+
+	const size_t crit = 5;
+	if (sample.size() < crit)
+		return ret;
+
+	for (size_t i = 0; i < sample.size() - crit; i++) {
+		bool ok = true;
+		for (size_t j = 0; j < crit; j++)
+			if (sample[i+j].v > 0)
+				ok = false;
+		if (ok) {
+			ret.push_back({i, sample[i].fl_time, sample[i].fired});
+			i += crit - 1;
+		}
+	}
+	return ret;
+}
+
+static void append_to_rm100_data(int rm100_id, const char *p, size_t sz,
+								 int fl_time, bool fired)
+{
+	//std::cout << "appending " << sz << " bytes to rm100 " <<
+	//	rm100_id << '\n';
+	std::size_t old_size = rm100_data[rm100_id].size();
+	rm100_data[rm100_id].resize(old_size + sz);
+	for (; old_size != rm100_data[rm100_id].size(); old_size++) {
+		rm100_data[rm100_id][old_size].fl_time = fl_time;
+		rm100_data[rm100_id][old_size].v = *p++;
+		rm100_data[rm100_id][old_size].fired = fired;
+	}
+}
+
+struct flight_data
+{
+	uint32_t fl_time;
+	double ils_height;
+	double gps_height;
+};
+
+static bool fired = false;
+static flight_data current = {0, 0, 0};
+static std::map<uint32_t, flight_data> FDR;
+
+static void dump_particles_aggr(const std::vector<particle> particles,
+								int id, uint32_t ts, size_t win_size)
+{
+	std::ofstream out(std::string("rm100_aggreated_") + "01"[id] + ".csv");
+	out << "fl_time,cnt,gps_height,ils_height\n";
+	uint32_t t0 = 0;
+	size_t cnt = 0;
+	std::vector<size_t> sum;
+	for (particle p: particles)
+		if (p.fired) {
+			while (t0 + ts < p.fl_time) {
+				sum.push_back(cnt);
+				cnt = 0;
+				t0 += ts;
+			}
+			cnt++;
+		}
+
+	cnt = 0;
+	flight_data fd{0, 0, 0};
+	for (size_t i = 0; i < sum.size(); i++) {
+		cnt += sum[i];
+		if (win_size < i)
+			cnt -= sum[i-win_size-1];
+		if (win_size <= i) {
+			if (FDR.find(ts*(i-win_size)) != FDR.end())
+				fd = FDR[ts*(i-win_size)];
+			out << (i-win_size) * ts * 0.0001 <<
+				',' << cnt * 1. / win_size <<
+				',' << fd.gps_height << ',' << fd.ils_height << '\n';
+		}
+	}
+}
+
+static void parse_and_record_flight_data(const char *buf, size_t sz)
+{
+	const tx01_telemetry_frame *frame = (const tx01_telemetry_frame *) buf;
+	uint16_t recv_crc = load<uint16_t>(&frame->payload[frame->len_payload]);
+	uint16_t crc = crc16_ccitt(&buf[2],
+							   sizeof(frame) + frame->len_payload - 4);
+	if (crc != recv_crc)
+		return ;
+
+	uint32_t fl_time = load<uint32_t>(&buf[7]);
+	current.fl_time = fl_time;
+
+	unsigned char frame_id = (unsigned char) buf[6];
+	double height;
+	switch (frame_id) {
+		case 0x85:
+			height = load<int32_t>(&buf[164]) * 0.00004656613;
+			if (height >= -300 && height <= 100000)
+				current.ils_height = height;
+			break;
+		case 0x87:
+			height = load<int32_t>(&buf[168]) * 0.1;
+			if (height >= -300 && height <= 100000)
+				current.gps_height = height;
+			break;
+	}
+
+	if (fl_time < 20000)
+		fired = true;
+	if (fired)
+		FDR[fl_time] = current;
+	if (conf_debug_dump_flight_data)
+		std::cout << current.fl_time * 0.0001 << ' ' <<
+			current.ils_height << ' ' << current.gps_height << ' ' <<
+			fired << '\n';
+}
+
+static void process_flight_ctrl_frame(const char *first, size_t sz)
+{
+	static union {
+		char buf[512];
+		tx01_telemetry_frame frame;
+	} x;
+	static size_t buf_tail = 0;
+
+	const char *last = first + sz;
+	while (first != last) {
+		unsigned char byte = (unsigned char) *first;
+		if (buf_tail == 0 && byte != 0x55) {
+			first++;
+			continue;
+		}
+		if (buf_tail == 1 && byte != 0xaa) {
+			if (byte != 0x55)
+				buf_tail = 0;
+			first++;
+			continue;
+		}
+
+		x.buf[buf_tail++] = *first++;
+		if (buf_tail >= sizeof(x.frame) &&
+			(sizeof(x.frame) + x.frame.len_payload + sizeof(uint16_t) ==
+			 buf_tail)) {
+			parse_and_record_flight_data(x.buf, buf_tail);
+			buf_tail = 0;
+		}
+	}
+}
+
+static int process_data_in_msg(const char *buf, size_t sz)
+{
+	// std::cout << sz << '\n';
+	// std::cout << (void *)buf << '\n';
+	// std::cout << std::hex << 0 + (unsigned char)buf[0] << ' '
+	//	<< 0 + (unsigned char)buf[1] << std::dec << '\n';
+
+	const tx01_data_frame *frame = (const tx01_data_frame *) buf;
+	if (frame->magic == 0xeb90) {
+		switch (frame->channel_id) {
+			case 9:
+			case 10:
+			case 11:
+			case 12:
+				append_to_rm100_data(0, frame->data,
+									 size_data_frame_content,
+									 current.fl_time, fired);
+				break;
+			case 13:
+			case 14:
+			case 15:
+			case 16:
+				append_to_rm100_data(1, frame->data,
+									 size_data_frame_content,
+									 current.fl_time, fired);
+				break;
+			case 17:
+			case 18:
+			case 19:
+			case 20:
+			case 21:
+			case 22:
+			case 23:
+				process_flight_ctrl_frame(frame->data,
+										  size_data_frame_content);
+				break;
+		}
+	}
+	return 0;
+}
+
+static int process_telemetry_data(void *ptr, size_t sz)
+{
+	//std::cout << sz << '\n';
+	//std::cout << std::string((const char *)ptr, (const char *)ptr + sz);
+
+	char *first = static_cast<char *>(ptr), *last = first + sz;
+	while (first != last) {
+		ptrdiff_t remain = last - first;
+		if (remain < sizeof(tx01_msg_header))
+			break;
+
+		const tx01_msg_header *p_hdr = tx01_try_match_msg_header(first);
+		if (!p_hdr) {
+			++first;
+			continue;
+		}
+
+		// ignore incomplete msg at the tail
+		if (last - first < p_hdr->size)
+			break;
+
+		const tx01_data_msg *p_msg = tx01_try_match_data_msg(p_hdr);
+		if (p_msg != NULL)
+			process_data_in_msg(p_msg->data,
+								p_hdr->size - sizeof(tx01_data_msg) - 4);
+
+		first += p_hdr->size;
+
+		// std::cout << static_cast<void *>(first) << '\n';
+	}
+	for (int k = 0; k < 2; k++) {
+		if (conf_debug_dump_adc_raw)
+			dump_rm100_raw_data(0);
+		auto particles = adc_data_to_particles(rm100_data[k]);
+		dump_particles(particles, k);
+		dump_particles_aggr(particles, k, (int)(conf_aggr_ts * 10000 + 0.5),
+							(int)(conf_aggr_tav/conf_aggr_ts + 0.5));
+	}
+	return 0;
+}
+
+int main(int argc, const char **argv)
+{
+	using namespace std;
+	using namespace boost::interprocess;
+	using namespace boost::filesystem;
+
+	const char *filename = "data.bin";
+	if (argc > 1)
+		filename = argv[1];
+
+	path p = filename;
+
+	size_t sz;
+	try {
+		sz = file_size(p);
+	} catch (const filesystem_error &ex) {
+		cerr << ex.what() << '\n';
+		exit(1);
+	}
+
+	try {
+		file_mapping fmap(filename, read_only);
+		mapped_region region(fmap, read_only, 0, sz);
+		return process_telemetry_data(region.get_address(), sz);
+	} catch (const interprocess_exception &ex) {
+		cerr << ex.what() << '\n';
+		exit(1);
+	}
+
+	return 0;
+}

tx01_data_frame.hpp

@@ -0,0 +1,27 @@
+#ifndef _DATA_FRAME_HPP_
+#define _DATA_FRAME_HPP_
+
+#include <boost/endian/arithmetic.hpp>
+
+#if __cplusplus < 201103L
+#include <boost/static_assert.hpp>
+#define static_assert BOOST_STATIC_ASSERT
+#endif
+
+typedef boost::endian::big_uint16_t tx01_data_frame_magic_t;
+const size_t size_data_frame_content = 125;
+
+struct tx01_data_frame
+{
+	tx01_data_frame_magic_t magic;
+	uint8_t channel_id;
+	char data[size_data_frame_content];
+};
+
+static_assert(sizeof(struct tx01_data_frame) == 128);
+
+#ifdef static_assert
+#undef static_assert
+#endif
+
+#endif

tx01_endian.hpp

@@ -0,0 +1,13 @@
+#ifndef _TX01_ENDIAN_HPP_
+#define _TX01_ENDIAN_HPP_ 1
+
+#include <boost/endian/arithmetic.hpp>
+
+using boost::endian::big_uint16_t;
+using boost::endian::big_uint32_t;
+using boost::endian::big_uint64_t;
+using boost::endian::big_int16_t;
+using boost::endian::big_int32_t;
+using boost::endian::big_int64_t;
+
+#endif

tx01_magic_match.hpp

@@ -0,0 +1,13 @@
+#ifndef _TX01_MAGIC_MATCH_HPP_
+#define _TX01_MAGIC_MATCH_HPP_ 1
+
+template <class T, intmax_t magic>
+const T *tx01_try_match_magic(const char *p)
+{
+	const T *ptr = (const T*) p;
+	if (ptr->magic == magic)
+		return ptr;
+	return NULL;
+}
+
+#endif

tx01_msg.cc

@@ -0,0 +1,22 @@
+#include <stdlib.h>
+#include "tx01_msg.hpp"
+#include "tx01_magic_match.hpp"
+
+const struct tx01_msg_header *tx01_try_match_msg_header(const char *ptr)
+{
+	return tx01_try_match_magic<tx01_msg_header,
+		                        TX01_MSG_HEADER_MAGIC>(ptr);
+}
+
+const struct tx01_data_msg *
+tx01_try_match_data_msg(const struct tx01_msg_header *ptr)
+{
+	if (ptr->size > sizeof(struct tx01_data_msg)) {
+		big_int32_t *p_postamble = (big_int32_t *) ((char *)ptr +
+													ptr->size -
+													sizeof(big_int32_t));
+		if (*p_postamble == TX01_MSG_POSTAMBLE)
+			return (struct tx01_data_msg *) ptr;
+	}
+	return NULL;
+}

tx01_msg.hpp

@@ -0,0 +1,58 @@
+#ifndef _TELEMETRY_HPP_
+#define _TELEMETRY_HPP_ 1
+
+#if __cplusplus < 201103L
+#include <boost/static_assert.hpp>
+#define static_assert BOOST_STATIC_ASSERT
+#endif
+
+#include "tx01_endian.hpp"
+
+#define TX01_MSG_HEADER_MAGIC 1234567890
+#define TX01_MSG_POSTAMBLE -1234567890
+
+struct tx01_msg_header
+{
+	// should be MSG_HEADER_MAGIC
+	big_int32_t magic;
+	// size of the message (including header and postamble)
+	big_int32_t size;
+	// user configurable for flow identification
+	big_int32_t flow;
+};
+
+static_assert(sizeof(struct tx01_msg_header) == 12);
+
+struct tx01_data_msg
+{
+	struct tx01_msg_header header;
+	big_int32_t time_tag_1;
+	big_int32_t time_tag_2;
+	big_uint32_t seq_counter;
+	big_int32_t frame_check_result;
+	big_int32_t frame_sync_stat;
+	big_int32_t bit_slip_stat;
+	big_int32_t tm_delay;
+	big_int32_t frame_len;
+	big_int32_t sync_word_len;
+	big_int32_t frame_check_rs_stat;
+	big_int32_t data_fmt;
+	big_int32_t _unused0;
+	big_int32_t _unused1;
+	char data[];
+};
+
+static_assert(sizeof(struct tx01_data_msg) == 64);
+
+/* Try to match a tx01_msg_header at ptr.  If success, return a pointer
+ * to this tx01_msg_header.  Otherwise return NULL. */
+extern const struct tx01_msg_header *
+tx01_try_match_msg_header(const char *ptr);
+
+extern const struct tx01_data_msg *
+tx01_try_match_data_msg(const struct tx01_msg_header *ptr);
+
+#undef _STATIC_ASSERT
+#undef _CAT2
+#undef _CAT
+#endif

tx01_telemetry_frame.hpp

@@ -0,0 +1,20 @@
+#ifndef _TX01_TELEMETRY_FRAME_HPP_
+#define _TX01_TELEMETRY_FRAME_HPP_ 1
+
+#include "tx01_endian.hpp"
+
+const uint16_t tx01_telemetry_frame_magic = 0x55aa;
+
+struct tx01_telemetry_frame
+{
+	big_uint16_t magic;
+	uint8_t len_payload;
+	uint8_t seq;
+	uint8_t dev_id;
+	uint8_t msg_id;
+	char payload[];
+};
+
+static_assert(sizeof(tx01_telemetry_frame) == 6);
+
+#endif