diff --git a/src/lib/matrix/matrix/Matrix.hpp b/src/lib/matrix/matrix/Matrix.hpp index fbf5c50188..f6c1981ba1 100644 --- a/src/lib/matrix/matrix/Matrix.hpp +++ b/src/lib/matrix/matrix/Matrix.hpp @@ -365,13 +365,55 @@ public: } } - void print() const + void print(float eps = 0.00001f) const { - // element: tab, point, 8 digits, 4 scientific notation chars; row: newline; string: \0 end - static const size_t n = 15 * N * M + M + 1; - char string[n]; - write_string(string, n); - printf("%s\n", string); + // print column numbering + if (N > 1) { + printf(" "); + + for (unsigned i = 0; i < N; i++) { + printf("|%2u ", i); + + } + + printf("\n"); + } + + const Matrix &self = *this; + + for (unsigned i = 0; i < M; i++) { + printf("%2u|", i); // print row numbering + + for (unsigned j = 0; j < N; j++) { + double d = static_cast(self(i, j)); + + // Matrix diagonal elements + if (N > 1 && M > 1 && i == j) { + // make diagonal elements bold (ANSI CSI n 1) + printf("\033[1m"); + } + + // avoid -0.0 for display + if (fabs(d - 0.0) < (double)eps) { + // print fixed width zero + printf(" 0 "); + + } else if ((fabs(d) < 1e-4) || (fabs(d) >= 10.0)) { + printf("% .1e ", d); + + } else { + printf("% 6.5f ", d); + } + + // Matrix diagonal elements + if (N > 1 && M > 1 && i == j) { + // reset any formatting (ANSI CSI n 0) + printf("\033[0m"); + } + } + + printf("\n"); + } } Matrix transpose() const diff --git a/src/lib/matrix/test/MatrixAssignmentTest.cpp b/src/lib/matrix/test/MatrixAssignmentTest.cpp index 29b7f9951d..0500f8734f 100644 --- a/src/lib/matrix/test/MatrixAssignmentTest.cpp +++ b/src/lib/matrix/test/MatrixAssignmentTest.cpp @@ -276,14 +276,20 @@ TEST(MatrixAssignmentTest, Assignment) } } + char print_out[] = " | 0 | 1 \n 0| 1.00000 1.2e+04\n 1| 1.2e+04 0.12346\n 2| 1.2e+10 1.2e+12\n"; + printf("%s\n", print_out); // for debugging in case of failure + // check print() // Redirect stdout - EXPECT_TRUE(freopen("testoutput.txt", "w", stdout) != NULL); + FILE *fp = freopen("testoutput.txt", "w", stdout); + EXPECT_NE(fp, nullptr); + // write Comma.print(); - fclose(stdout); + EXPECT_FALSE(fclose(fp)); // FIXME: this doesn't work as expected, further printf are not redirected to the console + // read - FILE *fp = fopen("testoutput.txt", "r"); + fp = fopen("testoutput.txt", "r"); EXPECT_NE(fp, nullptr); EXPECT_FALSE(fseek(fp, 0, SEEK_SET)); @@ -294,8 +300,8 @@ TEST(MatrixAssignmentTest, Assignment) break; } - printf("%d %d %d\n", static_cast(i), output[i], c); - EXPECT_EQ(c, output[i]); + printf("%d %d %d\n", static_cast(i), print_out[i], c); + EXPECT_EQ(c, print_out[i]); } EXPECT_FALSE(fclose(fp)); diff --git a/src/modules/ekf2/EKF/RingBuffer.h b/src/modules/ekf2/EKF/RingBuffer.h index aeb4471ad5..2e4ff02bb3 100644 --- a/src/modules/ekf2/EKF/RingBuffer.h +++ b/src/modules/ekf2/EKF/RingBuffer.h @@ -153,6 +153,7 @@ public: return false; } + int get_used_size() const { return sizeof(*this) + sizeof(data_type) * entries(); } int get_total_size() const { return sizeof(*this) + sizeof(data_type) * _size; } int entries() const diff --git a/src/modules/ekf2/EKF/ekf.cpp b/src/modules/ekf2/EKF/ekf.cpp index f3330fed8c..58a09f4cad 100644 --- a/src/modules/ekf2/EKF/ekf.cpp +++ b/src/modules/ekf2/EKF/ekf.cpp @@ -347,3 +347,96 @@ void Ekf::updateParameters() _aux_global_position.updateParameters(); #endif // CONFIG_EKF2_AUX_GLOBAL_POSITION } + +template +static void printRingBuffer(const char *name, RingBuffer *rb) +{ + if (rb) { + printf("%s: %d/%d entries (%d/%d Bytes) (%zu Bytes per entry)\n", + name, + rb->entries(), rb->get_length(), rb->get_used_size(), rb->get_total_size(), + sizeof(T)); + } +} + +void Ekf::print_status() +{ + printf("\nStates: (%.4f seconds ago)\n", (_time_latest_us - _time_delayed_us) * 1e-6); + printf("Orientation: [%.4f, %.4f, %.4f, %.4f] (Euler [%.3f, %.3f, %.3f])\n", + (double)_state.quat_nominal(0), (double)_state.quat_nominal(1), (double)_state.quat_nominal(2), + (double)_state.quat_nominal(3), + (double)matrix::Eulerf(_state.quat_nominal).phi(), (double)matrix::Eulerf(_state.quat_nominal).theta(), + (double)matrix::Eulerf(_state.quat_nominal).psi()); + + printf("Velocity: [%.3f, %.3f, %.3f]\n", + (double)_state.vel(0), (double)_state.vel(1), (double)_state.vel(2)); + + printf("Position: [%.3f, %.3f, %.3f]\n", + (double)_state.pos(0), (double)_state.pos(1), (double)_state.pos(2)); + + printf("Gyro Bias: [%.6f, %.6f, %.6f]\n", + (double)_state.gyro_bias(0), (double)_state.gyro_bias(1), (double)_state.gyro_bias(2)); + + printf("Accel Bias: [%.6f, %.6f, %.6f]\n", + (double)_state.accel_bias(0), (double)_state.accel_bias(1), (double)_state.accel_bias(2)); + +#if defined(CONFIG_EKF2_MAGNETOMETER) + printf("Magnetic Field: [%.3f, %.3f, %.3f]\n", + (double)_state.mag_I(0), (double)_state.mag_I(1), (double)_state.mag_I(2)); + + printf("Magnetic Bias: [%.3f, %.3f, %.3f]\n", + (double)_state.mag_B(0), (double)_state.mag_B(1), (double)_state.mag_B(2)); +#endif // CONFIG_EKF2_MAGNETOMETER + +#if defined(CONFIG_EKF2_WIND) + printf("Wind velocity: [%.3f, %.3f]\n", (double)_state.wind_vel(0), (double)_state.wind_vel(1)); +#endif // CONFIG_EKF2_WIND + + printf("\nP:\n"); + P.print(); + + printf("EKF average dt: %.6f seconds\n", (double)_dt_ekf_avg); + printf("minimum observation interval %d us\n", _min_obs_interval_us); + + printRingBuffer("IMU buffer", &_imu_buffer); + printRingBuffer("system flag buffer", _system_flag_buffer); + +#if defined(CONFIG_EKF2_AIRSPEED) + printRingBuffer("airspeed buffer", _airspeed_buffer); +#endif // CONFIG_EKF2_AIRSPEED + +#if defined(CONFIG_EKF2_AUXVEL) + printRingBuffer("aux vel buffer", _auxvel_buffer); +#endif // CONFIG_EKF2_AUXVEL + +#if defined(CONFIG_EKF2_BAROMETER) + printRingBuffer("baro buffer", _baro_buffer); +#endif // CONFIG_EKF2_BAROMETER + +#if defined(CONFIG_EKF2_DRAG_FUSION) + printRingBuffer("drag buffer", _drag_buffer); +#endif // CONFIG_EKF2_DRAG_FUSION + +#if defined(CONFIG_EKF2_EXTERNAL_VISION) + printRingBuffer("ext vision buffer", _ext_vision_buffer); +#endif // CONFIG_EKF2_EXTERNAL_VISION + +#if defined(CONFIG_EKF2_GNSS) + printRingBuffer("gps buffer", _gps_buffer); +#endif // CONFIG_EKF2_GNSS + +#if defined(CONFIG_EKF2_MAGNETOMETER) + printRingBuffer("mag buffer", _mag_buffer); +#endif // CONFIG_EKF2_MAGNETOMETER + +#if defined(CONFIG_EKF2_OPTICAL_FLOW) + printRingBuffer("flow buffer", _flow_buffer); +#endif // CONFIG_EKF2_OPTICAL_FLOW + +#if defined(CONFIG_EKF2_RANGE_FINDER) + printRingBuffer("range buffer", _range_buffer); +#endif // CONFIG_EKF2_RANGE_FINDER + + + _output_predictor.print_status(); +} diff --git a/src/modules/ekf2/EKF/ekf.h b/src/modules/ekf2/EKF/ekf.h index cff0426ada..e816c50b17 100644 --- a/src/modules/ekf2/EKF/ekf.h +++ b/src/modules/ekf2/EKF/ekf.h @@ -85,6 +85,8 @@ public: // initialise variables to sane values (also interface class) bool init(uint64_t timestamp) override; + void print_status(); + // should be called every time new data is pushed into the filter bool update(); diff --git a/src/modules/ekf2/EKF/estimator_interface.cpp b/src/modules/ekf2/EKF/estimator_interface.cpp index a8a53f5a29..0b384bfb56 100644 --- a/src/modules/ekf2/EKF/estimator_interface.cpp +++ b/src/modules/ekf2/EKF/estimator_interface.cpp @@ -676,62 +676,3 @@ void EstimatorInterface::printBufferAllocationFailed(const char *buffer_name) ECL_ERR("%s buffer allocation failed", buffer_name); } } - -void EstimatorInterface::print_status() -{ - printf("EKF average dt: %.6f seconds\n", (double)_dt_ekf_avg); - - printf("IMU buffer: %d (%d Bytes)\n", _imu_buffer.get_length(), _imu_buffer.get_total_size()); - - printf("minimum observation interval %d us\n", _min_obs_interval_us); - -#if defined(CONFIG_EKF2_GNSS) - if (_gps_buffer) { - printf("gps buffer: %d/%d (%d Bytes)\n", _gps_buffer->entries(), _gps_buffer->get_length(), _gps_buffer->get_total_size()); - } -#endif // CONFIG_EKF2_GNSS - -#if defined(CONFIG_EKF2_MAGNETOMETER) - if (_mag_buffer) { - printf("mag buffer: %d/%d (%d Bytes)\n", _mag_buffer->entries(), _mag_buffer->get_length(), _mag_buffer->get_total_size()); - } -#endif // CONFIG_EKF2_MAGNETOMETER - -#if defined(CONFIG_EKF2_BAROMETER) - if (_baro_buffer) { - printf("baro buffer: %d/%d (%d Bytes)\n", _baro_buffer->entries(), _baro_buffer->get_length(), _baro_buffer->get_total_size()); - } -#endif // CONFIG_EKF2_BAROMETER - -#if defined(CONFIG_EKF2_RANGE_FINDER) - if (_range_buffer) { - printf("range buffer: %d/%d (%d Bytes)\n", _range_buffer->entries(), _range_buffer->get_length(), _range_buffer->get_total_size()); - } -#endif // CONFIG_EKF2_RANGE_FINDER - -#if defined(CONFIG_EKF2_AIRSPEED) - if (_airspeed_buffer) { - printf("airspeed buffer: %d/%d (%d Bytes)\n", _airspeed_buffer->entries(), _airspeed_buffer->get_length(), _airspeed_buffer->get_total_size()); - } -#endif // CONFIG_EKF2_AIRSPEED - -#if defined(CONFIG_EKF2_OPTICAL_FLOW) - if (_flow_buffer) { - printf("flow buffer: %d/%d (%d Bytes)\n", _flow_buffer->entries(), _flow_buffer->get_length(), _flow_buffer->get_total_size()); - } -#endif // CONFIG_EKF2_OPTICAL_FLOW - -#if defined(CONFIG_EKF2_EXTERNAL_VISION) - if (_ext_vision_buffer) { - printf("vision buffer: %d/%d (%d Bytes)\n", _ext_vision_buffer->entries(), _ext_vision_buffer->get_length(), _ext_vision_buffer->get_total_size()); - } -#endif // CONFIG_EKF2_EXTERNAL_VISION - -#if defined(CONFIG_EKF2_DRAG_FUSION) - if (_drag_buffer) { - printf("drag buffer: %d/%d (%d Bytes)\n", _drag_buffer->entries(), _drag_buffer->get_length(), _drag_buffer->get_total_size()); - } -#endif // CONFIG_EKF2_DRAG_FUSION - - _output_predictor.print_status(); -} diff --git a/src/modules/ekf2/EKF/estimator_interface.h b/src/modules/ekf2/EKF/estimator_interface.h index 4fec3244eb..e94d14b5e6 100644 --- a/src/modules/ekf2/EKF/estimator_interface.h +++ b/src/modules/ekf2/EKF/estimator_interface.h @@ -312,8 +312,6 @@ public: const MapProjection &global_origin() const { return _pos_ref; } float getEkfGlobalOriginAltitude() const { return PX4_ISFINITE(_gps_alt_ref) ? _gps_alt_ref : 0.f; } - void print_status(); - OutputPredictor &output_predictor() { return _output_predictor; }; protected: @@ -479,7 +477,7 @@ protected: warning_event_status_u _warning_events{}; information_event_status_u _information_events{}; -private: + unsigned _min_obs_interval_us{0}; // minimum time interval between observations that will guarantee data is not lost (usec) #if defined(CONFIG_EKF2_DRAG_FUSION) void setDragData(const imuSample &imu); @@ -492,7 +490,5 @@ private: void printBufferAllocationFailed(const char *buffer_name); ImuDownSampler _imu_down_sampler{_params.filter_update_interval_us}; - - unsigned _min_obs_interval_us{0}; // minimum time interval between observations that will guarantee data is not lost (usec) }; #endif // !EKF_ESTIMATOR_INTERFACE_H diff --git a/src/modules/ekf2/EKF/output_predictor.cpp b/src/modules/ekf2/EKF/output_predictor.cpp index 32bf4fe8ad..93086dc7e6 100644 --- a/src/modules/ekf2/EKF/output_predictor.cpp +++ b/src/modules/ekf2/EKF/output_predictor.cpp @@ -41,16 +41,32 @@ using matrix::Vector3f; void OutputPredictor::print_status() { - printf("output predictor: IMU dt: %.4f, EKF dt: %.4f\n", (double)_dt_update_states_avg, (double)_dt_correct_states_avg); + printf("[output predictor] IMU dt: %.6f, EKF dt: %.6f\n", + (double)_dt_update_states_avg, (double)_dt_correct_states_avg); - printf("output predictor: tracking error, angular: %.6f rad, velocity: %.3f m/s, position: %.3f m\n", + const matrix::Quatf q_att = _output_buffer.get_newest().quat_nominal; + const matrix::Eulerf euler = q_att; + + printf("[output predictor] orientation: [%.4f, %.4f, %.4f, %.4f] (Euler [%.3f, %.3f, %.3f])\n", + (double)q_att(0), (double)q_att(1), (double)q_att(2), (double)q_att(3), + (double)euler.phi(), (double)euler.theta(), (double)euler.psi()); + + printf("[output predictor] velocity: [%.3f, %.3f, %.3f]\n", + (double)_output_buffer.get_newest().vel(0), (double)_output_buffer.get_newest().vel(1), + (double)_output_buffer.get_newest().vel(2)); + + printf("[output predictor] position: [%.3f, %.3f, %.3f]\n", + (double)_output_buffer.get_newest().pos(0), (double)_output_buffer.get_newest().pos(1), + (double)_output_buffer.get_newest().pos(2)); + + printf("[output predictor] tracking error, angular: %.6f rad, velocity: %.4f m/s, position: %.4f m\n", (double)_output_tracking_error(0), (double)_output_tracking_error(1), (double)_output_tracking_error(2)); - printf("output buffer: %d/%d (%d Bytes)\n", _output_buffer.entries(), _output_buffer.get_length(), - _output_buffer.get_total_size()); + printf("[output predictor] output buffer: %d/%d (%d Bytes)\n", + _output_buffer.entries(), _output_buffer.get_length(), _output_buffer.get_total_size()); - printf("output vert buffer: %d/%d (%d Bytes)\n", _output_vert_buffer.entries(), _output_vert_buffer.get_length(), - _output_vert_buffer.get_total_size()); + printf("[output predictor] output vert buffer: %d/%d (%d Bytes)\n", + _output_vert_buffer.entries(), _output_vert_buffer.get_length(), _output_vert_buffer.get_total_size()); } void OutputPredictor::alignOutputFilter(const Quatf &quat_state, const Vector3f &vel_state, const Vector3f &pos_state)