diff --git a/src/ControlTasks/RadioControlTask.cpp b/src/ControlTasks/RadioControlTask.cpp
index e4bc1b2..9321697 100644
--- a/src/ControlTasks/RadioControlTask.cpp
+++ b/src/ControlTasks/RadioControlTask.cpp
@@ -263,6 +263,9 @@ bool RadioControlTask::normalReportDownlink()
     flags |= sfr::gps::on << 2;                                  // boot mode flag
     flags |= (sfr::radio::mode == radio_mode_type::listen) << 1; // listen flag
 
+    Serial.println("Acc X: " + String(sfr::imu::acc_x));
+    Serial.println("Acc Y: " + String(sfr::imu::acc_y));
+
     uint8_t dlink[] = {
         (uint8_t)lat, (uint8_t)(lat >> 8),
         (uint8_t)lon, (uint8_t)(lon >> 8),
@@ -292,10 +295,9 @@ bool RadioControlTask::normalReportDownlink()
 
 uint8_t RadioControlTask::map_range(float value, int min_val, int max_val)
 {
-    long raw = map(value, min_val, max_val, 0, 255);
-    raw = min(raw, 255);
-    raw = max(raw, 0);
-    return (uint8_t)raw;
+    float raw = (value - min_val) / (max_val - min_val) * 255.0f;
+    raw = fminf(fmaxf(raw, 0.0f), 255.0f);
+    return static_cast<uint8_t>(raw);
 }
 
 void RadioControlTask::processUplink()
diff --git a/src/MainControlLoop.cpp b/src/MainControlLoop.cpp
index ccf6445..2209cd2 100644
--- a/src/MainControlLoop.cpp
+++ b/src/MainControlLoop.cpp
@@ -13,58 +13,58 @@ void MainControlLoop::execute()
 {
 
 #ifdef VERBOSE
-    Serial.println(F("-------------------- START LOOP --------------------"));
+    Serial.println("-------------------- START LOOP --------------------");
 
-    Serial.print(F("Gyro X: "));
+    Serial.print("Gyro X: ");
     Serial.println(sfr::imu::gyro_x);
 
-    Serial.print(F("Gyro Y: "));
+    Serial.print("Gyro Y: ");
     Serial.println(sfr::imu::gyro_y);
 
-    Serial.print(F("Gyro Z: "));
+    Serial.print("Gyro Z: ");
     Serial.println(sfr::imu::gyro_z);
 
-    Serial.print(F("Accel X: "));
+    Serial.print("Accel X: ");
     Serial.println(sfr::imu::acc_x);
 
-    Serial.print(F("Accel Y: "));
+    Serial.print("Accel Y: ");
     Serial.println(sfr::imu::acc_y);
 
-    Serial.print(F("Accel Z: "));
+    Serial.print("Accel Z: ");
     Serial.println(sfr::imu::acc_z);
 
-    Serial.print(F("Mag X: "));
+    Serial.print("Mag X: ");
     Serial.println(sfr::imu::mag_x);
 
-    Serial.print(F("Mag Y: "));
+    Serial.print("Mag Y: ");
     Serial.println(sfr::imu::mag_y);
 
-    Serial.print(F("Mag Z: "));
+    Serial.print("Mag Z: ");
     Serial.println(sfr::imu::mag_z);
 
-    Serial.print(F("Temperature (C): "));
+    Serial.print("Temperature (C): ");
     Serial.print(sfr::temperature::temp_c);
-    Serial.println(F(" C"));
+    Serial.println(" C");
 
-    Serial.print(F("GPS On: "));
+    Serial.print("GPS On: ");
     Serial.println(sfr::gps::on);
 
     Serial.print("UTC Time: ");
     Serial.println(sfr::gps::utc_time);
 
-    Serial.print(F("GPS Latitude: "));
+    Serial.print("GPS Latitude: ");
     Serial.println(sfr::gps::latitude);
 
-    Serial.print(F("GPS Longitude: "));
+    Serial.print("GPS Longitude: ");
     Serial.println(sfr::gps::longitude);
 
-    Serial.print(F("GPS Altitude (m): "));
+    Serial.print("GPS Altitude (m): ");
     Serial.println(sfr::gps::altitude);
 
-    Serial.print(F("Downlink Slot: "));
+    Serial.print("Downlink Slot: ");
     Serial.println(sfr::radio::downlink_slot);
 
-    Serial.print(F("Alive Time: "));
+    Serial.print("Alive Time: ");
     Serial.println(millis() - sfr::gps::boot_time);
 
 #endif
diff --git a/src/Monitors/TempMonitor.cpp b/src/Monitors/TempMonitor.cpp
index c83ef1b..35e33a5 100644
--- a/src/Monitors/TempMonitor.cpp
+++ b/src/Monitors/TempMonitor.cpp
@@ -6,12 +6,30 @@ TempMonitor::TempMonitor()
 
 void TempMonitor::execute()
 {
-    // Start I2C transmit
+    if (!sfr::temperature::initialized) {
+        Wire.beginTransmission(constants::temperature::i2c_address);
+        Wire.write(0xE7);
+        Wire.endTransmission();
+
+        Wire.requestFrom(constants::temperature::i2c_address, 1);
+
+        if (Wire.available() == 1) {
+            uint8_t user_register = Wire.read();
+            user_register = user_register | 0x81;
+
+            Wire.beginTransmission(constants::temperature::i2c_address);
+            Wire.write(0xE6);
+            Wire.write(user_register);
+            Wire.endTransmission();
+        }
+        sfr::temperature::initialized = true;
+    }
+
     Wire.beginTransmission(constants::temperature::i2c_address);
     // Select no hold master
     Wire.write(0xF3);
     Wire.endTransmission();
-    delay(300); // TODO: Look at this delay (for real)
+    delay(15);
 
     // Request 2 bytes of data
     Wire.requestFrom(constants::temperature::i2c_address, 2);
@@ -22,6 +40,6 @@ void TempMonitor::execute()
     }
 
     // Convert the data
-    uint32_t rawtemp = (data[0] * 256 + data[1]) & 0xFFFC;
-    sfr::temperature::temp_c = -46.85 + (175.72 * (rawtemp / 65536.0));
+    uint32_t raw_temp = (data[0] * 256 + data[1]) & 0xFFFC;
+    sfr::temperature::temp_c = -46.85 + (175.72 * (raw_temp / 65536.0));
 }
diff --git a/src/constants.hpp b/src/constants.hpp
index 4cf7f89..e22c286 100644
--- a/src/constants.hpp
+++ b/src/constants.hpp
@@ -40,9 +40,9 @@ namespace constants {
     namespace imu {
         constexpr int gyro_min = -245; // Default gyro range is +/- 245 dps
         constexpr int gyro_max = 245;
-        constexpr int acc_min = -20; // Default accel range is +/- 2 G
-        constexpr int acc_max = 20;
-        constexpr int mag_min = -100; // Default mag range is +/- 4 guass -> 100 uT
+        constexpr int acc_min = -10; // Default accel range is +/- 2 G
+        constexpr int acc_max = 10;
+        constexpr int mag_min = -100; // Default mag range is +/- 4 gauss -> 100 uT
         constexpr int mag_max = 100;
     } // namespace imu
     namespace temperature {
diff --git a/src/sfr.cpp b/src/sfr.cpp
index 210b713..ad4f519 100644
--- a/src/sfr.cpp
+++ b/src/sfr.cpp
@@ -10,6 +10,8 @@ namespace sfr {
 
     } // namespace imu
     namespace temperature {
+        bool initialized = false;
+
         float temp_c;
     } // namespace temperature
     namespace gps {
diff --git a/src/sfr.hpp b/src/sfr.hpp
index 2ad9c83..4aa967f 100644
--- a/src/sfr.hpp
+++ b/src/sfr.hpp
@@ -23,6 +23,8 @@ namespace sfr {
 
     } // namespace imu
     namespace temperature {
+        extern bool initialized;
+
         extern float temp_c;
     } // namespace temperature
     namespace radio {