tx01-data/main.cc

#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 || sample[i+j].v < -10)
				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, uint32_t tw)
{
	std::ofstream out(std::string("rm100_aggreated_") + "01"[id] + ".csv");
	out << "fl_time,cnt,gps_height,ils_height\n";
	size_t cnt = 0;

	uint32_t last_time = particles.rbegin()->fl_time;
	auto cmp = [](particle a, particle b) {
		return a.fl_time < b.fl_time;
	};

	auto first = particles.begin();
	while (first != particles.end() && !first->fired)
		++first;

	std::vector<size_t> sum;
	for (uint32_t t0 = 0; t0 < last_time; t0 += ts) {
		auto a = std::lower_bound(first, particles.end(),
				                  particle{0, t0, 0}, cmp);
		auto b = std::lower_bound(first, particles.end(),
				                  particle{0, t0+tw, 0}, cmp);
		sum.push_back(b - a);
	}

	flight_data fd{0, 0, 0};
	for (size_t i = 0; i < sum.size(); i++) {
		auto cnt = sum[i];
		if (FDR.find(ts*i) != FDR.end())
			fd = FDR[ts*i];
		out << i * ts * 0.0001 <<
			',' << cnt * 1. <<
			',' << 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.01;
			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),
							(int)(conf_aggr_tav * 10000));
	}
	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;
}