test recording of velocity reset

test/test_helper/reset_logging_checker.cpp

@@ -0,0 +1,81 @@
+#include "reset_logging_checker.h"
+
+// call immediately after state reset
+void ResetLoggingChecker::capturePreResetState() {
+	float a[2];
+	float b;
+	uint8_t c;
+	_ekf->get_velNE_reset(a, &c);
+	horz_vel_reset_counter_before_reset = c;
+	_ekf->get_velD_reset(&b, &c);
+	vert_vel_reset_counter_before_reset = c;
+	_ekf->get_posNE_reset(a, &c);
+	horz_pos_reset_counter_before_reset = c;
+	_ekf->get_posD_reset(&b, &c);
+	vert_pos_reset_counter_before_reset = c;
+
+	velocity_before_reset = _ekf->getVelocity();
+	position_before_reset = _ekf->getPosition();
+}
+
+// call immediately after state reset
+void ResetLoggingChecker::capturePostResetState() {
+	float a[2];
+	float b;
+	uint8_t c;
+	_ekf->get_velNE_reset(a, &c);
+	logged_delta_velocity(0) = a[0];
+	logged_delta_velocity(1) = a[1];
+	horz_vel_reset_counter_after_reset = c;
+	_ekf->get_velD_reset(&b, &c);
+	logged_delta_velocity(2) = b;
+	vert_vel_reset_counter_after_reset = c;
+	_ekf->get_posNE_reset(a, &c);
+	logged_delta_position(0) = a[0];
+	logged_delta_position(1) = a[1];
+	horz_pos_reset_counter_after_reset = c;
+	_ekf->get_posD_reset(&b, &c);
+	logged_delta_position(2) = b;
+	vert_pos_reset_counter_after_reset = c;
+
+	velocity_after_reset = _ekf->getVelocity();
+	position_after_reset = _ekf->getPosition();
+}
+
+bool ResetLoggingChecker::isVelocityDeltaLoggedCorrectly(float accuracy) {
+	const  Vector3f measured_delta_velocity = velocity_after_reset -
+							velocity_before_reset;
+
+	return matrix::isEqual(logged_delta_velocity,
+				measured_delta_velocity,
+				accuracy);
+}
+
+bool ResetLoggingChecker::isHorizontalVelocityResetCounterIncreasedBy(int offset) {
+	return horz_vel_reset_counter_after_reset ==
+		horz_vel_reset_counter_before_reset + offset;
+}
+
+bool ResetLoggingChecker::isVerticalVelocityResetCounterIncreasedBy(int offset) {
+	return vert_vel_reset_counter_after_reset ==
+		vert_vel_reset_counter_before_reset + offset;
+}
+
+bool ResetLoggingChecker::isPositionDeltaLoggedCorrectly(float accuracy) {
+	const  Vector3f measured_delta_position = position_after_reset -
+							position_before_reset;
+
+	return matrix::isEqual(logged_delta_position,
+				measured_delta_position,
+				accuracy);
+}
+
+bool ResetLoggingChecker::isHorizontalPositionResetCounterIncreasedBy(int offset) {
+	return horz_pos_reset_counter_after_reset ==
+		horz_pos_reset_counter_before_reset + offset;
+}
+
+bool ResetLoggingChecker::isVerticalPositionResetCounterIncreasedBy(int offset) {
+	return vert_pos_reset_counter_after_reset ==
+		vert_pos_reset_counter_before_reset + offset;
+}