diff --git a/src/hardware/IMU/V5InertialSensor.cpp b/src/hardware/IMU/V5InertialSensor.cpp index 992a4b2..fb4013a 100644 --- a/src/hardware/IMU/V5InertialSensor.cpp +++ b/src/hardware/IMU/V5InertialSensor.cpp @@ -42,8 +42,8 @@ int32_t V5InertialSensor::setRotation(Angle rotation) { Angle raw = this->getRotation(); if (to_stRot(raw) == INFINITY) return INT32_MAX; else { - m_offset = rotation - raw; + m_offset = rotation - raw + m_offset; return 0; } } -} // namespace lemlib \ No newline at end of file +} // namespace lemlib