diff --git a/src/shared/sensors/UBXGPS/UBXFrame.h b/src/shared/sensors/UBXGPS/UBXFrame.h
index 46437171199cdad96bf4477b06d8629969f4dfda..a54d31c0334b85c8c806d308fac776110af56a8f 100644
--- a/src/shared/sensors/UBXGPS/UBXFrame.h
+++ b/src/shared/sensors/UBXGPS/UBXFrame.h
@@ -43,7 +43,6 @@ enum class UBXMessage : uint16_t
     UBX_CFG_RST    = 0x0406,  // Reset receiver
     UBX_CFG_RATE   = 0x0806,  // Navigation/measurement rate settings
     UBX_CFG_NAV5   = 0x2406,  // Navigation engine settings
-    UBX_CFG_VALSET = 0x8a06,  // TODO: Update to newer command
 };
 
 /**
diff --git a/src/shared/sensors/UBXGPS/UBXGPSSerial.cpp b/src/shared/sensors/UBXGPS/UBXGPSSerial.cpp
index c709c9d97a390a2dc555fbbbed16443c1b8f621c..4bd60787c5a033840be60e2b64a2bb4c4e218bda 100644
--- a/src/shared/sensors/UBXGPS/UBXGPSSerial.cpp
+++ b/src/shared/sensors/UBXGPS/UBXGPSSerial.cpp
@@ -66,7 +66,7 @@ bool UBXGPSSerial::init()
 
     LOG_DEBUG(logger, "Setting the UBX protocol...");
 
-    if (!setUBXProtocol())
+    if (!setBaudrateAndUBX())
     {
         lastError = SensorErrors::INIT_FAIL;
         LOG_ERR(logger, "Could not set the UBX protocol");
@@ -131,14 +131,18 @@ bool UBXGPSSerial::reset()
     return true;
 }
 
-bool UBXGPSSerial::setBaudrate()
+bool UBXGPSSerial::setBaudrateAndUBX(bool safe)
 {
     uint8_t payload[] = {
-        0x00,                    // Version
-        0xff,                    // All layers
-        0x00, 0x00,              // Reserved
-        0x01, 0x00, 0x52, 0x40,  // Configuration item key ID
-        0xff, 0xff, 0xff, 0xff,  // Value
+        0x01,                    // UART port
+        0x00,                    // Reserved
+        0x00, 0x00,              // txReady
+        0xc0, 0x08, 0x00, 0x00,  // 8bit, no parity, 1 stop bit
+        0x00, 0x00, 0x00, 0x00,  // Baudrate
+        0x01, 0x00,              // inProtoMask = UBX
+        0x01, 0x00,              // outProtoMask = UBX
+        0x00, 0x00,              // flags
+        0x00, 0x00               // reserved2
     };
 
     // Prepare baudrate
@@ -147,9 +151,12 @@ bool UBXGPSSerial::setBaudrate()
     payload[10] = baudrate >> 16;
     payload[11] = baudrate >> 24;
 
-    UBXFrame frame{UBXMessage::UBX_CFG_VALSET, payload, sizeof(payload)};
+    UBXFrame frame{UBXMessage::UBX_CFG_PRT, payload, sizeof(payload)};
 
-    return safeWriteUBXFrame(frame);
+    if (safe)
+        return safeWriteUBXFrame(frame);
+    else
+        return writeUBXFrame(frame);
 }
 
 bool UBXGPSSerial::setSerialCommunication()
@@ -182,7 +189,7 @@ bool UBXGPSSerial::setSerialCommunication()
         }
 
         // Change baudrate
-        if (!setBaudrate())
+        if (!setBaudrateAndUBX(false))
         {
             return false;
         };
@@ -224,25 +231,6 @@ bool UBXGPSSerial::setSerialCommunication()
     return true;
 }
 
-bool UBXGPSSerial::setUBXProtocol()
-{
-    uint8_t payload[] = {
-        0x04,                    // SPI port
-        0x00,                    // reserved0
-        0x00, 0x00,              // txReady
-        0x00, 0x00, 0x00, 0x00,  // spiMode = 0, ffCnt = 0 (mechanism off)
-        0x00, 0x00, 0x00, 0x00,  // reserved1
-        0x01, 0x00,              // inProtoMask = UBX
-        0x01, 0x00,              // outProtoMask = UBX
-        0x00, 0x00,              // flags
-        0x00, 0x00               // reserved2
-    };
-
-    UBXFrame frame{UBXMessage::UBX_CFG_PRT, payload, sizeof(payload)};
-
-    return safeWriteUBXFrame(frame);
-}
-
 bool UBXGPSSerial::setDynamicModelToAirborne4g()
 {
     uint8_t payload[] = {
diff --git a/src/shared/sensors/UBXGPS/UBXGPSSerial.h b/src/shared/sensors/UBXGPS/UBXGPSSerial.h
index 7f8bcc646cac712ac3395a15c6ff15aad9d3503f..4785778c2beab52185b2b614f06db7ab2c0e4769 100644
--- a/src/shared/sensors/UBXGPS/UBXGPSSerial.h
+++ b/src/shared/sensors/UBXGPS/UBXGPSSerial.h
@@ -91,7 +91,14 @@ private:
      */
     bool reset();
 
-    bool setBaudrate();
+    /**
+     * @brief Sets the UART port baudrate and enables UBX and disables NMEA on
+     * the UART port.
+     *
+     * @param safe Whether to expect an ack after the command.
+     * @return True if the configuration received an acknowledgement.
+     */
+    bool setBaudrateAndUBX(bool safe = true);
 
     /**
      * @brief Sets up the serial port with the correct baudrate.
@@ -104,13 +111,6 @@ private:
      */
     bool setSerialCommunication();
 
-    /**
-     * @brief Enables UBX and disables NMEA on the SPI port.
-     *
-     * @return True if the configuration received an acknowledgement.
-     */
-    bool setUBXProtocol();
-
     /**
      * @brief Configures the dynamic model to airborn 4g.
      *
diff --git a/src/tests/sensors/test-ubxgps-serial.cpp b/src/tests/sensors/test-ubxgps-serial.cpp
index 5518137419f3e03ea55e72011efdab7dbae65111..cf7fe1c50ed6a63c56ec534207b02bbee16a6898 100644
--- a/src/tests/sensors/test-ubxgps-serial.cpp
+++ b/src/tests/sensors/test-ubxgps-serial.cpp
@@ -39,7 +39,7 @@ int main()
     printf("Welcome to the ublox test\n");
 
     // Keep GPS baud rate at default for easier testing
-    UBXGPSSerial gps(921600, RATE, 2, "gps", 38400);
+    UBXGPSSerial gps(38400, RATE, 2, "gps", 9600);
     UBXGPSData dataGPS;
     printf("Gps allocated\n");