diff --git a/src/boards/Payload/Radio/MessageHandler.cpp b/src/boards/Payload/Radio/MessageHandler.cpp
index b58c8651618ba53d8703b11a7b691e73d38de02f..89eff4225dbdf40a72f2e2c9115c6a8c09276b96 100644
--- a/src/boards/Payload/Radio/MessageHandler.cpp
+++ b/src/boards/Payload/Radio/MessageHandler.cpp
@@ -270,6 +270,39 @@ void Radio::MavlinkBackend::handleMessage(const mavlink_message_t& msg)
             }
         }
 
+        case MAVLINK_MSG_ID_SET_CALIBRATION_PRESSURE_TC:
+        {
+            if (parent.getModule<FlightModeManager>()->getState() !=
+                FlightModeManagerState::ON_GROUND_DISARMED)
+            {
+                return enqueueNack(msg);
+            }
+
+            float press =
+                mavlink_msg_set_calibration_pressure_tc_get_pressure(&msg);
+
+            if (press == 0)
+            {
+                parent.getModule<Sensors>()->resetBaroCalibrationReference();
+                EventBroker::getInstance().post(TMTC_CALIBRATE, TOPIC_TMTC);
+                enqueueAck(msg);
+            }
+            else
+            {
+                parent.getModule<Sensors>()->setBaroCalibrationReference(press);
+                EventBroker::getInstance().post(TMTC_CALIBRATE, TOPIC_TMTC);
+
+                if (press < 50000)
+                {
+                    enqueueWack(msg);
+                }
+                else
+                {
+                    enqueueAck(msg);
+                }
+            }
+        }
+
         case MAVLINK_MSG_ID_RAW_EVENT_TC:
         {
             uint8_t topicId = mavlink_msg_raw_event_tc_get_topic_id(&msg);
diff --git a/src/boards/Payload/Sensors/Sensors.cpp b/src/boards/Payload/Sensors/Sensors.cpp
index ed768b7e56b860967860646db5fb0e040d39392a..a60bdb73d028de3143ac6ce103b25f33d3c93387 100644
--- a/src/boards/Payload/Sensors/Sensors.cpp
+++ b/src/boards/Payload/Sensors/Sensors.cpp
@@ -161,8 +161,15 @@ void Sensors::calibrate()
         dynamicPressureSum / config::Calibration::SAMPLE_COUNT;
     float lps28dfwAvg = lps28dfwSum / config::Calibration::SAMPLE_COUNT;
 
-    // Calibrate the analog static pressure sensor against the LPS28DFW
-    float reference = lps28dfwAvg;
+    // Calibrate all analog pressure sensors against the LPS28DFW or the
+    // telemetry reference
+    float reference = 0;
+    {
+        miosix::Lock<miosix::FastMutex> lock{baroCalibrationMutex};
+        reference = useBaroCalibrationReference ? baroCalibrationReference
+                                                : lps28dfwAvg;
+    }
+
     if (reference > config::Calibration::ATMOS_THRESHOLD)
     {
         // Apply the offset only if reference is valid
@@ -224,6 +231,20 @@ bool Sensors::saveMagCalibration()
     }
 }
 
+void Sensors::setBaroCalibrationReference(float reference)
+{
+    miosix::Lock<miosix::FastMutex> lock{baroCalibrationMutex};
+    baroCalibrationReference    = reference;
+    useBaroCalibrationReference = true;
+}
+
+void Sensors::resetBaroCalibrationReference()
+{
+    miosix::Lock<miosix::FastMutex> lock{baroCalibrationMutex};
+    baroCalibrationReference    = 0.0f;
+    useBaroCalibrationReference = false;
+}
+
 LPS22DFData Sensors::getLPS22DFLastSample()
 {
     return lps22df ? lps22df->getLastSample() : LPS22DFData{};
diff --git a/src/boards/Payload/Sensors/Sensors.h b/src/boards/Payload/Sensors/Sensors.h
index 591547258d35730c545b49c35bbe5474fc502b4a..03e3023655407dc437c3ab89ebb4d8e4c3509a76 100644
--- a/src/boards/Payload/Sensors/Sensors.h
+++ b/src/boards/Payload/Sensors/Sensors.h
@@ -90,6 +90,9 @@ public:
      */
     bool saveMagCalibration();
 
+    void setBaroCalibrationReference(float reference);
+    void resetBaroCalibrationReference();
+
     Boardcore::LPS22DFData getLPS22DFLastSample();
     Boardcore::LPS28DFWData getLPS28DFWLastSample();
     Boardcore::H3LIS331DLData getH3LIS331DLLastSample();
@@ -185,6 +188,10 @@ private:
 
     bool sensorManagerInit();
 
+    miosix::FastMutex baroCalibrationMutex;
+    float baroCalibrationReference   = 0;
+    bool useBaroCalibrationReference = false;
+
     miosix::FastMutex magCalibrationMutex;
     Boardcore::SoftAndHardIronCalibration magCalibrator;
     Boardcore::SixParametersCorrector magCalibration;