rotate accel/gyro FIFO before publish and fix angular velocity filter resets

- rotates accel & gyro FIFO data before publication both to simplify downstream usage (including log review) and fix other issues
     - to best handle int16_t data rotations are now either performed with swaps if possible, otherwise promoted to float, rotated using the full rotation matrix, then rounded back to int16_t
 - fix sensors/vehicle_angular_velocity filter reset both with proper rotation and new calibration uncorrect helper
      - in FIFO case filtering is done before calibration is applied, but we need to handle a possible reset from a completely different sensor (vehicle body angular velocity -> sensor frame uncorrected data)

src/lib/mathlib/math/Functions.hpp

@@ -210,4 +210,17 @@ const T lerp(const T &a, const T &b, const T &s)
 	return (static_cast<T>(1) - s) * a + s * b;
 }
 
+template<typename T>
+constexpr T negate(T value)
+{
+	static_assert(sizeof(T) > 2, "implement for T");
+	return -value;
+}
+
+template<>
+constexpr int16_t negate<int16_t>(int16_t value)
+{
+	return (value == INT16_MIN) ? INT16_MAX : -value;
+}
+
 } /* namespace math */