diff --git a/devicehandlers/GyroL3GD20Handler.cpp b/devicehandlers/GyroL3GD20Handler.cpp index a732daf..94e61cc 100644 --- a/devicehandlers/GyroL3GD20Handler.cpp +++ b/devicehandlers/GyroL3GD20Handler.cpp @@ -187,12 +187,12 @@ ReturnValue_t GyroHandlerL3GD20H::interpretDeviceReply(DeviceCommandId_t id, statusReg = packet[L3GD20H::STATUS_IDX]; - float angVelocX = (packet[L3GD20H::OUT_X_H] << 8 | - packet[L3GD20H::OUT_X_L]) * scaleFactor; - float angVelocY = (packet[L3GD20H::OUT_Y_H] << 8 | - packet[L3GD20H::OUT_Y_L]) * scaleFactor; - float angVelocZ = (packet[L3GD20H::OUT_Z_H] << 8 | - packet[L3GD20H::OUT_Z_L]) * scaleFactor; + int16_t angVelocXRaw = packet[L3GD20H::OUT_X_H] << 8 | packet[L3GD20H::OUT_X_L]; + int16_t angVelocYRaw = packet[L3GD20H::OUT_Y_H] << 8 | packet[L3GD20H::OUT_Y_L]; + int16_t angVelocZRaw = packet[L3GD20H::OUT_Z_H] << 8 | packet[L3GD20H::OUT_Z_L]; + float angVelocX = angVelocXRaw * scaleFactor; + float angVelocY = angVelocYRaw * scaleFactor; + float angVelocZ = angVelocZRaw * scaleFactor; int8_t temperaturOffset = (-1) * packet[L3GD20H::TEMPERATURE_IDX]; float temperature = 25.0 + temperaturOffset; @@ -207,9 +207,9 @@ ReturnValue_t GyroHandlerL3GD20H::interpretDeviceReply(DeviceCommandId_t id, sif::info << "Z: " << angVelocZ << " \xC2\xB0" << std::endl; #else sif::printInfo("GyroHandlerL3GD20H: Angular velocities in degrees per second:\n"); - sif::printInfo("X: %f " "\xC2\xB0" "T\n", angVelocX); - sif::printInfo("Y: %f " "\xC2\xB0" "T\n", angVelocY); - sif::printInfo("Z: %f " "\xC2\xB0" "T\n", angVelocZ); + sif::printInfo("X: %f \xC2\xB0\n", angVelocX); + sif::printInfo("Y: %f \xC2\xB0\n", angVelocY); + sif::printInfo("Z: %f \xC2\xB0\n", angVelocZ); #endif } #endif