diff --git a/pico-sensor/src/application.c b/pico-sensor/src/application.c
index 16268fe..d118256 100644
--- a/pico-sensor/src/application.c
+++ b/pico-sensor/src/application.c
@@ -136,6 +136,10 @@ static void MqttTask(void *pv) {
   } /* for */
 }
 
+static void MyMqttMessageCallback(topic_ID_e topic, const unsigned char *payload, size_t len) {
+  McuLog_info("MQTT message received, topic: %d, payload: %.*s", topic, (int)len, payload);
+}
+
 #endif
 
 void App_Init(void) {
@@ -154,6 +158,7 @@ void App_Init(void) {
   }
 #endif
 #if PL_CONFIG_USE_MQTT_CLIENT
+  MqttClient_RegisterMessageCallback(MyMqttMessageCallback); // register the callback for incoming messages
   if (xTaskCreate(
       MqttTask,  /* pointer to the task */
       "mqtt", /* task name for kernel awareness debugging */
diff --git a/pico-sensor/src/mqtt_client.c b/pico-sensor/src/mqtt_client.c
index 32fd1f3..c9856e4 100644
--- a/pico-sensor/src/mqtt_client.c
+++ b/pico-sensor/src/mqtt_client.c
@@ -39,20 +39,6 @@
   #define TOPIC_NAME_CHARGER_CHARGING_POWER "home/charger/power"
 #endif
 
-typedef enum topic_ID_e {
-  Topic_ID_None,
-#if MQTT_CLIENT_IS_EV_CHARGER
-  Topic_ID_Solar_Power,       /* power from PV panels */
-  Topic_ID_Site_Power,        /* power to the house/site */
-  Topic_ID_Grid_Power,        /* power from/to grid */
-  Topic_ID_Battery_Power,     /* power from/to battery */
-  Topic_ID_Battery_Percentage,/* battery level percentage */
-  Topic_ID_Charging_Power,     /* actual charging power */
-#elif MQTT_CLIENT_IS_SENSOR
-  Topic_ID_Sensor_Update,
-#endif
-} topic_ID_e;
-
 /* default entries for the MQTT broker connection */
 #define MQTT_DEFAULT_BROKER   "homeassistant"
 #define MQTT_DEFAULT_CLIENT   "client"
@@ -91,6 +77,13 @@ static const struct mqtt_connect_client_info_t mqtt_client_info = {
 #endif
 };
 
+// Static variable to hold the callback pointer
+static MqttClient_MessageCallback_t app_message_callback = NULL;
+
+void MqttClient_RegisterMessageCallback(MqttClient_MessageCallback_t cb) {
+  app_message_callback = cb;
+}
+
 static void mqtt_publish_request_cb(void *arg, err_t err) {
 #if 0 && MQTT_EXTRA_LOGS
   const struct mqtt_connect_client_info_t *client_info = (const struct mqtt_connect_client_info_t*)arg;
@@ -275,8 +268,8 @@ static void mqtt_incoming_data_cb(void *arg, const u8_t *data, u16_t len, u8_t f
         McuLog_trace("bat%%: %s%%", buf);
       }
 #elif MQTT_CLIENT_IS_SENSOR
-    if (mqtt.in_pub_ID == Topic_ID_Sensor_Update) {
-      McuLog_trace("Sensor update");
+        if (app_message_callback != NULL) {
+          app_message_callback(mqtt.in_pub_ID, data, len);
 #endif
     } else {
       McuLog_trace("mqtt_incoming_data_cb: Ignoring payload...");
@@ -381,7 +374,15 @@ static void mqtt_connection_cb(mqtt_client_t *client, void *arg, mqtt_connection
       McuLog_error("failed subscribing, err %d", err);
     }
 #elif MQTT_CLIENT_IS_SENSOR
-  	/* no subscriptions */
+  	  err = mqtt_sub_unsub(client,
+            "<user>/<room>/<device>/command/new_measure",
+            1, /* quality of service */
+            mqtt_request_cb, // Callback to call when subscribe/unsubscribe response is received
+            LWIP_CONST_CAST(void*, client_info),
+            1); // 1 for subscribe, 0 for unsubscribe
+    if (err!=ERR_OK) {
+      McuLog_error("failed subscribing, err %d", err);
+    }
 #endif
   } else if (status==MQTT_CONNECT_DISCONNECTED) {
     McuLog_trace("MQTT connect disconnect");
diff --git a/pico-sensor/src/mqtt_client.h b/pico-sensor/src/mqtt_client.h
index 4215f7c..b1bf130 100644
--- a/pico-sensor/src/mqtt_client.h
+++ b/pico-sensor/src/mqtt_client.h
@@ -70,6 +70,32 @@ void MqttClient_Deinit(void);
  */
 void MqttClient_Init(void);
 
+typedef enum topic_ID_e {
+  Topic_ID_None,
+#if MQTT_CLIENT_IS_EV_CHARGER
+  Topic_ID_Solar_Power,       /* power from PV panels */
+  Topic_ID_Site_Power,        /* power to the house/site */
+  Topic_ID_Grid_Power,        /* power from/to grid */
+  Topic_ID_Battery_Power,     /* power from/to battery */
+  Topic_ID_Battery_Percentage,/* battery level percentage */
+  Topic_ID_Charging_Power,     /* actual charging power */
+#elif MQTT_CLIENT_IS_SENSOR
+  Topic_ID_Sensor_Update,
+#endif
+} topic_ID_e;
+
+/*!
+ * \brief Callback function for incoming messages
+ */
+typedef void (*MqttClient_MessageCallback_t)(topic_ID_e topic, const unsigned char *payload, size_t len);
+
+/*!
+ * \brief Register a callback function for incoming messages
+ * \param cb Callback function to be registered
+ */
+void MqttClient_RegisterMessageCallback(MqttClient_MessageCallback_t cb);
+
+
 #ifdef __cplusplus
 }  /* extern "C" */
 #endif