From 31909e12aca558d636ff0ce962c1a52e2b0992f2 Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Wed, 26 Aug 2015 22:11:29 +1000 Subject: [PATCH 1/6] la: add a vehicle to hold data --- Makefile | 4 +- analyze.cpp | 56 ++++++++++++++--- analyze.h | 4 ++ analyzer.h | 13 +++- analyzer_battery.h | 4 +- analyzer_brownout.cpp | 27 ++------- analyzer_brownout.h | 14 +---- analyzer_compass_offsets.h | 4 +- analyzer_crashed.cpp | 80 ++++++++----------------- analyzer_crashed.h | 18 +++--- analyzer_ever_armed.cpp | 10 +--- analyzer_ever_armed.h | 5 +- analyzer_ever_flew.cpp | 46 ++++++-------- analyzer_ever_flew.h | 8 +-- analyzer_good_ekf.h | 4 +- analyzer_util.h | 17 ++++++ analyzervehicle.cpp | 67 +++++++++++++++++++++ analyzervehicle.h | 119 +++++++++++++++++++++++++++++++++++++ analyzervehicle_copter.cpp | 68 +++++++++++++++++++++ analyzervehicle_copter.h | 31 ++++++++++ mavlink_message_handler.h | 3 +- mavlink_reader.cpp | 12 +++- 22 files changed, 451 insertions(+), 163 deletions(-) create mode 100644 analyzervehicle.cpp create mode 100644 analyzervehicle.h create mode 100644 analyzervehicle_copter.cpp create mode 100644 analyzervehicle_copter.h diff --git a/Makefile b/Makefile index aa37f4c..49e5c9d 100644 --- a/Makefile +++ b/Makefile @@ -17,7 +17,7 @@ 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 @@ -35,6 +35,8 @@ SRCS_CPP += analyzer_good_ekf.cpp SRCS_CPP += analyzer_battery.cpp SRCS_CPP += analyzer_brownout.cpp SRCS_CPP += analyzer_crashed.cpp +SRCS_CPP += analyzervehicle_copter.cpp +SRCS_CPP += analyzervehicle.cpp SRCS_CPP += la-log.cpp SRCS_C = util.c ini.c diff --git a/analyze.cpp b/analyze.cpp index cb4cc92..2f6d56f 100644 --- a/analyze.cpp +++ b/analyze.cpp @@ -18,21 +18,21 @@ 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 { @@ -40,21 +40,21 @@ void Analyze::instantiate_analyzers(INIReader *config) } - 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_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 { @@ -62,7 +62,7 @@ void Analyze::instantiate_analyzers(INIReader *config) } - Analyzer_Crashed *analyzer_crashed = new Analyzer_Crashed(_fd_telem_forwarder, _sa_telemetry_forwarder); + Analyzer_Crashed *analyzer_crashed = new Analyzer_Crashed(_fd_telem_forwarder, _sa_telemetry_forwarder, vehicle); if (analyzer_crashed != NULL) { configure_analyzer(config, analyzer_crashed, "Analyzer_Crashed"); } else { @@ -364,41 +364,83 @@ 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; ihandle_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; ihandle_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; ihandle_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; ihandle_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; ihandle_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; ihandle_decoded_message(T, msg); } } void Analyze::handle_decoded_message(uint64_t T, mavlink_sys_status_t &msg) { + if (!vehicle) { + return; + } for(int i=0; ihandle_decoded_message(T, msg); } } void Analyze::handle_decoded_message(uint64_t T, mavlink_vfr_hud_t &msg) { + if (!vehicle) { + return; + } + for(int i=0; ihandle_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; ihandle_decoded_message(T, msg); } diff --git a/analyze.h b/analyze.h index 91af5d6..5f3eedc 100644 --- a/analyze.h +++ b/analyze.h @@ -3,6 +3,7 @@ #include "mavlink_message_handler.h" #include "analyzer.h" +#include "analyzervehicle_copter.h" class Analyze : public MAVLink_Message_Handler { @@ -26,6 +27,8 @@ class Analyze : public MAVLink_Message_Handler { void set_output_style(output_style_option option) { _output_style = option;} private: + AnalyzerVehicle::Base *vehicle; + output_style_option _output_style; #define MAX_ANALYZERS 10 uint8_t next_analyzer; @@ -45,6 +48,7 @@ class Analyze : public MAVLink_Message_Handler { virtual void handle_decoded_message(uint64_t T, mavlink_heartbeat_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); diff --git a/analyzer.h b/analyzer.h index 06c720e..38f0a76 100644 --- a/analyzer.h +++ b/analyzer.h @@ -6,12 +6,15 @@ #include // libjsoncpp0 and libjsoncpp-dev on debian #include // libjsoncpp0 and libjsoncpp-dev on debian +#include "analyzervehicle.h" + 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) { } virtual const char *name() = 0; @@ -27,9 +30,15 @@ class Analyzer : public MAVLink_Message_Handler { protected: std::string to_string(double x); uint16_t evilness; + AnalyzerVehicle::Base *&_vehicle; private: }; #endif + + +// - two fundamental types of test +// - is the software working correctly (EKF issues) +// - is the vehicle doing sensible things (attitude etc) diff --git a/analyzer_battery.h b/analyzer_battery.h index 2c60f10..773cc2b 100644 --- a/analyzer_battery.h +++ b/analyzer_battery.h @@ -11,8 +11,8 @@ class Analyzer_Battery : public Analyzer { public: - Analyzer_Battery(int fd, struct sockaddr_in &sa) : - Analyzer(fd, sa), + Analyzer_Battery(int fd, struct sockaddr_in &sa, AnalyzerVehicle::Base *&vehicle) : + Analyzer(fd, sa, vehicle), seen_sys_status_packets(false), lowest_battery_remaining_seen(999999999.0f) { diff --git a/analyzer_brownout.cpp b/analyzer_brownout.cpp index 4085d8d..532ba0f 100644 --- a/analyzer_brownout.cpp +++ b/analyzer_brownout.cpp @@ -13,38 +13,21 @@ bool Analyzer_Brownout::configure(INIReader *config) { return true; } -void Analyzer_Brownout::handle_decoded_message(uint64_t T, mavlink_vfr_hud_t &vfr_hud) -{ - last_altitude = vfr_hud.alt; - seen_packets = true; -} - -void Analyzer_Brownout::handle_decoded_message(uint64_t T, mavlink_servo_output_raw_t &servos) -{ - last_servo_output[1] = servos.servo1_raw; - last_servo_output[2] = servos.servo2_raw; - last_servo_output[3] = servos.servo3_raw; - last_servo_output[4] = servos.servo4_raw; -} - void Analyzer_Brownout::results_json_results(Json::Value &root) { - if (seen_packets) { + if (_vehicle->pos().alt_modtime() > 0) { Json::Value result(Json::objectValue); result["timestamp"] = 0; Json::Value reason(Json::arrayValue); + + const float last_altitude = _vehicle->pos().alt(); if (last_altitude > max_last_altitude && - (last_servo_output[1] > 1250 || - last_servo_output[2] > 1250 || - last_servo_output[3] > 1250 || - last_servo_output[4] > 1250 - )) { + _vehicle->is_flying()) { uint8_t this_sin_score = 10; result["status"] = "FAILED"; result["evilness"] = this_sin_score; add_evilness(this_sin_score); - reason.append(string_format("Possible brownout detected (end of log at %f metres, servo outputs (%f/%f/%f/%f))", - last_altitude, last_servo_output[1], last_servo_output[2], last_servo_output[3], last_servo_output[4] )); + reason.append(string_format("Possible brownout detected (end of log at %f metres, still flying)", last_altitude)); } else { result["status"] = "OK"; reason.append(string_format("No brownout detected (final altitude %f metres", last_altitude)); diff --git a/analyzer_brownout.h b/analyzer_brownout.h index 0efea27..13f9d53 100644 --- a/analyzer_brownout.h +++ b/analyzer_brownout.h @@ -11,11 +11,9 @@ class Analyzer_Brownout : public Analyzer { public: - Analyzer_Brownout(int fd, struct sockaddr_in &sa) : - Analyzer(fd, sa), - seen_packets(false), - last_altitude(0.0f), - last_servo_output{ 0 } + Analyzer_Brownout(int fd, struct sockaddr_in &sa, AnalyzerVehicle::Base *&vehicle) : + Analyzer(fd, sa, vehicle), + seen_packets(false) { } @@ -24,17 +22,11 @@ class Analyzer_Brownout : public Analyzer { return "Log does not end while vehicle appears to be flying"; } bool configure(INIReader *config); - void handle_decoded_message(uint64_t T, mavlink_vfr_hud_t &) override; - void handle_decoded_message(uint64_t T, mavlink_servo_output_raw_t &) override; - bool has_failed(); void results_json_results(Json::Value &root); private: bool seen_packets; - double last_altitude; - - double last_servo_output[17]; const double max_last_altitude = 5.0f; // metres }; diff --git a/analyzer_compass_offsets.h b/analyzer_compass_offsets.h index 20d798d..feec511 100644 --- a/analyzer_compass_offsets.h +++ b/analyzer_compass_offsets.h @@ -11,8 +11,8 @@ class Analyzer_Compass_Offsets : public Analyzer { public: - Analyzer_Compass_Offsets(int fd, struct sockaddr_in &sa) : - Analyzer(fd, sa), + Analyzer_Compass_Offsets(int fd, struct sockaddr_in &sa, AnalyzerVehicle::Base *&vehicle) : + Analyzer(fd, sa, vehicle), warn_offset(100), fail_offset(200), compass_offset_results_offset(0) diff --git a/analyzer_crashed.cpp b/analyzer_crashed.cpp index 8a02acd..63618a2 100644 --- a/analyzer_crashed.cpp +++ b/analyzer_crashed.cpp @@ -6,86 +6,56 @@ #include "util.h" #include "analyzer_util.h" +#include "analyzervehicle_copter.h" -bool Analyzer_Crashed::configure(INIReader *config) { - if (!MAVLink_Message_Handler::configure(config)) { - return false; - } - return true; -} void Analyzer_Crashed::handle_decoded_message(uint64_t T, mavlink_param_value_t ¶m) { - const char *name = param.param_id; - float value = param.param_value/100; // centi-degrees to degrees - if (streq(name, "ANGLE_MAX")) { - // convert to radians - angle_max = value/180 * M_PI; - } + evaluate(T); } - -bool Analyzer_Crashed::has_crashed() +void Analyzer_Crashed::handle_decoded_message(uint64_t T, mavlink_attitude_t &msg) { - if (!exceeded_angle_max) { - return false; - } - - if (last_servo_output[1] < servo_output_threshold && - last_servo_output[2] < servo_output_threshold && - last_servo_output[3] < servo_output_threshold && - last_servo_output[4] < servo_output_threshold) { - return false; - } - - return true; + Analyzer::handle_decoded_message(T, msg); + evaluate(T); + seen_packets_attitude = true; } - -void Analyzer_Crashed::evaluate_crashed(uint64_t T) +void Analyzer_Crashed::handle_decoded_message(uint64_t T, mavlink_servo_output_raw_t &servos) { - if (has_crashed()) { - crashed = true; - crashed_timestamp = T; - } + Analyzer::handle_decoded_message(T, servos); + evaluate(T); } -void Analyzer_Crashed::handle_decoded_message(uint64_t T, mavlink_attitude_t &att) -{ - if (att.roll > angle_max || att.pitch > angle_max) { - exceeded_angle_max = true; - if (angle_max > angle_max_achieved) { - angle_max_achieved = angle_max; - exceeded_angle_max_timestamp = T; - } - } - evaluate_crashed(T); - seen_packets_attitude = true; -} -void Analyzer_Crashed::handle_decoded_message(uint64_t T, mavlink_servo_output_raw_t &servos) +void Analyzer_Crashed::evaluate(uint64_t T) { - last_servo_output[1] = servos.servo1_raw; - last_servo_output[2] = servos.servo2_raw; - last_servo_output[3] = servos.servo3_raw; - last_servo_output[4] = servos.servo4_raw; + AnalyzerVehicle::Copter *v = (AnalyzerVehicle::Copter*&)_vehicle; - evaluate_crashed(T); + if (v->exceeding_angle_max() && + v->any_motor_running_fast()) { + crashed = true; + crashed_timestamp = T; + crashed_angle = (v->att().roll() > v->att().pitch()) ? v->att().roll() : v->att().pitch(); + for (uint8_t i=1; i<=v->_num_motors; i++) { + crash_servo_output[i] = v->_servo_output[i]; + } + } } - void Analyzer_Crashed::results_json_results(Json::Value &root) { Json::Value result(Json::objectValue); Json::Value reason(Json::arrayValue); uint8_t this_sin_score = 0; + AnalyzerVehicle::Copter *v = (AnalyzerVehicle::Copter*&)_vehicle; if (crashed) { reason.append("Crashed"); - reason.append(string_format("ANGLE_MAX (%f > %f)", angle_max_achieved*180/M_PI, angle_max*180/M_PI)); // FIXME - for (uint8_t i=1; i<=4; i++) { - if (last_servo_output[i] > servo_output_threshold) { + reason.append(string_format("ANGLE_MAX (%f > %f)", rad_to_deg(crashed_angle), v->params["ANGLE_MAX"]/100)); // FIXME + for (uint8_t i=1; i<=v->_num_motors; i++) { + if (v->_servo_output[i] > v->is_flying_motor_threshold) { reason.append(string_format("SERVO_OUTPUT_RAW.servo%d_raw=%f", - i, last_servo_output[i])); + i, crash_servo_output[i])); } } this_sin_score = 10; diff --git a/analyzer_crashed.h b/analyzer_crashed.h index 2bab820..5f20a6d 100644 --- a/analyzer_crashed.h +++ b/analyzer_crashed.h @@ -13,13 +13,12 @@ class Analyzer_Crashed : public Analyzer { public: - Analyzer_Crashed(int fd, struct sockaddr_in &sa) : - Analyzer(fd, sa), + Analyzer_Crashed(int fd, struct sockaddr_in &sa, AnalyzerVehicle::Base *&vehicle) : + Analyzer(fd, sa, vehicle), seen_packets_attitude(false), crashed(false), - angle_max(angle_max_default_degrees/180 * M_PI), - angle_max_achieved(0), - last_servo_output{ 0 } + // angle_max(angle_max_default_degrees/180 * M_PI), + crash_servo_output{ 0 } { } const uint16_t servo_output_threshold = 1250; @@ -29,7 +28,6 @@ class Analyzer_Crashed : public Analyzer { return "The vehicle crashed"; } - bool configure(INIReader *config); void handle_decoded_message(uint64_t T, mavlink_param_value_t ¶m) override; void handle_decoded_message(uint64_t T, mavlink_attitude_t &att); void handle_decoded_message(uint64_t T, mavlink_servo_output_raw_t &servos); @@ -38,19 +36,19 @@ class Analyzer_Crashed : public Analyzer { private: bool has_crashed(); - void evaluate_crashed(uint64_t T); + void evaluate(uint64_t T); bool seen_packets_attitude; bool crashed; - double angle_max; // radians + float crashed_angle; + bool exceeded_angle_max; - bool angle_max_achieved; uint64_t crashed_timestamp; uint64_t exceeded_angle_max_timestamp; // const double angle_max_default_degrees = 20.0f; - double last_servo_output[17]; // FIXME take 17 from somewhere... + double crash_servo_output[17]; // FIXME take 17 from somewhere... void results_json_results(Json::Value &root); }; diff --git a/analyzer_ever_armed.cpp b/analyzer_ever_armed.cpp index efcc409..abbb282 100644 --- a/analyzer_ever_armed.cpp +++ b/analyzer_ever_armed.cpp @@ -5,17 +5,9 @@ #include "util.h" -bool Analyzer_Ever_Armed::configure(INIReader *config) { - if (!MAVLink_Message_Handler::configure(config)) { - return false; - } - return true; -} - -// swiped from AnalyzerTest_Compass in experimental ArduPilot tree: void Analyzer_Ever_Armed::handle_decoded_message(uint64_t T, mavlink_heartbeat_t &heartbeat) { - if (heartbeat.base_mode & MAV_MODE_FLAG_SAFETY_ARMED) { + if (_vehicle->is_armed()) { arm_time = T; ever_armed = true; } diff --git a/analyzer_ever_armed.h b/analyzer_ever_armed.h index 79a2290..78a06ab 100644 --- a/analyzer_ever_armed.h +++ b/analyzer_ever_armed.h @@ -11,15 +11,14 @@ class Analyzer_Ever_Armed : public Analyzer { public: - Analyzer_Ever_Armed(int fd, struct sockaddr_in &sa) : - Analyzer(fd, sa), + Analyzer_Ever_Armed(int fd, struct sockaddr_in &sa, AnalyzerVehicle::Base *&vehicle) : + Analyzer(fd, sa, vehicle), ever_armed(false) { } const char *name() { return "Ever Armed"; } const char *description(); - bool configure(INIReader *config); void handle_decoded_message(uint64_t T, mavlink_heartbeat_t &hearbeat) override; void results_json_results(Json::Value &root); diff --git a/analyzer_ever_flew.cpp b/analyzer_ever_flew.cpp index e8c5d47..8b5d118 100644 --- a/analyzer_ever_flew.cpp +++ b/analyzer_ever_flew.cpp @@ -5,51 +5,39 @@ #include "util.h" -bool Analyzer_Ever_Flew::configure(INIReader *config) { - if (!MAVLink_Message_Handler::configure(config)) { - return false; +void Analyzer_Ever_Flew::evaluate(uint64_t T) +{ + if (pass_timestamp != 0) { + // already passed + return; } - return true; -} -bool Analyzer_Ever_Flew::has_passed() { - if (ever_armed == false) { - return false; + if (_vehicle->is_armed()) { + ever_armed = true; } - if (servos_past_threshold == false) { - return false; + if (((AnalyzerVehicle::Copter*&)_vehicle)->any_motor_running_fast()) { + servos_past_threshold = true; + } + + if (_vehicle->is_flying()) { + pass_timestamp = T; } - return true; } -// swiped from AnalyzeTest_Compass in experimental ArduPilot tree: + void Analyzer_Ever_Flew::handle_decoded_message(uint64_t T, mavlink_heartbeat_t &heartbeat) { - if (heartbeat.base_mode & MAV_MODE_FLAG_SAFETY_ARMED) { - ever_armed = true; - if (has_passed()) { - pass_timestamp = T; - } - } + evaluate(T); } void Analyzer_Ever_Flew::handle_decoded_message(uint64_t T, mavlink_servo_output_raw_t &servos) { - uint16_t threshold = 1250; - if (servos.servo1_raw > threshold || - servos.servo2_raw > threshold || - servos.servo3_raw > threshold || - servos.servo4_raw > threshold) { - servos_past_threshold = true; - if (has_passed()) { - pass_timestamp = 1; - } - } + evaluate(T); } void Analyzer_Ever_Flew::results_json_results(Json::Value &results) { Json::Value everflew(Json::objectValue); - if (has_passed()) { + if (pass_timestamp) { Json::Value timestamp(Json::objectValue); timestamp = (Json::UInt64)pass_timestamp; everflew["timestamp"] = timestamp; diff --git a/analyzer_ever_flew.h b/analyzer_ever_flew.h index ab36898..452f5cc 100644 --- a/analyzer_ever_flew.h +++ b/analyzer_ever_flew.h @@ -7,11 +7,12 @@ */ #include "analyzer.h" +#include "analyzervehicle_copter.h" class Analyzer_Ever_Flew : public Analyzer { public: - Analyzer_Ever_Flew(int fd, struct sockaddr_in &sa) : - Analyzer(fd, sa), + Analyzer_Ever_Flew(int fd, struct sockaddr_in &sa, AnalyzerVehicle::Base *&vehicle) : + Analyzer(fd, sa, vehicle), ever_armed(false), servos_past_threshold(false), pass_timestamp(0) @@ -20,7 +21,6 @@ class Analyzer_Ever_Flew : public Analyzer { const char *name() { return "Ever Flew"; } const char *description() { return "The vehicle flew, as defined by having ever armed and having the motor outputs pass a threshold value"; } - bool configure(INIReader *config); void handle_decoded_message(uint64_t T, mavlink_heartbeat_t &hearbeat) override; void handle_decoded_message(uint64_t T, mavlink_servo_output_raw_t &servos) override; @@ -30,7 +30,7 @@ const char *description() { return "The vehicle flew, as defined by having ever bool servos_past_threshold; uint64_t pass_timestamp; - bool has_passed(); + void evaluate(uint64_t T); }; #endif diff --git a/analyzer_good_ekf.h b/analyzer_good_ekf.h index f86db75..c05601e 100644 --- a/analyzer_good_ekf.h +++ b/analyzer_good_ekf.h @@ -11,8 +11,8 @@ class Analyzer_Good_EKF : public Analyzer { public: - Analyzer_Good_EKF(int fd, struct sockaddr_in &sa) : - Analyzer(fd, sa), + Analyzer_Good_EKF(int fd, struct sockaddr_in &sa, AnalyzerVehicle::Base *&vehicle) : + Analyzer(fd, sa, vehicle), seen_ekf_packets(false), velocity_variance{ diff --git a/analyzer_util.h b/analyzer_util.h index d724e78..3aa8494 100644 --- a/analyzer_util.h +++ b/analyzer_util.h @@ -1,3 +1,6 @@ +#ifndef _ANALYZER_UTIL +#define _ANALYZER_UTIL + // from: http://stackoverflow.com/questions/2342162/stdstring-formatting-like-sprintf #include template @@ -10,3 +13,17 @@ std::string string_format( const char* format, Args ... args ) } #define streq(x,y) (!strcmp(x,y)) + +// inline float deg_to_rad(const float deg) { +// return deg/M_PI * 180; +// } + +// inline float rad_to_deg(const float rad) { +// return rad*180/M_PI; +// } + +#define deg_to_rad(x) (x/M_PI * 180) +#define rad_to_deg(x) (x/180 * M_PI) + + +#endif diff --git a/analyzervehicle.cpp b/analyzervehicle.cpp new file mode 100644 index 0000000..b23b441 --- /dev/null +++ b/analyzervehicle.cpp @@ -0,0 +1,67 @@ +#include "analyzervehicle.h" + +using namespace AnalyzerVehicle; + +void Base::handle_decoded_message(uint64_t T, mavlink_statustext_t &msg) +{ + history_statustext.packet(msg); +} + +void Base::handle_decoded_message(uint64_t T, mavlink_heartbeat_t &msg) +{ + history_heartbeat.packet(msg); + _armed = msg.base_mode & MAV_MODE_FLAG_SAFETY_ARMED; +} + +void Base::handle_decoded_message(uint64_t T, mavlink_servo_output_raw_t &msg) +{ + history_servo_output_raw.packet(msg); +} + +char * +xstrdup(const char *x) +{ + char *ret = strdup(x); + if (ret == NULL) { + fprintf(stderr, "Failed to strdup: %s", strerror(errno)); + abort(); + } + return ret; +} + +char * +xcalloc(size_t size) +{ + char *ret = (char*)calloc(1, size); + if (ret == NULL) { + fprintf(stderr, "Failed to calloc: %s", strerror(errno)); + abort(); + } + return ret; +} + + + +void Base::handle_decoded_message(uint64_t T, mavlink_param_value_t &msg) { + // FIXME: getting the same parameter multiple times leaks memory + char *str = xcalloc(17); // FIXME constant + memcpy(str, msg.param_id, 16); + params[str] = msg.param_value; +} + +void Base::handle_decoded_message(uint64_t T, mavlink_attitude_t &msg) +{ + att().set_roll(T, msg.roll); + att().set_pitch(T, msg.pitch); + att().set_yaw(T, msg.yaw); +} + +bool Base::seen_parameter(const char *name) +{ + // ::printf("Looking for (%s)\n", name); + std::map::iterator it = params.find(std::string(name)); + if (it != params.end()) { + return true; + } + return false; +} diff --git a/analyzervehicle.h b/analyzervehicle.h new file mode 100644 index 0000000..e747a52 --- /dev/null +++ b/analyzervehicle.h @@ -0,0 +1,119 @@ +#ifndef _ANALYZER_VEHICLE +#define _ANALYZER_VEHICLE + +#include +#include + +#include "mavlink/c_library/ardupilotmega/mavlink.h" +#include "mavlink/c_library/common/mavlink.h" + +namespace AnalyzerVehicle { + + // AV_Attitude should be the best guess as to what the vehicle's + // status is - typicall the POS message from dataflash, for + // example + class AV_Attitude { + public: + float roll() { return _roll; }; + float pitch() { return _pitch; }; + float yaw() { return _yaw; }; + void set_roll(uint64_t T, float roll) { + _roll = roll; + _roll_modtime = T; + } + void set_pitch(uint64_t T, float pitch) { + _pitch = pitch; + _pitch_modtime = T; + } + void set_yaw(uint64_t T, float yaw) { + _yaw = yaw; + _yaw_modtime = T; + } + + private: + float _roll; + float _pitch; + float _yaw; + uint64_t _roll_modtime; + uint64_t _pitch_modtime; + uint64_t _yaw_modtime; + }; + + class AV_Position { + public: + void set_alt(uint64_t T, float alt) { + _alt = alt; + _alt_modtime = T; + } + float alt() { return _alt; }; + uint64_t alt_modtime() { return _alt_modtime; }; + + private: + float _lat; + float _lon; + float _alt; // relative + uint64_t _alt_modtime; + }; + + class AV_PosNED { + public: + uint16_t N; + uint16_t E; + uint16_t D; + }; + + template + class PacketHistory { + public: + PacketHistory() : + next(0) { } + void packet(packettype &packet) { + memcpy(&packets[next++], &packet, sizeof(packet)); + if (next >= size) { + next = 0; + } + if (count < size) { + count++; + } + } + private: + static const uint8_t size = 20; + uint8_t next; + uint8_t count; + packettype packets[size]; + }; + +class Base { +public: + bool _armed; + std::map params; + + virtual bool is_flying() = 0; + virtual bool is_armed() { return _armed; }; + void exceeding_angle_max(); + + bool seen_parameter(const char *name); + + virtual void handle_decoded_message(uint64_t T, mavlink_attitude_t&); + virtual void handle_decoded_message(uint64_t T, mavlink_heartbeat_t&); + virtual void handle_decoded_message(uint64_t T, mavlink_param_value_t &msg); + virtual void handle_decoded_message(uint64_t T, mavlink_statustext_t&); + virtual void handle_decoded_message(uint64_t T, mavlink_servo_output_raw_t &); + AV_Attitude& att() { return _att; }; + AV_Position& pos() { return _pos; }; + +protected: + AV_Attitude _att; + AV_Position _pos; + +private: + + PacketHistory history_heartbeat; + PacketHistory history_servo_output_raw; + PacketHistory history_statustext; + +}; // end class + +}; // end namespace + +#endif diff --git a/analyzervehicle_copter.cpp b/analyzervehicle_copter.cpp new file mode 100644 index 0000000..c364078 --- /dev/null +++ b/analyzervehicle_copter.cpp @@ -0,0 +1,68 @@ +#include "analyzervehicle_copter.h" + +using namespace AnalyzerVehicle; + +bool Copter::is_flying() { + if (! is_armed()) { + // we hope we're not flying, anyway! + return false; + } + + if (! any_motor_running_fast()) { + return false; + } + + return true; +} + +bool Copter::any_motor_running_fast() { + for (uint8_t i=1; i<_num_motors; i++) { + if (_servo_output[i] > is_flying_motor_threshold) { + return true; + } + } + return false; +} +void Copter::handle_decoded_message(uint64_t T, mavlink_statustext_t &msg) { + Base::handle_decoded_message(T, msg); + + if (strstr(msg.text, "Frame")) { + if (strstr(msg.text, "QUAD")) { + _num_motors = 4; + } + } +} + +bool Copter::exceeding_angle_max() +{ + if (!seen_parameter("ANGLE_MAX")) { + return false; + } + + float angle_max = params["ANGLE_MAX"] / 100; // convert from centidegrees + float angle_max_radians = angle_max/180.0 * M_PI; + + ::printf("att.roll=%f\n", att().roll()); + if (att().roll() > angle_max_radians) { + return true; + } + if (att().pitch() > angle_max_radians) { + return true; + } + return false; +} + + +void Copter::handle_decoded_message(uint64_t T, mavlink_servo_output_raw_t &msg) +{ + Base::handle_decoded_message(T, msg); + + _servo_output[1] = msg.servo1_raw; + _servo_output[2] = msg.servo2_raw; + _servo_output[3] = msg.servo3_raw; + _servo_output[4] = msg.servo4_raw; + _servo_output[5] = msg.servo5_raw; + _servo_output[6] = msg.servo6_raw; + _servo_output[7] = msg.servo7_raw; + _servo_output[8] = msg.servo8_raw; +} diff --git a/analyzervehicle_copter.h b/analyzervehicle_copter.h new file mode 100644 index 0000000..de52dd6 --- /dev/null +++ b/analyzervehicle_copter.h @@ -0,0 +1,31 @@ +#include "analyzervehicle.h" + +#ifndef _ANALYZER_VEHICLE_COPTER +#define _ANALYZER_VEHICLE_COPTER + +namespace AnalyzerVehicle { + + class Copter : public Base { + public: + Copter() : + _num_motors(0), + _servo_output{ 0 } + { } + bool is_flying(); + void handle_decoded_message(uint64_t T, mavlink_statustext_t &msg); + void handle_decoded_message(uint64_t T, mavlink_servo_output_raw_t &msg); + + bool any_motor_running_fast(); + bool exceeding_angle_max(); + + uint8_t _num_motors; // e.g. 4 for a quad... + uint16_t _servo_output[9]; // indexed from 1 + static const uint16_t is_flying_motor_threshold = 1250; + protected: + + private: + }; + +}; // end namepsace + +#endif diff --git a/mavlink_message_handler.h b/mavlink_message_handler.h index e1958e4..3afeab4 100644 --- a/mavlink_message_handler.h +++ b/mavlink_message_handler.h @@ -46,11 +46,12 @@ class MAVLink_Message_Handler { virtual void handle_decoded_message(uint64_t T, mavlink_ahrs2_t &msg) { } 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_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_sys_status_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_statustext_t &msg) { } virtual void handle_decoded_message(uint64_t T, mavlink_vfr_hud_t &msg) { } virtual void end_of_log(uint32_t packet_count) { } diff --git a/mavlink_reader.cpp b/mavlink_reader.cpp index 56842e9..39914aa 100644 --- a/mavlink_reader.cpp +++ b/mavlink_reader.cpp @@ -196,6 +196,12 @@ void MAVLink_Reader::handle_message_received(uint64_t timestamp, mavlink_message handle_decoded_message_received(timestamp, decoded); // template in .h break; } + case MAVLINK_MSG_ID_EKF_STATUS_REPORT: { + mavlink_ekf_status_report_t decoded; + mavlink_msg_ekf_status_report_decode(&msg, &decoded); + handle_decoded_message_received(timestamp, decoded); // template in .h + break; + } case MAVLINK_MSG_ID_HEARTBEAT: { mavlink_heartbeat_t decoded; mavlink_msg_heartbeat_decode(&msg, &decoded); @@ -214,9 +220,9 @@ void MAVLink_Reader::handle_message_received(uint64_t timestamp, mavlink_message handle_decoded_message_received(timestamp, decoded); // template in .h break; } - case MAVLINK_MSG_ID_EKF_STATUS_REPORT: { - mavlink_ekf_status_report_t decoded; - mavlink_msg_ekf_status_report_decode(&msg, &decoded); + case MAVLINK_MSG_ID_STATUSTEXT: { + mavlink_statustext_t decoded; + mavlink_msg_statustext_decode(&msg, &decoded); handle_decoded_message_received(timestamp, decoded); // template in .h break; } From e4d526bf5181ad8454b17b20f28f3de2b23a7b69 Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Thu, 27 Aug 2015 09:25:45 +1000 Subject: [PATCH 2/6] la: rejig compasss offsets to use vehicle --- analyze.h | 1 + analyzer_compass_offsets.cpp | 116 ++++++++++++++++++----------------- analyzer_compass_offsets.h | 14 ++--- analyzer_crashed.cpp | 2 +- analyzervehicle.cpp | 17 ++--- analyzervehicle.h | 21 +++++-- analyzervehicle_copter.cpp | 5 +- 7 files changed, 97 insertions(+), 79 deletions(-) diff --git a/analyze.h b/analyze.h index 5f3eedc..e993b00 100644 --- a/analyze.h +++ b/analyze.h @@ -10,6 +10,7 @@ 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) { diff --git a/analyzer_compass_offsets.cpp b/analyzer_compass_offsets.cpp index 9d83e12..9f2a4d0 100644 --- a/analyzer_compass_offsets.cpp +++ b/analyzer_compass_offsets.cpp @@ -12,73 +12,79 @@ bool Analyzer_Compass_Offsets::configure(INIReader *config) { return true; } -// void Analyzer_Compass_Offsets::handle_decoded_message(mavlink_param_value_t ¶m) -// { -// ::printf("Param found\n"); -// } - double vec_len(double vec[3]) { return sqrt(vec[0]*vec[0] + vec[1]*vec[1] + vec[2]*vec[2]); } -// swiped from AnalyzeTest_Compass in experimental ArduPilot tree: -void Analyzer_Compass_Offsets::handle_decoded_message(uint64_t T, mavlink_param_value_t ¶m) +// true if we have all elements of vector and we haven't processed +// this vector before +bool Analyzer_Compass_Offsets::new_compass_results() +{ + if (!_vehicle->param_seen("COMPASS_OFS_X") || + !_vehicle->param_seen("COMPASS_OFS_Y") || + !_vehicle->param_seen("COMPASS_OFS_Z")) { + // we haven't seen the entire vector yet + return false; + } + + if (_vehicle->param_modtime("COMPASS_OFS_X") == modtime_compass_ofs[0] && + _vehicle->param_modtime("COMPASS_OFS_Y") == modtime_compass_ofs[1] && + _vehicle->param_modtime("COMPASS_OFS_Z") == modtime_compass_ofs[2]) { + // nothing has changed; at time of writing we get called for + // every parameter which is set (low-bandwidth, so *shrug*) + return false; + } + + modtime_compass_ofs[0] = _vehicle->param_modtime("COMPASS_OFS_X"); + modtime_compass_ofs[1] = _vehicle->param_modtime("COMPASS_OFS_Y"); + modtime_compass_ofs[2] = _vehicle->param_modtime("COMPASS_OFS_Z"); + + return true; +} + +void Analyzer_Compass_Offsets::evaluate(uint64_t T) { - int8_t ofs = -1; - const char *name = param.param_id; - const uint8_t namelen = MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN; - float value = param.param_value; if (compass_offset_results_offset == MAX_COMPASS_OFFSET_RESULTS) { compass_offset_results_overrun = true; return; } - // watch the following; name is not null-terminated when 16-bytes long ;-) - // ::printf("Param: %s = %f\n", name, value); - - if (!strncmp(name, "COMPASS_OFS_X", namelen)) { - ofs = 0; - } else if (!strncmp(name, "COMPASS_OFS_Y", namelen)) { - ofs = 1; - } else if (!strncmp(name, "COMPASS_OFS_Z", namelen)) { - ofs = 2; + + if (! new_compass_results()) { + return; } - if (ofs != -1) { - if (have_compass_ofs[ofs] && - compass_ofs[ofs] == value) { - // skip duplicate values - return; - } - compass_ofs[ofs] = value; - have_compass_ofs[ofs] = true; - if (have_compass_ofs[0] && - have_compass_ofs[1] && - have_compass_ofs[2]) { - double len = vec_len(compass_ofs); - compass_offset_status status; - if (len >= fail_offset) { - status = compass_offset_fail; - // ::printf("FAIL: COMPASS_OFS %f>%d\n", len, fail_offset); - } else if (len >= warn_offset) { - // ::printf("WARNING: COMPASS_OFS %f>%d\n", len, warn_offset); - status = compass_offset_warn; - } else if (is_zero(len)) { - // ::printf("WARNING: Zero COMPASS_OFS - that's probably bad\n"); - status = compass_offset_zero; - } else { - // ::printf("OK: COMPASS_OFS %f looks good\n", len); - status = compass_offset_ok; - } - - compass_offset_results[compass_offset_results_offset].len = len; - compass_offset_results[compass_offset_results_offset].status = status; - compass_offset_results[compass_offset_results_offset].timestamp = T; - compass_offset_results_offset++; - // have_compass_ofs[0] = false; - // have_compass_ofs[1] = false; - // have_compass_ofs[2] = false; - } + + double compass_ofs[3]; + compass_ofs[0] = _vehicle->param("COMPASS_OFS_X"); + compass_ofs[1] = _vehicle->param("COMPASS_OFS_Y"); + compass_ofs[2] = _vehicle->param("COMPASS_OFS_Z"); + + double len = vec_len(compass_ofs); + compass_offset_status status; + if (len >= fail_offset) { + status = compass_offset_fail; + // ::printf("FAIL: COMPASS_OFS %f>%d\n", len, fail_offset); + } else if (len >= warn_offset) { + // ::printf("WARNING: COMPASS_OFS %f>%d\n", len, warn_offset); + status = compass_offset_warn; + } else if (is_zero(len)) { + // ::printf("WARNING: Zero COMPASS_OFS - that's probably bad\n"); + status = compass_offset_zero; + } else { + // ::printf("OK: COMPASS_OFS %f looks good\n", len); + status = compass_offset_ok; } + + compass_offset_results[compass_offset_results_offset].len = len; + compass_offset_results[compass_offset_results_offset].status = status; + compass_offset_results[compass_offset_results_offset].timestamp = T; + compass_offset_results_offset++; +} + +// swiped from AnalyzeTest_Compass in experimental ArduPilot tree: +void Analyzer_Compass_Offsets::handle_decoded_message(uint64_t T, mavlink_param_value_t ¶m) +{ + evaluate(T); } #include diff --git a/analyzer_compass_offsets.h b/analyzer_compass_offsets.h index feec511..2f0af7b 100644 --- a/analyzer_compass_offsets.h +++ b/analyzer_compass_offsets.h @@ -13,14 +13,11 @@ class Analyzer_Compass_Offsets : public Analyzer { public: Analyzer_Compass_Offsets(int fd, struct sockaddr_in &sa, AnalyzerVehicle::Base *&vehicle) : Analyzer(fd, sa, vehicle), + modtime_compass_ofs{ }, warn_offset(100), fail_offset(200), compass_offset_results_offset(0) - { - have_compass_ofs[0] = false; - have_compass_ofs[1] = false; - have_compass_ofs[2] = false; - } + { } const char *name() { return "Compass Offsets"; } const char *description() { @@ -30,10 +27,11 @@ class Analyzer_Compass_Offsets : public Analyzer { bool configure(INIReader *config); void handle_decoded_message(uint64_t T, mavlink_param_value_t ¶m) override; bool is_zero(double x) { return x < 0.00001; } // FIXME + void evaluate(uint64_t T); private: - double compass_ofs[3]; - bool have_compass_ofs[3]; + uint64_t modtime_compass_ofs[3]; + const uint16_t warn_offset; const uint16_t fail_offset; @@ -48,6 +46,8 @@ class Analyzer_Compass_Offsets : public Analyzer { double len; compass_offset_status status; }; + bool new_compass_results(); + void do_add_evilness(struct compass_offset_result result); #define MAX_COMPASS_OFFSET_RESULTS 100 diff --git a/analyzer_crashed.cpp b/analyzer_crashed.cpp index 63618a2..5a1ff5e 100644 --- a/analyzer_crashed.cpp +++ b/analyzer_crashed.cpp @@ -51,7 +51,7 @@ void Analyzer_Crashed::results_json_results(Json::Value &root) AnalyzerVehicle::Copter *v = (AnalyzerVehicle::Copter*&)_vehicle; if (crashed) { reason.append("Crashed"); - reason.append(string_format("ANGLE_MAX (%f > %f)", rad_to_deg(crashed_angle), v->params["ANGLE_MAX"]/100)); // FIXME + reason.append(string_format("ANGLE_MAX (%f > %f)", rad_to_deg(crashed_angle), v->param("ANGLE_MAX")/100)); // FIXME for (uint8_t i=1; i<=v->_num_motors; i++) { if (v->_servo_output[i] > v->is_flying_motor_threshold) { reason.append(string_format("SERVO_OUTPUT_RAW.servo%d_raw=%f", diff --git a/analyzervehicle.cpp b/analyzervehicle.cpp index b23b441..472853c 100644 --- a/analyzervehicle.cpp +++ b/analyzervehicle.cpp @@ -46,7 +46,8 @@ void Base::handle_decoded_message(uint64_t T, mavlink_param_value_t &msg) { // FIXME: getting the same parameter multiple times leaks memory char *str = xcalloc(17); // FIXME constant memcpy(str, msg.param_id, 16); - params[str] = msg.param_value; + _param[str] = msg.param_value; + _param_modtime[str] = T; } void Base::handle_decoded_message(uint64_t T, mavlink_attitude_t &msg) @@ -56,12 +57,12 @@ void Base::handle_decoded_message(uint64_t T, mavlink_attitude_t &msg) att().set_yaw(T, msg.yaw); } -bool Base::seen_parameter(const char *name) +bool Base::param_seen(const char *name) const { - // ::printf("Looking for (%s)\n", name); - std::map::iterator it = params.find(std::string(name)); - if (it != params.end()) { - return true; - } - return false; + const std::string x = std::string(name); + std::map::const_iterator it = _param.find(x); + if (it != _param.end()) { + return true; + } + return false; } diff --git a/analyzervehicle.h b/analyzervehicle.h index e747a52..7a313e2 100644 --- a/analyzervehicle.h +++ b/analyzervehicle.h @@ -66,7 +66,9 @@ namespace AnalyzerVehicle { class PacketHistory { public: PacketHistory() : - next(0) { } + next(0), + count(0) + { } void packet(packettype &packet) { memcpy(&packets[next++], &packet, sizeof(packet)); if (next >= size) { @@ -85,14 +87,19 @@ namespace AnalyzerVehicle { class Base { public: - bool _armed; - std::map params; + Base() : + _armed(false), + _att{ }, // not convinced we should be zeroing these -pb2150827 + _pos{ } + { } virtual bool is_flying() = 0; virtual bool is_armed() { return _armed; }; - void exceeding_angle_max(); + void exceeding_angle_max() const; - bool seen_parameter(const char *name); + float param(const char *name) { return _param[name]; }; + bool param_seen(const char *name) const; + uint64_t param_modtime(const char *name) { return _param_modtime[name]; } virtual void handle_decoded_message(uint64_t T, mavlink_attitude_t&); virtual void handle_decoded_message(uint64_t T, mavlink_heartbeat_t&); @@ -103,6 +110,10 @@ class Base { AV_Position& pos() { return _pos; }; protected: + bool _armed; + + std::map _param; + std::map _param_modtime; AV_Attitude _att; AV_Position _pos; diff --git a/analyzervehicle_copter.cpp b/analyzervehicle_copter.cpp index c364078..3a27c2a 100644 --- a/analyzervehicle_copter.cpp +++ b/analyzervehicle_copter.cpp @@ -35,14 +35,13 @@ void Copter::handle_decoded_message(uint64_t T, mavlink_statustext_t &msg) { bool Copter::exceeding_angle_max() { - if (!seen_parameter("ANGLE_MAX")) { + if (!param_seen("ANGLE_MAX")) { return false; } - float angle_max = params["ANGLE_MAX"] / 100; // convert from centidegrees + float angle_max = param("ANGLE_MAX") / 100; // convert from centidegrees float angle_max_radians = angle_max/180.0 * M_PI; - ::printf("att.roll=%f\n", att().roll()); if (att().roll() > angle_max_radians) { return true; } From 9fcafb019cbd173356ca534807196b29ea33b5c4 Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Thu, 27 Aug 2015 20:01:48 +1000 Subject: [PATCH 3/6] la: add attitude control test --- Makefile | 2 + analyze.cpp | 17 +++++ analyze.h | 1 + analyzer.h | 16 +++- analyzer_attitude_control.cpp | 136 ++++++++++++++++++++++++++++++++++ analyzer_attitude_control.h | 88 ++++++++++++++++++++++ analyzer_battery.cpp | 4 - analyzer_brownout.cpp | 6 +- analyzer_compass_offsets.cpp | 2 +- analyzer_compass_offsets.h | 1 - analyzer_crashed.cpp | 5 +- analyzer_crashed.h | 2 - analyzer_ever_armed.cpp | 3 - analyzer_ever_flew.cpp | 3 - analyzer_good_ekf.cpp | 3 - analyzer_util.cpp | 9 +++ analyzer_util.h | 9 ++- analyzervehicle.cpp | 60 +++++++-------- analyzervehicle.h | 73 ++++++++++++++---- analyzervehicle_copter.cpp | 5 +- mavlink_message_handler.h | 1 + mavlink_reader.cpp | 6 ++ 22 files changed, 372 insertions(+), 80 deletions(-) create mode 100644 analyzer_attitude_control.cpp create mode 100644 analyzer_attitude_control.h create mode 100644 analyzer_util.cpp diff --git a/Makefile b/Makefile index 49e5c9d..41b5205 100644 --- a/Makefile +++ b/Makefile @@ -23,6 +23,7 @@ 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 @@ -35,6 +36,7 @@ SRCS_CPP += analyzer_good_ekf.cpp SRCS_CPP += analyzer_battery.cpp SRCS_CPP += analyzer_brownout.cpp SRCS_CPP += analyzer_crashed.cpp +SRCS_CPP += analyzer_attitude_control.cpp SRCS_CPP += analyzervehicle_copter.cpp SRCS_CPP += analyzervehicle.cpp SRCS_CPP += la-log.cpp diff --git a/analyze.cpp b/analyze.cpp index 2f6d56f..b6120b0 100644 --- a/analyze.cpp +++ b/analyze.cpp @@ -7,6 +7,7 @@ #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" @@ -47,6 +48,13 @@ void Analyze::instantiate_analyzers(INIReader *config) syslog(LOG_INFO, "Failed to create analyzer_good_ekf"); } + 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"); @@ -389,6 +397,15 @@ void Analyze::handle_decoded_message(uint64_t T, mavlink_heartbeat_t &msg) { 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; ihandle_decoded_message(T, msg); + } +} void Analyze::handle_decoded_message(uint64_t T, mavlink_param_value_t &msg) { if (!vehicle) { return; diff --git a/analyze.h b/analyze.h index e993b00..021bc90 100644 --- a/analyze.h +++ b/analyze.h @@ -47,6 +47,7 @@ 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); diff --git a/analyzer.h b/analyzer.h index 38f0a76..3d358ef 100644 --- a/analyzer.h +++ b/analyzer.h @@ -11,10 +11,17 @@ class Analyzer : public MAVLink_Message_Handler { public: + enum analyzer_status { + analyzer_status_warn = 17, + analyzer_status_fail, + analyzer_status_ok, + }; + Analyzer(int fd, struct sockaddr_in &sa, AnalyzerVehicle::Base *&vehicle) : MAVLink_Message_Handler(fd, sa), evilness(0), - _vehicle(vehicle) + _vehicle(vehicle), + _status(analyzer_status_ok) { } virtual const char *name() = 0; @@ -25,14 +32,19 @@ 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 diff --git a/analyzer_attitude_control.cpp b/analyzer_attitude_control.cpp new file mode 100644 index 0000000..8199d6e --- /dev/null +++ b/analyzer_attitude_control.cpp @@ -0,0 +1,136 @@ +#include "analyzer_attitude_control.h" + +#include +#include "util.h" +#include "analyzer_util.h" + +bool Analyzer_Attitude_Control::configure(INIReader *config) { + if (!MAVLink_Message_Handler::configure(config)) { + return false; + } + return true; +} + +void Analyzer_Attitude_Control::evaluate(uint64_t T) +{ + if (!_vehicle->is_flying()) { + return; + } + float roll = _vehicle->att().roll(); + float desroll = _vehicle->nav().desroll(); + float roll_delta = fabs(roll - desroll); + + float pitch = _vehicle->att().pitch(); + float despitch = _vehicle->nav().despitch(); + float pitch_delta = fabs(pitch - despitch); + + float delta = (pitch_delta > roll_delta) ? pitch_delta : roll_delta; + +// ::printf("%s: attitude control roll=%f desroll=%f (%f / %f)\n", timestamp, roll, desroll, delta, offset_fail); + analyzer_status _status_prev = status(); + if (delta > offset_fail) { + set_status(analyzer_status_fail); + } else if (delta > offset_warn) { + set_status(analyzer_status_warn); + } else { + set_status(analyzer_status_ok); + } + + + if (status() == _status_prev) { + if (status() != analyzer_status_ok) { + attitude_control_result &result = attitude_control_results[attitude_control_results_offset]; + if (delta > result.deltamax) { + result.deltamax = delta; + result.roll_at_deltamax = roll; + result.desroll_at_deltamax = desroll; + result.pitch_at_deltamax = pitch; + result.despitch_at_deltamax = despitch; + } + } + } else { + attitude_control_results[attitude_control_results_offset].timestamp_stop = T; + switch(_status_prev) { + case analyzer_status_ok: + break; + case analyzer_status_warn: + case analyzer_status_fail: + attitude_control_results_offset++; + } + if (status() != analyzer_status_ok) { + attitude_control_result &result = attitude_control_results[attitude_control_results_offset]; + result.status = status(); + result.timestamp_start = T; + result.deltamax = delta; + result.roll_at_deltamax = roll; + result.desroll_at_deltamax = desroll; + result.pitch_at_deltamax = pitch; + result.despitch_at_deltamax = despitch; + } + _status_prev = status(); + } +} + +void Analyzer_Attitude_Control::end_of_log(uint32_t packet_count) +{ + if (status() != analyzer_status_ok) { + // close off the final result + attitude_control_results_offset++; + } +} + +void Analyzer_Attitude_Control::handle_decoded_message(uint64_t T, mavlink_nav_controller_output_t &msg) +{ + evaluate(T); +} + +void Analyzer_Attitude_Control::handle_decoded_message(uint64_t T, mavlink_attitude_t &msg) +{ + evaluate(T); +} + +void Analyzer_Attitude_Control::results_json_results(Json::Value &root) +{ + if (_vehicle->att().roll_modtime() > 0) { + for (uint8_t i=0; i -#include -#include // for fork() #include "util.h" #include "analyzer_util.h" @@ -17,7 +14,6 @@ bool Analyzer_Battery::has_failed() { return (lowest_battery_remaining_seen < low_battery_threshold); } -// swiped from AnalyzerTest_Compass in experimental ArduPilot tree: void Analyzer_Battery::handle_decoded_message(uint64_t T, mavlink_sys_status_t &sys_status) { if (sys_status.battery_remaining < lowest_battery_remaining_seen) { diff --git a/analyzer_brownout.cpp b/analyzer_brownout.cpp index 532ba0f..07150d6 100644 --- a/analyzer_brownout.cpp +++ b/analyzer_brownout.cpp @@ -1,8 +1,5 @@ #include "analyzer_brownout.h" -#include -#include -#include // for fork() #include "util.h" #include "analyzer_util.h" @@ -15,6 +12,9 @@ bool Analyzer_Brownout::configure(INIReader *config) { void Analyzer_Brownout::results_json_results(Json::Value &root) { + if (!_vehicle) { + return; + } if (_vehicle->pos().alt_modtime() > 0) { Json::Value result(Json::objectValue); result["timestamp"] = 0; diff --git a/analyzer_compass_offsets.cpp b/analyzer_compass_offsets.cpp index 9f2a4d0..b46fd04 100644 --- a/analyzer_compass_offsets.cpp +++ b/analyzer_compass_offsets.cpp @@ -1,9 +1,9 @@ #include "analyzer_compass_offsets.h" -#include #include #include "util.h" +#include "analyzer_util.h" bool Analyzer_Compass_Offsets::configure(INIReader *config) { if (!MAVLink_Message_Handler::configure(config)) { diff --git a/analyzer_compass_offsets.h b/analyzer_compass_offsets.h index 2f0af7b..0c2317b 100644 --- a/analyzer_compass_offsets.h +++ b/analyzer_compass_offsets.h @@ -26,7 +26,6 @@ class Analyzer_Compass_Offsets : public Analyzer { bool configure(INIReader *config); void handle_decoded_message(uint64_t T, mavlink_param_value_t ¶m) override; - bool is_zero(double x) { return x < 0.00001; } // FIXME void evaluate(uint64_t T); private: diff --git a/analyzer_crashed.cpp b/analyzer_crashed.cpp index 5a1ff5e..45f2ef8 100644 --- a/analyzer_crashed.cpp +++ b/analyzer_crashed.cpp @@ -1,8 +1,5 @@ #include "analyzer_crashed.h" -#include -#include - #include "util.h" #include "analyzer_util.h" @@ -51,7 +48,7 @@ void Analyzer_Crashed::results_json_results(Json::Value &root) AnalyzerVehicle::Copter *v = (AnalyzerVehicle::Copter*&)_vehicle; if (crashed) { reason.append("Crashed"); - reason.append(string_format("ANGLE_MAX (%f > %f)", rad_to_deg(crashed_angle), v->param("ANGLE_MAX")/100)); // FIXME + reason.append(string_format("ANGLE_MAX (%f > %f)", crashed_angle, v->param("ANGLE_MAX")/100)); for (uint8_t i=1; i<=v->_num_motors; i++) { if (v->_servo_output[i] > v->is_flying_motor_threshold) { reason.append(string_format("SERVO_OUTPUT_RAW.servo%d_raw=%f", diff --git a/analyzer_crashed.h b/analyzer_crashed.h index 5f20a6d..a93c6ef 100644 --- a/analyzer_crashed.h +++ b/analyzer_crashed.h @@ -32,8 +32,6 @@ class Analyzer_Crashed : public Analyzer { void handle_decoded_message(uint64_t T, mavlink_attitude_t &att); void handle_decoded_message(uint64_t T, mavlink_servo_output_raw_t &servos); - bool is_zero(double x) { return x < 0.00001; } // FIXME - private: bool has_crashed(); void evaluate(uint64_t T); diff --git a/analyzer_ever_armed.cpp b/analyzer_ever_armed.cpp index abbb282..35cb81c 100644 --- a/analyzer_ever_armed.cpp +++ b/analyzer_ever_armed.cpp @@ -1,8 +1,5 @@ #include "analyzer_ever_armed.h" -#include -#include - #include "util.h" void Analyzer_Ever_Armed::handle_decoded_message(uint64_t T, mavlink_heartbeat_t &heartbeat) diff --git a/analyzer_ever_flew.cpp b/analyzer_ever_flew.cpp index 8b5d118..34d5828 100644 --- a/analyzer_ever_flew.cpp +++ b/analyzer_ever_flew.cpp @@ -1,8 +1,5 @@ #include "analyzer_ever_flew.h" -#include -#include - #include "util.h" void Analyzer_Ever_Flew::evaluate(uint64_t T) diff --git a/analyzer_good_ekf.cpp b/analyzer_good_ekf.cpp index 77afa1f..da23a26 100644 --- a/analyzer_good_ekf.cpp +++ b/analyzer_good_ekf.cpp @@ -1,8 +1,5 @@ #include "analyzer_good_ekf.h" -#include -#include -#include // for fork() #include "util.h" #include "analyzer_util.h" diff --git a/analyzer_util.cpp b/analyzer_util.cpp new file mode 100644 index 0000000..aef3877 --- /dev/null +++ b/analyzer_util.cpp @@ -0,0 +1,9 @@ +#include "analyzer_util.h" + +void format_timestamp(char *buf, const uint8_t buflen, const uint64_t T) +{ + struct tm *tmp; + time_t t = T / 1000000; + tmp = localtime(&t); + ::strftime(buf, buflen, "%Y%m%d%H%M%S", tmp); +} diff --git a/analyzer_util.h b/analyzer_util.h index 3aa8494..7dfb67c 100644 --- a/analyzer_util.h +++ b/analyzer_util.h @@ -1,6 +1,8 @@ #ifndef _ANALYZER_UTIL #define _ANALYZER_UTIL +#include + // from: http://stackoverflow.com/questions/2342162/stdstring-formatting-like-sprintf #include template @@ -22,8 +24,11 @@ std::string string_format( const char* format, Args ... args ) // return rad*180/M_PI; // } -#define deg_to_rad(x) (x/M_PI * 180) -#define rad_to_deg(x) (x/180 * M_PI) +#define deg_to_rad(x) (x/180*M_PI) +#define rad_to_deg(x) (x/M_PI * 180) + +#define is_zero(x) (x < 0.00001) +void format_timestamp(char *buf, uint8_t buflen, uint64_t T); #endif diff --git a/analyzervehicle.cpp b/analyzervehicle.cpp index 472853c..e268545 100644 --- a/analyzervehicle.cpp +++ b/analyzervehicle.cpp @@ -2,32 +2,7 @@ using namespace AnalyzerVehicle; -void Base::handle_decoded_message(uint64_t T, mavlink_statustext_t &msg) -{ - history_statustext.packet(msg); -} - -void Base::handle_decoded_message(uint64_t T, mavlink_heartbeat_t &msg) -{ - history_heartbeat.packet(msg); - _armed = msg.base_mode & MAV_MODE_FLAG_SAFETY_ARMED; -} - -void Base::handle_decoded_message(uint64_t T, mavlink_servo_output_raw_t &msg) -{ - history_servo_output_raw.packet(msg); -} - -char * -xstrdup(const char *x) -{ - char *ret = strdup(x); - if (ret == NULL) { - fprintf(stderr, "Failed to strdup: %s", strerror(errno)); - abort(); - } - return ret; -} +#include "analyzer_util.h" char * xcalloc(size_t size) @@ -40,8 +15,17 @@ xcalloc(size_t size) return ret; } - - +void Base::handle_decoded_message(uint64_t T, mavlink_attitude_t &msg) +{ + att().set_roll(T, rad_to_deg(msg.roll)); + att().set_pitch(T, rad_to_deg(msg.pitch)); + att().set_yaw(T, rad_to_deg(msg.yaw)); +} +void Base::handle_decoded_message(uint64_t T, mavlink_heartbeat_t &msg) +{ + history_heartbeat.packet(msg); + _armed = msg.base_mode & MAV_MODE_FLAG_SAFETY_ARMED; +} void Base::handle_decoded_message(uint64_t T, mavlink_param_value_t &msg) { // FIXME: getting the same parameter multiple times leaks memory char *str = xcalloc(17); // FIXME constant @@ -49,13 +33,23 @@ void Base::handle_decoded_message(uint64_t T, mavlink_param_value_t &msg) { _param[str] = msg.param_value; _param_modtime[str] = T; } - -void Base::handle_decoded_message(uint64_t T, mavlink_attitude_t &msg) +void Base::handle_decoded_message(uint64_t T, mavlink_nav_controller_output_t &msg) +{ + history_nav_controller_output.packet(msg); + nav().set_desroll(T, msg.nav_roll); + nav().set_despitch(T, msg.nav_pitch); + nav().set_desyaw(T, msg.nav_bearing); +} +void Base::handle_decoded_message(uint64_t T, mavlink_servo_output_raw_t &msg) { - att().set_roll(T, msg.roll); - att().set_pitch(T, msg.pitch); - att().set_yaw(T, msg.yaw); + history_servo_output_raw.packet(msg); } +void Base::handle_decoded_message(uint64_t T, mavlink_statustext_t &msg) +{ + history_statustext.packet(msg); +} + + bool Base::param_seen(const char *name) const { diff --git a/analyzervehicle.h b/analyzervehicle.h index 7a313e2..3fa1d26 100644 --- a/analyzervehicle.h +++ b/analyzervehicle.h @@ -14,30 +14,41 @@ namespace AnalyzerVehicle { // example class AV_Attitude { public: - float roll() { return _roll; }; - float pitch() { return _pitch; }; - float yaw() { return _yaw; }; + float roll() const { return _att[0]; }; + float pitch() const { return _att[1]; }; + float yaw() const { return _att[2]; }; + uint64_t roll_modtime() const { + return get_att_modtime(0); + } + uint64_t pitch_modtime() const { + return get_att_modtime(1); + } + uint64_t yaw_modtime() const { + return get_att_modtime(2); + } void set_roll(uint64_t T, float roll) { - _roll = roll; - _roll_modtime = T; + _set_att_attrib(0, T, roll); } void set_pitch(uint64_t T, float pitch) { - _pitch = pitch; - _pitch_modtime = T; + _set_att_attrib(1, T, pitch); } void set_yaw(uint64_t T, float yaw) { - _yaw = yaw; - _yaw_modtime = T; + _set_att_attrib(2, T, yaw); } private: - float _roll; - float _pitch; - float _yaw; - uint64_t _roll_modtime; - uint64_t _pitch_modtime; - uint64_t _yaw_modtime; - }; + // all in degrees: + float _att[3]; + uint64_t _att_modtime[3]; + + void _set_att_attrib(uint8_t offset, uint64_t T, float value) { + _att[offset] = value; + _att_modtime[offset] = T; + } + uint64_t get_att_modtime(uint8_t offset) const { + return _att_modtime[offset]; + } +}; class AV_Position { public: @@ -62,6 +73,32 @@ namespace AnalyzerVehicle { uint16_t D; }; + class AV_Nav { + public: + void set_desroll(uint64_t T, float roll) { + _des[0] = roll; + _modtimes[0] = T; + } + float desroll() { return _des[0]; } + + void set_despitch(uint64_t T, float pitch) { + _des[1] = pitch; + _modtimes[1] = T; + } + float despitch() { return _des[1]; } + + void set_desyaw(uint64_t T, float yaw) { + _des[1] = yaw; + _modtimes[2] = T; + } + float desyaw() { return _des[2]; } + + private: + float _des[3]; + uint64_t _modtimes[3]; + }; + + template class PacketHistory { public: @@ -103,11 +140,13 @@ class Base { virtual void handle_decoded_message(uint64_t T, mavlink_attitude_t&); virtual void handle_decoded_message(uint64_t T, mavlink_heartbeat_t&); + 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_statustext_t&); virtual void handle_decoded_message(uint64_t T, mavlink_servo_output_raw_t &); AV_Attitude& att() { return _att; }; AV_Position& pos() { return _pos; }; + AV_Nav& nav() { return _nav; }; protected: bool _armed; @@ -116,10 +155,12 @@ class Base { std::map _param_modtime; AV_Attitude _att; AV_Position _pos; + AV_Nav _nav; private: PacketHistory history_heartbeat; + PacketHistory history_nav_controller_output; PacketHistory history_servo_output_raw; PacketHistory history_statustext; diff --git a/analyzervehicle_copter.cpp b/analyzervehicle_copter.cpp index 3a27c2a..7b4b53d 100644 --- a/analyzervehicle_copter.cpp +++ b/analyzervehicle_copter.cpp @@ -40,12 +40,11 @@ bool Copter::exceeding_angle_max() } float angle_max = param("ANGLE_MAX") / 100; // convert from centidegrees - float angle_max_radians = angle_max/180.0 * M_PI; - if (att().roll() > angle_max_radians) { + if (att().roll() > angle_max) { return true; } - if (att().pitch() > angle_max_radians) { + if (att().pitch() > angle_max) { return true; } return false; diff --git a/mavlink_message_handler.h b/mavlink_message_handler.h index 3afeab4..9069449 100644 --- a/mavlink_message_handler.h +++ b/mavlink_message_handler.h @@ -48,6 +48,7 @@ class 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_sys_status_t &msg) { } diff --git a/mavlink_reader.cpp b/mavlink_reader.cpp index 39914aa..cbddbd5 100644 --- a/mavlink_reader.cpp +++ b/mavlink_reader.cpp @@ -208,6 +208,12 @@ void MAVLink_Reader::handle_message_received(uint64_t timestamp, mavlink_message handle_decoded_message_received(timestamp, decoded); // template in .h break; } + case MAVLINK_MSG_ID_NAV_CONTROLLER_OUTPUT: { + mavlink_nav_controller_output_t decoded; + mavlink_msg_nav_controller_output_decode(&msg, &decoded); + handle_decoded_message_received(timestamp, decoded); // template in .h + break; + } case MAVLINK_MSG_ID_PARAM_VALUE: { mavlink_param_value_t decoded; mavlink_msg_param_value_decode(&msg, &decoded); From 9b95d5a50289bbbc7d05d4cce02c1b68d724bf33 Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Thu, 27 Aug 2015 20:07:39 +1000 Subject: [PATCH 4/6] la: rename analyzer_crashed to analyzer_notcrashed --- analyzer_crashed.cpp => analyzer_notcrashed.cpp | 0 analyzer_crashed.h => analyzer_notcrashed.h | 0 2 files changed, 0 insertions(+), 0 deletions(-) rename analyzer_crashed.cpp => analyzer_notcrashed.cpp (100%) rename analyzer_crashed.h => analyzer_notcrashed.h (100%) diff --git a/analyzer_crashed.cpp b/analyzer_notcrashed.cpp similarity index 100% rename from analyzer_crashed.cpp rename to analyzer_notcrashed.cpp diff --git a/analyzer_crashed.h b/analyzer_notcrashed.h similarity index 100% rename from analyzer_crashed.h rename to analyzer_notcrashed.h From d8d195708c0ad315356a8236a2286759ae54741e Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Fri, 28 Aug 2015 08:27:51 +1000 Subject: [PATCH 5/6] la: rejig renamed crash analyzer to have a list of crashes --- Makefile | 2 +- analyze.cpp | 10 +-- analyzer.h | 32 ++++++-- analyzer_attitude_control.cpp | 18 +++-- analyzer_attitude_control.h | 19 ----- analyzer_notcrashed.cpp | 136 ++++++++++++++++++++++++---------- analyzer_notcrashed.h | 43 ++++++----- 7 files changed, 165 insertions(+), 95 deletions(-) diff --git a/Makefile b/Makefile index 41b5205..19eeab1 100644 --- a/Makefile +++ b/Makefile @@ -35,7 +35,7 @@ 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 diff --git a/analyze.cpp b/analyze.cpp index b6120b0..5dae524 100644 --- a/analyze.cpp +++ b/analyze.cpp @@ -10,7 +10,7 @@ #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) { @@ -70,11 +70,11 @@ void Analyze::instantiate_analyzers(INIReader *config) } - Analyzer_Crashed *analyzer_crashed = new Analyzer_Crashed(_fd_telem_forwarder, _sa_telemetry_forwarder, vehicle); - 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"); } } diff --git a/analyzer.h b/analyzer.h index 3d358ef..4202be6 100644 --- a/analyzer.h +++ b/analyzer.h @@ -8,15 +8,35 @@ #include "analyzervehicle.h" -class Analyzer : public MAVLink_Message_Handler { +enum analyzer_status { + analyzer_status_warn = 17, + analyzer_status_fail, + analyzer_status_ok, +}; +class analyzer_result { public: - enum analyzer_status { - analyzer_status_warn = 17, - analyzer_status_fail, - analyzer_status_ok, - }; + 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, AnalyzerVehicle::Base *&vehicle) : MAVLink_Message_Handler(fd, sa), evilness(0), diff --git a/analyzer_attitude_control.cpp b/analyzer_attitude_control.cpp index 8199d6e..ba9aee9 100644 --- a/analyzer_attitude_control.cpp +++ b/analyzer_attitude_control.cpp @@ -61,6 +61,7 @@ void Analyzer_Attitude_Control::evaluate(uint64_t T) attitude_control_result &result = attitude_control_results[attitude_control_results_offset]; result.status = status(); result.timestamp_start = T; + result.timestamp_stop = 0; result.deltamax = delta; result.roll_at_deltamax = roll; result.desroll_at_deltamax = desroll; @@ -112,6 +113,16 @@ void Analyzer_Attitude_Control::results_json_results(Json::Value &root) result["timestamp_stop"] = (Json::UInt64)x.timestamp_stop; } + // current structure makes this stuff awfully magical: + // tomorrow this should change because we'll be updated from + // df logs.... + Json::Value series(Json::arrayValue); + series.append("NAV_CONTROLLER_OUTPUT.nav_roll"); + series.append("NAV_CONTROLLER_OUTPUT.nav_pitch"); + series.append("ATTITUDE.roll"); + series.append("ATTITUDE.pitch"); + result["series"] = series; + root.append(result); } } else { @@ -125,11 +136,8 @@ void Analyzer_Attitude_Control::results_json_results(Json::Value &root) add_evilness(this_sin_score); result["reason"] = reason; Json::Value series(Json::arrayValue); - series.append("VFR_HUD.alt"); - series.append("SERVO_OUTPUT_RAW.servo1_raw"); - series.append("SERVO_OUTPUT_RAW.servo2_raw"); - series.append("SERVO_OUTPUT_RAW.servo3_raw"); - series.append("SERVO_OUTPUT_RAW.servo4_raw"); + series.append("ATTITUDE.roll"); + series.append("ATTITUDE.pitch"); result["series"] = series; root.append(result); } diff --git a/analyzer_attitude_control.h b/analyzer_attitude_control.h index 8d9dc8e..9b12a33 100644 --- a/analyzer_attitude_control.h +++ b/analyzer_attitude_control.h @@ -37,25 +37,6 @@ class Analyzer_Attitude_Control : public Analyzer { const float offset_warn; const float offset_fail; - 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 attitude_control_result : public analyzer_result { public: uint64_t timestamp_start; diff --git a/analyzer_notcrashed.cpp b/analyzer_notcrashed.cpp index 45f2ef8..2c68233 100644 --- a/analyzer_notcrashed.cpp +++ b/analyzer_notcrashed.cpp @@ -1,4 +1,4 @@ -#include "analyzer_crashed.h" +#include "analyzer_notcrashed.h" #include "util.h" @@ -6,17 +6,17 @@ #include "analyzervehicle_copter.h" -void Analyzer_Crashed::handle_decoded_message(uint64_t T, mavlink_param_value_t ¶m) +void Analyzer_NotCrashed::handle_decoded_message(uint64_t T, mavlink_param_value_t ¶m) { evaluate(T); } -void Analyzer_Crashed::handle_decoded_message(uint64_t T, mavlink_attitude_t &msg) +void Analyzer_NotCrashed::handle_decoded_message(uint64_t T, mavlink_attitude_t &msg) { Analyzer::handle_decoded_message(T, msg); evaluate(T); seen_packets_attitude = true; } -void Analyzer_Crashed::handle_decoded_message(uint64_t T, mavlink_servo_output_raw_t &servos) +void Analyzer_NotCrashed::handle_decoded_message(uint64_t T, mavlink_servo_output_raw_t &servos) { Analyzer::handle_decoded_message(T, servos); evaluate(T); @@ -24,60 +24,114 @@ void Analyzer_Crashed::handle_decoded_message(uint64_t T, mavlink_servo_output_r -void Analyzer_Crashed::evaluate(uint64_t T) +void Analyzer_NotCrashed::end_of_log(const uint32_t packet_count) +{ + if (status() != analyzer_status_ok) { + // close off the final result + notcrashed_results_offset++; + } +} + +void Analyzer_NotCrashed::evaluate(uint64_t T) { AnalyzerVehicle::Copter *v = (AnalyzerVehicle::Copter*&)_vehicle; + analyzer_status _status_prev = status(); if (v->exceeding_angle_max() && v->any_motor_running_fast()) { - crashed = true; - crashed_timestamp = T; - crashed_angle = (v->att().roll() > v->att().pitch()) ? v->att().roll() : v->att().pitch(); - for (uint8_t i=1; i<=v->_num_motors; i++) { - crash_servo_output[i] = v->_servo_output[i]; + set_status(analyzer_status_fail); + } else { + set_status(analyzer_status_ok); + } + + + if (status() == _status_prev) { + // nothing... yet... + } else { + switch(_status_prev) { + case analyzer_status_ok: + break; + case analyzer_status_warn: + case analyzer_status_fail: + if (status() != analyzer_status_ok) { + // only "crash" if this state persists for >1second: + if (T - notcrashed_results[notcrashed_results_offset].timestamp_start > 1000000) { + // accept this result + notcrashed_results[notcrashed_results_offset].timestamp_stop = T; + notcrashed_results_offset++; + } + } } + if (status() != analyzer_status_ok) { + notcrashed_result &result = notcrashed_results[notcrashed_results_offset]; + result.status = status(); + result.timestamp_start = T; + result.timestamp_stop = 0; + result.angle = (v->att().roll() > v->att().pitch()) ? v->att().roll() : v->att().pitch(); + result.angle_max = v->param("ANGLE_MAX")/100; + for (uint8_t i=1; i<=v->_num_motors; i++) { + result.servo_output[i] = v->_servo_output[i]; + } + } + _status_prev = status(); } } -void Analyzer_Crashed::results_json_results(Json::Value &root) +void Analyzer_NotCrashed::add_series(Json::Value &root) +{ + Json::Value series(Json::arrayValue); + series.append("PARAM"); + series.append("SERVO_OUTPUT_RAW.servo1_raw"); + series.append("SERVO_OUTPUT_RAW.servo2_raw"); + series.append("SERVO_OUTPUT_RAW.servo3_raw"); + series.append("SERVO_OUTPUT_RAW.servo4_raw"); + root["series"] = series; +} + +void Analyzer_NotCrashed::results_json_results(Json::Value &root) { - Json::Value result(Json::objectValue); - - Json::Value reason(Json::arrayValue); - uint8_t this_sin_score = 0; AnalyzerVehicle::Copter *v = (AnalyzerVehicle::Copter*&)_vehicle; - if (crashed) { - reason.append("Crashed"); - reason.append(string_format("ANGLE_MAX (%f > %f)", crashed_angle, v->param("ANGLE_MAX")/100)); + for (uint8_t i=0; i %f)", x.angle, x.angle_max)); for (uint8_t i=1; i<=v->_num_motors; i++) { if (v->_servo_output[i] > v->is_flying_motor_threshold) { - reason.append(string_format("SERVO_OUTPUT_RAW.servo%d_raw=%f", - i, crash_servo_output[i])); + evidence.append(string_format("SERVO_OUTPUT_RAW.servo%d_raw=%f", + i, x.servo_output[i])); } } - this_sin_score = 10; - Json::Value series(Json::arrayValue); - series.append("PARAM"); - series.append("SERVO_OUTPUT_RAW.servo1_raw"); - series.append("SERVO_OUTPUT_RAW.servo2_raw"); - series.append("SERVO_OUTPUT_RAW.servo3_raw"); - series.append("SERVO_OUTPUT_RAW.servo4_raw"); - result["series"] = series; - result["timestamp"] = (Json::UInt64)crashed_timestamp; - result["status"] = "FAIL"; - result["evilness"] = this_sin_score; - } else if (!seen_packets_attitude) { - reason.append("No attitude packets seen"); - result["status"] = "WARN"; - } else { - reason.append("Never Crashed"); - result["status"] = "OK"; - } - if (this_sin_score) { + result["evidence"] = evidence; + + result["timestamp_start"] = (Json::UInt64)x.timestamp_start; + if (x.timestamp_stop != 0) { + result["timestamp_stop"] = (Json::UInt64)x.timestamp_stop; + } + + add_series(result); + + const uint8_t this_sin_score = 10; result["evilness"] = this_sin_score; add_evilness(this_sin_score); + root.append(result); + } + + if (notcrashed_results_offset == 0) { + Json::Value result(Json::objectValue); + if (_vehicle->att().roll_modtime() == 0) { + result["reason"] = "Vehicle's attitude never updated"; + result["status"] = "WARN"; + } else { + result["reason"] = "Never Crashed"; + result["status"] = "OK"; + } + add_series(result); + root.append(result); } - result["reason"] = reason; - root.append(result); } diff --git a/analyzer_notcrashed.h b/analyzer_notcrashed.h index a93c6ef..81fe5d4 100644 --- a/analyzer_notcrashed.h +++ b/analyzer_notcrashed.h @@ -1,8 +1,8 @@ -#ifndef ANALYZE_CRASHED_H -#define ANALYZE_CRASHED_H +#ifndef ANALYZE_NOTCRASHED_H +#define ANALYZE_NOTCRASHED_H /* - * analyze_crashed + * analyze_notcrashed * */ @@ -10,44 +10,51 @@ #define angle_max_default_degrees 20 -class Analyzer_Crashed : public Analyzer { +class Analyzer_NotCrashed : public Analyzer { public: - Analyzer_Crashed(int fd, struct sockaddr_in &sa, AnalyzerVehicle::Base *&vehicle) : + Analyzer_NotCrashed(int fd, struct sockaddr_in &sa, AnalyzerVehicle::Base *&vehicle) : Analyzer(fd, sa, vehicle), seen_packets_attitude(false), - crashed(false), - // angle_max(angle_max_default_degrees/180 * M_PI), - crash_servo_output{ 0 } + notcrashed_results_offset(0) { } const uint16_t servo_output_threshold = 1250; const char *name() { return "Crash Test"; } const char *description() { - return "The vehicle crashed"; + return "The Vehicle Did Not Crash"; } + void end_of_log(const uint32_t packet_count); + void handle_decoded_message(uint64_t T, mavlink_param_value_t ¶m) override; void handle_decoded_message(uint64_t T, mavlink_attitude_t &att); void handle_decoded_message(uint64_t T, mavlink_servo_output_raw_t &servos); + class notcrashed_result : public analyzer_result { + public: + uint64_t timestamp_start; + uint64_t timestamp_stop; + float angle; + float angle_max; + double servo_output[17]; // FIXME take 17 from somewhere... + uint64_t duration() { return (timestamp_stop - timestamp_start); } + }; + #define MAX_NOTCRASHED_RESULTS 100 + uint8_t notcrashed_results_offset; + notcrashed_result notcrashed_results[MAX_NOTCRASHED_RESULTS]; + bool notcrashed_results_overrun; + private: - bool has_crashed(); void evaluate(uint64_t T); + void add_series(Json::Value &root); + bool seen_packets_attitude; - bool crashed; - float crashed_angle; - - bool exceeded_angle_max; - uint64_t crashed_timestamp; - uint64_t exceeded_angle_max_timestamp; // const double angle_max_default_degrees = 20.0f; - double crash_servo_output[17]; // FIXME take 17 from somewhere... - void results_json_results(Json::Value &root); }; From 3934c9c7580cec3776cd0eb21874035696c8baac Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Fri, 28 Aug 2015 10:06:43 +1000 Subject: [PATCH 6/6] 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) {