From 3934c9c7580cec3776cd0eb21874035696c8baac Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Fri, 28 Aug 2015 10:06:43 +1000 Subject: [PATCH] la: add a timestamp, version and duration --- analyze.cpp | 3 +++ analyze.h | 5 +++++ analyzer_attitude_control.h | 1 - analyzer_notcrashed.h | 4 ++-- analyzer_util.cpp | 7 +++++++ analyzer_util.h | 2 ++ mavlink_reader.cpp | 2 -- 7 files changed, 19 insertions(+), 5 deletions(-) diff --git a/analyze.cpp b/analyze.cpp index 5dae524..3ea5434 100644 --- a/analyze.cpp +++ b/analyze.cpp @@ -350,6 +350,9 @@ void Analyze::end_of_log(uint32_t packet_count) { } Json::Value root; + root["format-version"] = "0.1"; + root["timestamp"] = (Json::UInt64)start_time; + root["duration"] = (Json::UInt64)(now() - start_time); results_json(root); diff --git a/analyze.h b/analyze.h index 021bc90..c5fae27 100644 --- a/analyze.h +++ b/analyze.h @@ -5,6 +5,8 @@ #include "analyzer.h" #include "analyzervehicle_copter.h" +#include "analyzer_util.h" + class Analyze : public MAVLink_Message_Handler { public: @@ -14,6 +16,7 @@ class Analyze : public MAVLink_Message_Handler { _output_style(OUTPUT_JSON), next_analyzer(0) { + start_time = now(); } void instantiate_analyzers(INIReader *config); @@ -28,6 +31,8 @@ class Analyze : public MAVLink_Message_Handler { void set_output_style(output_style_option option) { _output_style = option;} private: + uint64_t start_time; + AnalyzerVehicle::Base *vehicle; output_style_option _output_style; diff --git a/analyzer_attitude_control.h b/analyzer_attitude_control.h index 9b12a33..efead68 100644 --- a/analyzer_attitude_control.h +++ b/analyzer_attitude_control.h @@ -28,7 +28,6 @@ class Analyzer_Attitude_Control : public Analyzer { void handle_decoded_message(uint64_t T, mavlink_nav_controller_output_t ¶m); void handle_decoded_message(uint64_t T, mavlink_attitude_t &msg); - bool is_zero(double x) { return x < 0.00001; } // FIXME void evaluate(uint64_t T); private: diff --git a/analyzer_notcrashed.h b/analyzer_notcrashed.h index 81fe5d4..cc5b6d9 100644 --- a/analyzer_notcrashed.h +++ b/analyzer_notcrashed.h @@ -15,8 +15,8 @@ class Analyzer_NotCrashed : public Analyzer { public: Analyzer_NotCrashed(int fd, struct sockaddr_in &sa, AnalyzerVehicle::Base *&vehicle) : Analyzer(fd, sa, vehicle), - seen_packets_attitude(false), - notcrashed_results_offset(0) + notcrashed_results_offset(0), + seen_packets_attitude(false) { } const uint16_t servo_output_threshold = 1250; diff --git a/analyzer_util.cpp b/analyzer_util.cpp index aef3877..e86936b 100644 --- a/analyzer_util.cpp +++ b/analyzer_util.cpp @@ -1,4 +1,5 @@ #include "analyzer_util.h" +#include void format_timestamp(char *buf, const uint8_t buflen, const uint64_t T) { @@ -7,3 +8,9 @@ void format_timestamp(char *buf, const uint8_t buflen, const uint64_t T) tmp = localtime(&t); ::strftime(buf, buflen, "%Y%m%d%H%M%S", tmp); } + +uint64_t now() { + struct timeval tv; + gettimeofday(&tv,NULL); + return tv.tv_sec*(uint64_t)1000000 + tv.tv_usec; +} diff --git a/analyzer_util.h b/analyzer_util.h index 7dfb67c..4a01aca 100644 --- a/analyzer_util.h +++ b/analyzer_util.h @@ -31,4 +31,6 @@ std::string string_format( const char* format, Args ... args ) void format_timestamp(char *buf, uint8_t buflen, uint64_t T); +uint64_t now(); + #endif diff --git a/mavlink_reader.cpp b/mavlink_reader.cpp index cbddbd5..942b4a9 100644 --- a/mavlink_reader.cpp +++ b/mavlink_reader.cpp @@ -24,8 +24,6 @@ #include -#define streq(a, b) (!strcmp(a, b)) - static bool sighup_received = false; void sighup_handler(int signal) {