From d9c8af54bd0471d7f6d2dfe03625f19050c3c124 Mon Sep 17 00:00:00 2001 From: Daniel Agar Date: Thu, 16 Nov 2017 13:36:03 -0500 Subject: [PATCH] EKF add print_status() with memory usage --- EKF/RingBuffer.h | 2 ++ EKF/estimator_interface.cpp | 17 +++++++++++++++++ EKF/estimator_interface.h | 2 ++ 3 files changed, 21 insertions(+) diff --git a/EKF/RingBuffer.h b/EKF/RingBuffer.h index 5ee39cf05b..0529aa16c6 100644 --- a/EKF/RingBuffer.h +++ b/EKF/RingBuffer.h @@ -155,6 +155,8 @@ public: return false; } + int get_total_size() { return sizeof(*this) + sizeof(data_type) * _size; } + private: data_type *_buffer{nullptr}; diff --git a/EKF/estimator_interface.cpp b/EKF/estimator_interface.cpp index 79cd8615b3..9f143c6627 100644 --- a/EKF/estimator_interface.cpp +++ b/EKF/estimator_interface.cpp @@ -519,3 +519,20 @@ bool EstimatorInterface::local_position_is_valid() // return true if we are not doing unconstrained free inertial navigation return !inertial_dead_reckoning(); } + +void EstimatorInterface::print_status() { + ECL_INFO("local position valid: %s", local_position_is_valid() ? "yes" : "no"); + ECL_INFO("global position valid: %s", global_position_is_valid() ? "yes" : "no"); + + ECL_INFO("imu buffer: %d (%d Bytes)", _imu_buffer.get_length(), _imu_buffer.get_total_size()); + ECL_INFO("gps buffer: %d (%d Bytes)", _gps_buffer.get_length(), _gps_buffer.get_total_size()); + ECL_INFO("mag buffer: %d (%d Bytes)", _mag_buffer.get_length(), _mag_buffer.get_total_size()); + ECL_INFO("baro buffer: %d (%d Bytes)", _baro_buffer.get_length(), _baro_buffer.get_total_size()); + ECL_INFO("range buffer: %d (%d Bytes)", _range_buffer.get_length(), _range_buffer.get_total_size()); + ECL_INFO("airspeed buffer: %d (%d Bytes)", _airspeed_buffer.get_length(), _airspeed_buffer.get_total_size()); + ECL_INFO("flow buffer: %d (%d Bytes)", _flow_buffer.get_length(), _flow_buffer.get_total_size()); + ECL_INFO("ext vision buffer: %d (%d Bytes)", _ext_vision_buffer.get_length(), _ext_vision_buffer.get_total_size()); + ECL_INFO("output buffer: %d (%d Bytes)", _output_buffer.get_length(), _output_buffer.get_total_size()); + ECL_INFO("output vert buffer: %d (%d Bytes)", _output_vert_buffer.get_length(), _output_vert_buffer.get_total_size()); + ECL_INFO("drag buffer: %d (%d Bytes)", _drag_buffer.get_length(), _drag_buffer.get_total_size()); +} diff --git a/EKF/estimator_interface.h b/EKF/estimator_interface.h index 4630230b10..5f9fb91c76 100644 --- a/EKF/estimator_interface.h +++ b/EKF/estimator_interface.h @@ -352,6 +352,8 @@ public: return _imu_updated; } + void print_status(); + static const unsigned FILTER_UPDATE_PERIOD_MS = 8; // ekf prediction period in milliseconds - this should ideally be an integer multiple of the IMU time delta protected: