diff --git a/skyward-boardcore b/skyward-boardcore
index 9973d441dcbf2e88a7ba52b2d35a923da9f5b55a..56f0491004203cfee6d417281a82aedb612ee8d7 160000
--- a/skyward-boardcore
+++ b/skyward-boardcore
@@ -1 +1 @@
-Subproject commit 9973d441dcbf2e88a7ba52b2d35a923da9f5b55a
+Subproject commit 56f0491004203cfee6d417281a82aedb612ee8d7
diff --git a/src/boards/Payload/Radio/Radio.cpp b/src/boards/Payload/Radio/Radio.cpp
index 5f5fe24c48639a225311df3788a056e70b412136..cba577c5dfaca63f65df524e61ed0f461a14eab2 100644
--- a/src/boards/Payload/Radio/Radio.cpp
+++ b/src/boards/Payload/Radio/Radio.cpp
@@ -394,6 +394,7 @@ void Radio::handleMavlinkMessage(MavDriver* driver,
                 mavlink_msg_set_reference_altitude_tc_get_ref_altitude(&msg);
 
             NASController::getInstance().setReferenceAltitude(altitude);
+            Sensors::getInstance().pitotSetReferenceAltitude(altitude);
             break;
         }
         case MAVLINK_MSG_ID_SET_REFERENCE_TEMPERATURE_TC:
@@ -402,6 +403,7 @@ void Radio::handleMavlinkMessage(MavDriver* driver,
                 mavlink_msg_set_reference_temperature_tc_get_ref_temp(&msg);
 
             NASController::getInstance().setReferenceTemperature(temperature);
+            Sensors::getInstance().pitotSetReferenceTemperature(temperature);
             break;
         }
         case MAVLINK_MSG_ID_SET_DEPLOYMENT_ALTITUDE_TC:
diff --git a/src/boards/Payload/Sensors/Sensors.cpp b/src/boards/Payload/Sensors/Sensors.cpp
index ec4c9a685eb93e3a2831f54d2279bfcc4850f555..960a28b290b22dd8d69325910bc19ebc72c7b00a 100644
--- a/src/boards/Payload/Sensors/Sensors.cpp
+++ b/src/boards/Payload/Sensors/Sensors.cpp
@@ -169,6 +169,28 @@ void Sensors::calibrate()
     calibrating = true;
 }
 
+void Sensors::pitotSetReferenceAltitude(float altitude)
+{
+    // Need to pause the kernel because the only invocation comes from the radio
+    // which is a separate thread
+    miosix::PauseKernelLock l;
+
+    ReferenceValues reference = pitot->getReferenceValues();
+    reference.refAltitude     = altitude;
+    pitot->setReferenceValues(reference);
+}
+
+void Sensors::pitotSetReferenceTemperature(float temperature)
+{
+    // Need to pause the kernel because the only invocation comes from the radio
+    // which is a separate thread
+    miosix::PauseKernelLock l;
+
+    ReferenceValues reference = pitot->getReferenceValues();
+    reference.refTemperature  = temperature + 273.15f;
+    pitot->setReferenceValues(reference);
+}
+
 std::map<string, bool> Sensors::getSensorsState()
 {
     std::map<string, bool> sensorsState;
diff --git a/src/boards/Payload/Sensors/Sensors.h b/src/boards/Payload/Sensors/Sensors.h
index 0a478e1922d04d3662b6046386b2edbb0d782723..97b8397005400411aeef050eed5f340ae22b4f91 100644
--- a/src/boards/Payload/Sensors/Sensors.h
+++ b/src/boards/Payload/Sensors/Sensors.h
@@ -73,6 +73,9 @@ public:
      */
     void calibrate();
 
+    void pitotSetReferenceAltitude(float altitude);
+    void pitotSetReferenceTemperature(float temperature);
+
     std::map<string, bool> getSensorsState();
 
 private: