diff --git a/src/MqttClient.cpp b/src/MqttClient.cpp
index 1f7539f..bce0d02 100644
--- a/src/MqttClient.cpp
+++ b/src/MqttClient.cpp
@@ -104,7 +104,11 @@ MqttClient::~MqttClient()
   }
 }
 
+#ifdef MQTT_CLIENT_STD_FUNCTION_CALLBACK
+void MqttClient::onMessage(MessageCallback callback)
+#else
 void MqttClient::onMessage(void(*callback)(int))
+#endif
 {
   _onMessage = callback;
 }
@@ -557,7 +561,11 @@ void MqttClient::poll()
             _rxState = MQTT_CLIENT_RX_STATE_READ_PUBLISH_PAYLOAD;
 
             if (_onMessage) {
+#ifdef MQTT_CLIENT_STD_FUNCTION_CALLBACK
+              _onMessage(this,_rxLength);
+#else
               _onMessage(_rxLength);
+#endif
 
               if (_rxLength == 0) {
                 _rxState = MQTT_CLIENT_RX_STATE_READ_TYPE;
@@ -580,7 +588,11 @@ void MqttClient::poll()
           _rxState = MQTT_CLIENT_RX_STATE_READ_PUBLISH_PAYLOAD;
 
           if (_onMessage) {
+#ifdef MQTT_CLIENT_STD_FUNCTION_CALLBACK
+            _onMessage(this,_rxLength);
+#else
             _onMessage(_rxLength);
+#endif
           }
 
           if (_rxLength == 0) {
diff --git a/src/MqttClient.h b/src/MqttClient.h
index de19a98..a94b9c9 100644
--- a/src/MqttClient.h
+++ b/src/MqttClient.h
@@ -32,17 +32,26 @@
 #define MQTT_BAD_USER_NAME_OR_PASSWORD      4
 #define MQTT_NOT_AUTHORIZED                 5
 
+// Make this definition in your application code to use std::functions for onMessage callbacks instead of C-pointers:
+// #define MQTT_CLIENT_STD_FUNCTION_CALLBACK
+
+#ifdef MQTT_CLIENT_STD_FUNCTION_CALLBACK
+#include <functional>
+#endif
+
 class MqttClient : public Client {
 public:
   MqttClient(Client* client);
   MqttClient(Client& client);
   virtual ~MqttClient();
 
-
+#ifdef MQTT_CLIENT_STD_FUNCTION_CALLBACK
+  typedef std::function<void(MqttClient *client, int messageSize)> MessageCallback;
+  void onMessage(MessageCallback callback);
+#else
   inline void setClient(Client& client) { _client = &client; }
-
-
   void onMessage(void(*)(int));
+#endif
 
   int parseMessage();
   String messageTopic() const;
@@ -133,7 +142,11 @@ class MqttClient : public Client {
 private:
   Client* _client;
 
+#ifdef MQTT_CLIENT_STD_FUNCTION_CALLBACK
+  MessageCallback _onMessage;
+#else
   void (*_onMessage)(int);
+#endif
 
   String _id;
   String _username;