diff --git a/src/ControlTasks/RadioControlTask.cpp b/src/ControlTasks/RadioControlTask.cpp
index cba8ced..bae61f3 100644
--- a/src/ControlTasks/RadioControlTask.cpp
+++ b/src/ControlTasks/RadioControlTask.cpp
@@ -110,7 +110,11 @@ void RadioControlTask::init()
 
 bool RadioControlTask::transmit(uint8_t *packet, uint8_t size)
 {
+    long start = millis();
     code = radio.transmit(packet, size);
+    long time = millis() - start;
+    Serial.print(F("Time to transmit (ms): "));
+    Serial.println(time);
 
     if (code == RADIOLIB_ERR_NONE) {
         // the packet was successfully transmitted
@@ -259,9 +263,9 @@ void RadioControlTask::execute()
 
 bool RadioControlTask::executeDownlink()
 {
-    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);
+    uint16_t lat = map_range_16(sfr::gps::latitude, constants::gps::lat_min, constants::gps::lat_max);
+    uint16_t lon = map_range_16(sfr::gps::longitude, constants::gps::lon_min, constants::gps::lon_max);
+    uint16_t alt = map_range_16(sfr::gps::altitude / 10, constants::gps::alt_min, constants::gps::alt_max);
 
     uint8_t flags = ((sfr::radio::mode == radio_mode_type::listen) ? 0xF0 : 0x00) | (sfr::gps::valid_msg ? 0x0F : 0x00);
 
@@ -288,17 +292,23 @@ 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;
-    }
+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;
 }
 
+uint16_t RadioControlTask::map_range_16(float value, int min_val, int max_val)
+{
+    long raw = map(value, min_val, max_val, 0, 65536);
+    Serial.println(raw);
+    raw = min(raw, 65536);
+    raw = max(raw, 0);
+    return (uint16_t)raw;
+}
+
 void RadioControlTask::processUplink()
 {
 
diff --git a/src/ControlTasks/RadioControlTask.hpp b/src/ControlTasks/RadioControlTask.hpp
index 32ec2f5..e69f1e9 100644
--- a/src/ControlTasks/RadioControlTask.hpp
+++ b/src/ControlTasks/RadioControlTask.hpp
@@ -19,6 +19,7 @@ class RadioControlTask
     bool receive();
     bool executeDownlink();
     uint8_t map_range(float value, int min, int max);
+    uint16_t map_range_16(float value, int min, int max);
     void processUplink();
     uint8_t *received;
 };
diff --git a/src/Monitors/GPSMonitor.cpp b/src/Monitors/GPSMonitor.cpp
index da7c452..2e5348f 100644
--- a/src/Monitors/GPSMonitor.cpp
+++ b/src/Monitors/GPSMonitor.cpp
@@ -81,7 +81,7 @@ bool GPSMonitor::encode(char c)
                 char temp_buf[11];
                 strncpy(temp_buf, term_buffer, 10);
                 temp_buf[10] = '\0';
-                sfr::gps::latitude = atof(temp_buf);
+                // sfr::gps::latitude = atof(temp_buf);
                 Serial.println(atof(temp_buf));
 
                 break;
@@ -89,7 +89,7 @@ bool GPSMonitor::encode(char c)
             case 3: // Latitude hemisphere
             {
                 if (term_buffer[0] == 'S') {
-                    sfr::gps::latitude = -sfr::gps::latitude;
+                    // sfr::gps::latitude = -sfr::gps::latitude;
                     // Serial.println(latitude);
                 }
                 break;
@@ -101,7 +101,7 @@ bool GPSMonitor::encode(char c)
                 char temp_buf[12];
                 strncpy(temp_buf, term_buffer, 11);
                 temp_buf[11] = '\0';
-                sfr::gps::longitude = atof(temp_buf);
+                // sfr::gps::longitude = atof(temp_buf);
                 Serial.println(atof(temp_buf));
 
                 break;
@@ -109,7 +109,7 @@ bool GPSMonitor::encode(char c)
             case 5: // Longitude hemisphere
             {
                 if (term_buffer[0] == 'W') {
-                    sfr::gps::longitude = -sfr::gps::longitude;
+                    // sfr::gps::longitude = -sfr::gps::longitude;
                     // Serial.println(longitude);
                 }
                 break;
@@ -121,7 +121,7 @@ bool GPSMonitor::encode(char c)
                 char temp_buf[char_count];
                 strncpy(temp_buf, term_buffer, char_count - 1);
                 temp_buf[char_count - 1] = '\0';
-                sfr::gps::altitude = atof(temp_buf);
+                // sfr::gps::altitude = atof(temp_buf);
                 Serial.println(atof(temp_buf));
 
                 break;
diff --git a/src/sfr.cpp b/src/sfr.cpp
index 56af6a5..c2b691f 100644
--- a/src/sfr.cpp
+++ b/src/sfr.cpp
@@ -4,12 +4,13 @@ namespace sfr {
 
     namespace imu {
 
+        // TEMP Values
         float gyro_x = -250;
         float gyro_y = 250;
         float gyro_z = 500;
 
         float acc_x = 30;
-        float acc_y = -30;
+        float acc_y = 0;
         float acc_z = -50;
 
         bool initialized = false;
@@ -26,9 +27,10 @@ namespace sfr {
     namespace gps {
         float utc_time;
 
-        float latitude;
-        float longitude;
-        float altitude;
+        // TEMP values
+        float latitude = 9000;
+        float longitude = 18000;
+        float altitude = 500000;
 
         bool valid_msg = false;
 
@@ -37,7 +39,7 @@ namespace sfr {
         sensor_init_mode_type init_mode = sensor_init_mode_type::init;
         radio_mode_type mode = radio_mode_type::init;
         uint8_t start_progress = 0;
-        uint32_t downlink_period = 30 * constants::time::one_second;
+        uint32_t downlink_period = 10 * constants::time::one_second;
         uint32_t listen_period = 30 * constants::time::one_second;
         uint32_t command_wait_period = 30 * constants::time::one_second;
         uint32_t downlink_period_start;