Merge pull request #6 from peterbarker/la-enhancements

La enhancements

Author: mrpollo

Makefile

@@ -17,12 +17,13 @@ INCS = -I./util -I./ini -I./ini/cpp
 
 STD=-std=c++11
 CFLAGS += -Wall $(INCS) -DGIT_VERSION=\"$(GIT_VERSION)\"
-CXXFLAGS += -Wall $(INCS) $(STD) -g -fpermissive -DGIT_VERSION=\"$(GIT_VERSION)\"
+CXXFLAGS += -Wall $(INCS) $(STD) -g -DGIT_VERSION=\"$(GIT_VERSION)\"
 
 DLIBS += -ljsoncpp
 
 SRCS_CPP = dataflash_logger.cpp
 SRCS_CPP += INIReader.cpp
+SRCS_CPP += analyzer_util.cpp
 SRCS_CPP += mavlink_message_handler.cpp
 SRCS_CPP += mavlink_reader.cpp
 SRCS_CPP += analyze.cpp
@@ -34,7 +35,10 @@ SRCS_CPP += analyzer_ever_flew.cpp
 SRCS_CPP += analyzer_good_ekf.cpp
 SRCS_CPP += analyzer_battery.cpp
 SRCS_CPP += analyzer_brownout.cpp
-SRCS_CPP += analyzer_crashed.cpp
+SRCS_CPP += analyzer_notcrashed.cpp
+SRCS_CPP += analyzer_attitude_control.cpp
+SRCS_CPP += analyzervehicle_copter.cpp
+SRCS_CPP += analyzervehicle.cpp
 SRCS_CPP += la-log.cpp
 SRCS_C = util.c ini.c
 

analyze.cpp

@@ -7,9 +7,10 @@
 #include "analyzer_ever_armed.h"
 #include "analyzer_ever_flew.h"
 #include "analyzer_good_ekf.h"
+#include "analyzer_attitude_control.h"
 #include "analyzer_battery.h"
 #include "analyzer_brownout.h"
-#include "analyzer_crashed.h"
+#include "analyzer_notcrashed.h"
 
 void Analyze::instantiate_analyzers(INIReader *config)
 {
@@ -18,55 +19,62 @@ void Analyze::instantiate_analyzers(INIReader *config)
 	exit(1);  // FIXME - throw exception
     }
 
-    Analyzer_Compass_Offsets *analyzer_compass_offsets = new Analyzer_Compass_Offsets(_fd_telem_forwarder, _sa_telemetry_forwarder);
+    Analyzer_Compass_Offsets *analyzer_compass_offsets = new Analyzer_Compass_Offsets(_fd_telem_forwarder, _sa_telemetry_forwarder, vehicle);
     if (analyzer_compass_offsets != NULL) {
         configure_analyzer(config, analyzer_compass_offsets, "Analyzer_Compass_Offsets");
     } else {
         syslog(LOG_INFO, "Failed to create analyzer_compass_offsets");
     }
 
-    Analyzer_Ever_Armed *analyzer_ever_armed = new Analyzer_Ever_Armed(_fd_telem_forwarder, _sa_telemetry_forwarder);
+    Analyzer_Ever_Armed *analyzer_ever_armed = new Analyzer_Ever_Armed(_fd_telem_forwarder, _sa_telemetry_forwarder, vehicle);
     if (analyzer_ever_armed != NULL) {
         configure_analyzer(config, analyzer_ever_armed, "Analyzer_Ever_Armed");
     } else {
         syslog(LOG_INFO, "Failed to create analyzer_ever_armed");
     }
 
-    Analyzer_Ever_Flew *analyzer_ever_flew = new Analyzer_Ever_Flew(_fd_telem_forwarder, _sa_telemetry_forwarder);
+    Analyzer_Ever_Flew *analyzer_ever_flew = new Analyzer_Ever_Flew(_fd_telem_forwarder, _sa_telemetry_forwarder, vehicle);
     if (analyzer_ever_flew != NULL) {
         configure_analyzer(config, analyzer_ever_flew, "Analyzer_Ever_Flew");
     } else {
         syslog(LOG_INFO, "Failed to create analyzer_ever_flew");
     }
 
 
-    Analyzer_Good_EKF *analyzer_good_ekf = new Analyzer_Good_EKF(_fd_telem_forwarder, _sa_telemetry_forwarder);
+    Analyzer_Good_EKF *analyzer_good_ekf = new Analyzer_Good_EKF(_fd_telem_forwarder, _sa_telemetry_forwarder, vehicle);
     if (analyzer_good_ekf != NULL) {
         configure_analyzer(config, analyzer_good_ekf, "Analyzer_Good_EKF");
     } else {
         syslog(LOG_INFO, "Failed to create analyzer_good_ekf");
     }
 
-    Analyzer_Battery *analyzer_battery = new Analyzer_Battery(_fd_telem_forwarder, _sa_telemetry_forwarder);
+    Analyzer_Attitude_Control *analyzer_attitude_control = new Analyzer_Attitude_Control(_fd_telem_forwarder, _sa_telemetry_forwarder, vehicle);
+    if (analyzer_attitude_control != NULL) {
+        configure_analyzer(config, analyzer_attitude_control, "Analyzer_Attitude_Control");
+    } else {
+        syslog(LOG_INFO, "Failed to create analyzer_attitude_control");
+    }
+
+    Analyzer_Battery *analyzer_battery = new Analyzer_Battery(_fd_telem_forwarder, _sa_telemetry_forwarder, vehicle);
     if (analyzer_battery != NULL) {
         configure_analyzer(config, analyzer_battery, "Analyzer_Battery");
     } else {
         syslog(LOG_INFO, "Failed to create analyzer_battery");
     }
 
-    Analyzer_Brownout *analyzer_brownout = new Analyzer_Brownout(_fd_telem_forwarder, _sa_telemetry_forwarder);
+    Analyzer_Brownout *analyzer_brownout = new Analyzer_Brownout(_fd_telem_forwarder, _sa_telemetry_forwarder, vehicle);
     if (analyzer_brownout != NULL) {
         configure_analyzer(config, analyzer_brownout, "Analyzer_Brownout");
     } else {
         syslog(LOG_INFO, "Failed to create analyzer_brownout");
     }
 
 
-    Analyzer_Crashed *analyzer_crashed = new Analyzer_Crashed(_fd_telem_forwarder, _sa_telemetry_forwarder);
-    if (analyzer_crashed != NULL) {
-        configure_analyzer(config, analyzer_crashed, "Analyzer_Crashed");
+    Analyzer_NotCrashed *analyzer_notcrashed = new Analyzer_NotCrashed(_fd_telem_forwarder, _sa_telemetry_forwarder, vehicle);
+    if (analyzer_notcrashed != NULL) {
+        configure_analyzer(config, analyzer_notcrashed, "Analyzer_NotCrashed");
     } else {
-        syslog(LOG_INFO, "Failed to create analyzer_crashed");
+        syslog(LOG_INFO, "Failed to create analyzer_not_crashed");
     }
 }
 
@@ -342,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);
 
@@ -364,41 +375,92 @@ void Analyze::end_of_log(uint32_t packet_count) {
 }
 
 void Analyze::handle_decoded_message(uint64_t T, mavlink_ahrs2_t &msg) {
+    if (!vehicle) {
+        return;
+    }
     for(int i=0; i<next_analyzer; i++) {
         analyzer[i]->handle_decoded_message(T, msg);
     }
 }
 void Analyze::handle_decoded_message(uint64_t T, mavlink_attitude_t &msg) {
+    if (!vehicle) {
+        return;
+    }
+    vehicle->handle_decoded_message(T, msg);
     for(int i=0; i<next_analyzer; i++) {
         analyzer[i]->handle_decoded_message(T, msg);
     }
 }
 void Analyze::handle_decoded_message(uint64_t T, mavlink_heartbeat_t &msg) {
+    if (!vehicle) {
+        return;
+    }
+    vehicle->handle_decoded_message(T, msg);
+    for(int i=0; i<next_analyzer; i++) {
+        analyzer[i]->handle_decoded_message(T, msg);
+    }
+}
+void Analyze::handle_decoded_message(uint64_t T, mavlink_nav_controller_output_t &msg) {
+    if (!vehicle) {
+        return;
+    }
+    vehicle->handle_decoded_message(T, msg);
     for(int i=0; i<next_analyzer; i++) {
         analyzer[i]->handle_decoded_message(T, msg);
     }
 }
 void Analyze::handle_decoded_message(uint64_t T, mavlink_param_value_t &msg) {
+    if (!vehicle) {
+        return;
+    }
+    vehicle->handle_decoded_message(T, msg);
     for(int i=0; i<next_analyzer; i++) {
         analyzer[i]->handle_decoded_message(T, msg);
     }
 }
 void Analyze::handle_decoded_message(uint64_t T, mavlink_servo_output_raw_t &msg) {
+    if (!vehicle) {
+        return;
+    }
+    vehicle->handle_decoded_message(T, msg);
     for(int i=0; i<next_analyzer; i++) {
         analyzer[i]->handle_decoded_message(T, msg);
     }
 }
 void Analyze::handle_decoded_message(uint64_t T, mavlink_ekf_status_report_t &msg) {
+    if (!vehicle) {
+        return;
+    }
     for(int i=0; i<next_analyzer; i++) {
         analyzer[i]->handle_decoded_message(T, msg);
     }
 }
 void Analyze::handle_decoded_message(uint64_t T, mavlink_sys_status_t &msg) {
+    if (!vehicle) {
+        return;
+    }
     for(int i=0; i<next_analyzer; i++) {
         analyzer[i]->handle_decoded_message(T, msg);
     }
 }
 void Analyze::handle_decoded_message(uint64_t T, mavlink_vfr_hud_t &msg) {
+    if (!vehicle) {
+        return;
+    }
+    for(int i=0; i<next_analyzer; i++) {
+        analyzer[i]->handle_decoded_message(T, msg);
+    }
+}
+void Analyze::handle_decoded_message(uint64_t T, mavlink_statustext_t &msg) {
+    if (!vehicle) {
+        if (strstr(msg.text, "APM:Copter")) {
+            vehicle = new AnalyzerVehicle::Copter();
+        }
+    }
+    if (!vehicle) {
+        return;
+    }
+    vehicle->handle_decoded_message(T, msg);
     for(int i=0; i<next_analyzer; i++) {
         analyzer[i]->handle_decoded_message(T, msg);
     }

analyze.h

@@ -3,15 +3,20 @@
 
 #include "mavlink_message_handler.h"
 #include "analyzer.h"
+#include "analyzervehicle_copter.h"
+
+#include "analyzer_util.h"
 
 class Analyze : public MAVLink_Message_Handler {
 
 public:
     Analyze(int fd, struct sockaddr_in &sa) :
 	MAVLink_Message_Handler(fd, sa),
+        vehicle(NULL),
         _output_style(OUTPUT_JSON),
         next_analyzer(0)
         {
+            start_time = now();
         }
     void instantiate_analyzers(INIReader *config);
 
@@ -26,6 +31,10 @@ 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;
 #define MAX_ANALYZERS 10
     uint8_t next_analyzer;
@@ -43,8 +52,10 @@ class Analyze : public MAVLink_Message_Handler {
     virtual void handle_decoded_message(uint64_t T, mavlink_attitude_t &msg);
     virtual void handle_decoded_message(uint64_t T, mavlink_ekf_status_report_t &msg);
     virtual void handle_decoded_message(uint64_t T, mavlink_heartbeat_t &msg);
+    virtual void handle_decoded_message(uint64_t T, mavlink_nav_controller_output_t &msg);
     virtual void handle_decoded_message(uint64_t T, mavlink_param_value_t &msg);    
     virtual void handle_decoded_message(uint64_t T, mavlink_servo_output_raw_t &msg);
+    virtual void handle_decoded_message(uint64_t T, mavlink_statustext_t &msg);
     virtual void handle_decoded_message(uint64_t T, mavlink_sys_status_t &msg);
     virtual void handle_decoded_message(uint64_t T, mavlink_vfr_hud_t &msg);
 

analyzer.h

@@ -6,12 +6,42 @@
 #include <jsoncpp/json/json.h> // libjsoncpp0 and libjsoncpp-dev on debian
 #include <jsoncpp/json/writer.h> // libjsoncpp0 and libjsoncpp-dev on debian
 
+#include "analyzervehicle.h"
+
+enum analyzer_status {
+    analyzer_status_warn = 17,
+    analyzer_status_fail,
+    analyzer_status_ok,
+};
+
+class analyzer_result {
+public:
+    analyzer_status status;
+    const char *status_as_string() const {
+        switch(status) {
+        case analyzer_status_fail:
+            return "FAIL";
+        case analyzer_status_warn:
+            return "WARN";
+        case analyzer_status_ok:
+            return "OK";
+        }
+        return "STRANGE";
+    }
+    // std::string reason_as_string() const {
+    //     return string_format("Desired attitude not achieved");
+    // }
+};
+
+
 class Analyzer : public MAVLink_Message_Handler {
 
 public:
-    Analyzer(int fd, struct sockaddr_in &sa) :
+    Analyzer(int fd, struct sockaddr_in &sa, AnalyzerVehicle::Base *&vehicle) :
 	MAVLink_Message_Handler(fd, sa),
-        evilness(0)
+        evilness(0),
+        _vehicle(vehicle),
+        _status(analyzer_status_ok)
         { }
 
     virtual const char *name() = 0;
@@ -22,14 +52,25 @@ class Analyzer : public MAVLink_Message_Handler {
     void add_evilness(uint8_t sin_points) {
         evilness += sin_points;
     }
-    uint16_t get_evilness() { return evilness; }
+    uint16_t get_evilness() const { return evilness; }
+
+    analyzer_status status() const { return _status; }
 
 protected:
+
     std::string to_string(double x);
     uint16_t evilness;
+    AnalyzerVehicle::Base *&_vehicle;
+    void set_status(analyzer_status status) { _status = status; }
 
 private:
+    analyzer_status _status;
 };
 
 #endif
 
+
+
+// - two fundamental types of test
+//  - is the software working correctly (EKF issues)
+//  - is the vehicle doing sensible things (attitude etc)