diff --git a/platformio.ini b/platformio.ini
index 470aefe..ee1a3ed 100644
--- a/platformio.ini
+++ b/platformio.ini
@@ -15,4 +15,4 @@ board = atmega328p
 build_flags = -D VERBOSE
 upload_protocol = custom
 upload_speed = 19200
-upload_command = avrdude -C /Users/camerongoddard/Library/Arduino15/packages/arduino/tools/avrdude/6.3.0-arduino17/etc/avrdude.conf -P /dev/cu.usbmodem1301 -b 19200 -v -V -p atmega328p -c stk500v1 $UPLOAD_FLAGS -U flash:w:$SOURCE:i
\ No newline at end of file
+upload_command = avrdude -C /Users/camerongoddard/Library/Arduino15/packages/arduino/tools/avrdude/6.3.0-arduino17/etc/avrdude.conf -P /dev/cu.usbmodem21301 -b 19200 -v -V -p atmega328p -c stk500v1 $UPLOAD_FLAGS -U flash:w:$SOURCE:i
\ No newline at end of file
diff --git a/src/ControlTasks/RadioControlTask.cpp b/src/ControlTasks/RadioControlTask.cpp
index aa91f24..cba8ced 100644
--- a/src/ControlTasks/RadioControlTask.cpp
+++ b/src/ControlTasks/RadioControlTask.cpp
@@ -259,9 +259,9 @@ void RadioControlTask::execute()
 
 bool RadioControlTask::executeDownlink()
 {
-    uint16_t lat = round(map(sfr::gps::latitude, constants::gps::lat_min, constants::gps::lat_max, 0, 65536));
-    uint16_t lon = round(map(sfr::gps::longitude, constants::gps::lon_min, constants::gps::lon_max, 0, 65536));
-    uint16_t alt = round(map(sfr::gps::altitude / 10, constants::gps::alt_min, constants::gps::alt_max, 0, 65536));
+    uint16_t lat = map(sfr::gps::latitude, constants::gps::lat_min, constants::gps::lat_max, 0, 65536);
+    uint16_t lon = map(sfr::gps::longitude, constants::gps::lon_min, constants::gps::lon_max, 0, 65536);
+    uint16_t alt = map(sfr::gps::altitude / 10, constants::gps::alt_min, constants::gps::alt_max, 0, 65536);
 
     uint8_t flags = ((sfr::radio::mode == radio_mode_type::listen) ? 0xF0 : 0x00) | (sfr::gps::valid_msg ? 0x0F : 0x00);
 
@@ -269,13 +269,13 @@ bool RadioControlTask::executeDownlink()
         (uint8_t)(lat >> 8), (uint8_t)lat,
         (uint8_t)(lon >> 8), (uint8_t)lon,
         (uint8_t)(alt >> 8), (uint8_t)alt,
-        (uint8_t)round(map(sfr::imu::gyro_x, constants::imu::gyro_min, constants::imu::gyro_max, 0, 255)),
-        (uint8_t)round(map(sfr::imu::gyro_y, constants::imu::gyro_min, constants::imu::gyro_max, 0, 255)),
-        (uint8_t)round(map(sfr::imu::gyro_z, constants::imu::gyro_min, constants::imu::gyro_max, 0, 255)),
-        (uint8_t)round(map(sfr::imu::acc_x, constants::imu::acc_min, constants::imu::acc_max, 0, 255)),
-        (uint8_t)round(map(sfr::imu::acc_y, constants::imu::acc_min, constants::imu::acc_max, 0, 255)),
-        (uint8_t)round(map(sfr::imu::acc_z, constants::imu::acc_min, constants::imu::acc_max, 0, 255)),
-        (uint8_t)round(map(sfr::temperature::temp_c, constants::temperature::min, constants::temperature::max, 0, 255)),
+        map_range(sfr::imu::gyro_x, constants::imu::gyro_min, constants::imu::gyro_max),
+        map_range(sfr::imu::gyro_y, constants::imu::gyro_min, constants::imu::gyro_max),
+        map_range(sfr::imu::gyro_z, constants::imu::gyro_min, constants::imu::gyro_max),
+        map_range(sfr::imu::acc_x, constants::imu::acc_min, constants::imu::acc_max),
+        map_range(sfr::imu::acc_y, constants::imu::acc_min, constants::imu::acc_max),
+        map_range(sfr::imu::acc_z, constants::imu::acc_min, constants::imu::acc_max),
+        map_range(sfr::temperature::temp_c, constants::temperature::min, constants::temperature::max),
         flags};
 
 #ifdef VERBOSE
@@ -288,6 +288,17 @@ bool RadioControlTask::executeDownlink()
     return transmit(dlink, sizeof(dlink));
 }
 
+uint8_t RadioControlTask::map_range(float value, int min, int max) {
+    long raw = map(value, min, max, 0, 255);
+    if (raw > 255) {
+        raw = 255;
+    }
+    if (raw < 0) {
+        raw = 0;
+    }
+    return (uint8_t)raw;
+}
+
 void RadioControlTask::processUplink()
 {
 
diff --git a/src/ControlTasks/RadioControlTask.hpp b/src/ControlTasks/RadioControlTask.hpp
index 66d73e8..32ec2f5 100644
--- a/src/ControlTasks/RadioControlTask.hpp
+++ b/src/ControlTasks/RadioControlTask.hpp
@@ -18,6 +18,7 @@ class RadioControlTask
     bool transmit(uint8_t *packet, uint8_t size);
     bool receive();
     bool executeDownlink();
+    uint8_t map_range(float value, int min, int max);
     void processUplink();
     uint8_t *received;
 };
diff --git a/src/Monitors/IMUMonitor.cpp b/src/Monitors/IMUMonitor.cpp
index a9a574e..55ba04e 100644
--- a/src/Monitors/IMUMonitor.cpp
+++ b/src/Monitors/IMUMonitor.cpp
@@ -53,19 +53,19 @@ void IMUMonitor::execute()
 void IMUMonitor::capture_imu_values()
 {
 
-    if (IMU.gyroscopeAvailable()) { // check if the gyroscope has new data available
-        IMU.readGyroscope(
-            sfr::imu::gyro_x,
-            sfr::imu::gyro_y,
-            sfr::imu::gyro_z); // data is in degrees/s
-    }
+    // if (IMU.gyroscopeAvailable()) { // check if the gyroscope has new data available
+    //     IMU.readGyroscope(
+    //         sfr::imu::gyro_x,
+    //         sfr::imu::gyro_y,
+    //         sfr::imu::gyro_z); // data is in degrees/s
+    // }
 
-    if (IMU.accelerationAvailable()) { // check if accelerometer is available
-        IMU.readAcceleration(
-            sfr::imu::acc_x,
-            sfr::imu::acc_y,
-            sfr::imu::acc_z); // data is in m/s^2
-    }
+    // if (IMU.accelerationAvailable()) { // check if accelerometer is available
+    //     IMU.readAcceleration(
+    //         sfr::imu::acc_x,
+    //         sfr::imu::acc_y,
+    //         sfr::imu::acc_z); // data is in m/s^2
+    // }
 }
 
 void IMUMonitor::transition_to_normal()
diff --git a/src/sfr.cpp b/src/sfr.cpp
index c6d373a..56af6a5 100644
--- a/src/sfr.cpp
+++ b/src/sfr.cpp
@@ -4,9 +4,13 @@ namespace sfr {
 
     namespace imu {
 
-        float gyro_x, gyro_y, gyro_z;
+        float gyro_x = -250;
+        float gyro_y = 250;
+        float gyro_z = 500;
 
-        float acc_x, acc_y, acc_z;
+        float acc_x = 30;
+        float acc_y = -30;
+        float acc_z = -50;
 
         bool initialized = false;
         uint16_t failed_times = 0;