diff --git a/mos.yml b/mos.yml
index 4f0b6ca..07b616a 100644
--- a/mos.yml
+++ b/mos.yml
@@ -33,11 +33,12 @@ config_schema:
   - ["i2c.scl_gpio", 5] # D1
   - ["flashLight", "o", {title: "Flash light / alarm light settings"}]
   - ["flashLight.address", "i", 0x30, {title: "i2c address of motor controller TB6612 (e.g. WEMOS)"}]
+  - ["flashLight.motorFrequency", "i", 100, {title: "Frequency of PWM in kHz"}]
   - ["flashLight.mqttCtrlTopic", "s", "flashLight/%s/ctrl", {title: "MQTT channel to subscribe to receive commands; %s is replaced by clientId"}]
   - ["flashLight.mqttStatusTopic", "s", "flashLight/%s/status", {title: "MQTT channel to publish to send status change infos; %s is replaced by clientId"}]
 
   - ["mqtt.enable", true]
-  - ["mqtt.server", "mqtt.pmpark.de:41883"]
+  - ["mqtt.server", "mqtt.pmpark.de:1883"]
   - ["mqtt.user", "default"]
   - ["mqtt.pass", "12345678"]
   - ["mqtt.will_message", "offline"]
diff --git a/src/main.c b/src/main.c
index c28ac7b..a76f443 100644
--- a/src/main.c
+++ b/src/main.c
@@ -21,6 +21,7 @@ static char commandTopic[256] = {'\0'};
 #if 0
 #endif
 static char pubStatusTopic[256] = {'\0'};
+static bool mqttConnected = false;
 
 // Motor numbers
 //Motor shiled I2C Address: 0x30
@@ -28,11 +29,15 @@ static char pubStatusTopic[256] = {'\0'};
 #define M1 0
 #define M1_addr 0x30
 #define M1_freq 5000
+static uint8_t motorAddress = M1_addr;
+static uint32_t motorFrequency = M1_freq;
 
 static void pubStatus(const char *statusString, float percentage) {
   struct mbuf fb;
   struct json_out out = JSON_OUT_MBUF(&fb);
 
+  if (!mqttConnected) return;
+
   mbuf_init(&fb, 30);
   if (mgos_mqtt_global_connect()) {
     json_printf(&out, "{statusString: %Q, speed: %f}", statusString, percentage);
@@ -42,17 +47,22 @@ static void pubStatus(const char *statusString, float percentage) {
 }
 
 static void motor_timer_cb(void *arg) {
+  static bool stopped = false;
+
   if (pwm > flashLightSpeed + 5.0) {
     pwm = 0.0; // start again
+    stopped = false;
   } else {
+    pwm += 0.1;
+    if (stopped) return;
+
     wemos_motor_setmotor(M1, _CW, pwm);
     LOG(LL_INFO, ("M1, pwm=%f", pwm));
     pubStatus("on", pwm);
 
-    pwm += 0.1;
-
     if (pwm > flashLightSpeed) {
       wemos_motor_setmotor(M1, _STOP, 0.0);
+      stopped = true;
       LOG(LL_INFO, ("Stopped"));
       pubStatus("off", 0);
     }
@@ -155,16 +165,8 @@ void net_changed(int ev, void *evd, void *arg) {
   (void) arg;
 }
 
-void onMqttConnection(struct mg_connection *c, const char *client_id, struct mg_send_mqtt_handshake_opts *opts, void *fn_arg) {
-  // add MQTT cmd subscription
-  LOG(LL_INFO, ("onMqttConnection handler called with clientId=%s", client_id));
-
-  (void) c;
-  (void) client_id;
-  (void) opts;
-  (void) fn_arg;
-}
 #if 0
+#endif
 static void mqttCommandHandler(struct mg_connection *c, const char *topic, int topic_len,
                     const char *msg, int msg_len, void *userdata) {
   LOG(LL_INFO, ("Got message on topic %.*s", topic_len, topic));
@@ -176,16 +178,31 @@ static void mqttCommandHandler(struct mg_connection *c, const char *topic, int t
   (void) msg_len;
   (void) userdata;
 }
-#endif
+
+// void onMqttConnection(struct mg_connection *c, const char *client_id, struct mg_send_mqtt_handshake_opts *opts, void *fn_arg) {
+void onMqttConnection(struct mg_connection *c, const char *client_id, struct mg_send_mqtt_handshake_opts *opts, void *fn_arg) {
+  // add MQTT cmd subscription
+  LOG(LL_INFO, ("onMqttConnection handler called with clientId=%s", client_id));
+  #if 0
+  #endif
+  mgos_mqtt_sub(commandTopic, mqttCommandHandler, NULL);
+  mqttConnected = true;
+
+  (void) c;
+  (void) client_id;
+  (void) opts;
+  (void) fn_arg;
+}
+
 enum mgos_app_init_result mgos_app_init(void) {
   struct mg_rpc *c = mgos_rpc_get_global();
   mg_rpc_add_handler(c, "FlashLight.On", NULL, flashLightOn, NULL);
   mg_rpc_add_handler(c, "FlashLight.Off", NULL, flashLightOff, NULL);
   mg_rpc_add_handler(c, "FlashLight.Speed", "{num: %d}", flashLightSetSpeed, NULL);
   mgos_event_add_group_handler(MGOS_EVENT_GRP_NET, net_changed, NULL);
-  LOG(LL_INFO, ("Initializing MQTT"));
+
   // add MQTT cmd subscription
-  // mgos_mqtt_set_connect_fn(onMqttConnection, NULL);
+  LOG(LL_INFO, ("Initializing MQTT"));
   clientId = mgos_sys_config_get_mqtt_client_id();
   clientId = clientId ? clientId : mgos_sys_config_get_device_id();
   LOG(LL_INFO, ("clientId=%s", clientId));
@@ -193,13 +210,14 @@ enum mgos_app_init_result mgos_app_init(void) {
   LOG(LL_INFO, ("pubStatusTopic=%s", mgos_sys_config_get_flashLight_mqttStatusTopic()));
   c_snprintf(commandTopic, sizeof(commandTopic), mgos_sys_config_get_flashLight_mqttCtrlTopic(), clientId);
   c_snprintf(pubStatusTopic, sizeof(pubStatusTopic), mgos_sys_config_get_flashLight_mqttStatusTopic(), clientId);
-  #if 0
-  mgos_mqtt_sub(commandTopic, mqttCommandHandler, NULL);
-  #endif
+  mgos_mqtt_set_connect_fn(onMqttConnection, NULL);
+
   LOG(LL_INFO, ("Initializing motor controller"));
+  motorFrequency = mgos_sys_config_get_flashLight_motorFrequency();
+  motorAddress = mgos_sys_config_get_flashLight_address();
   wemos_motor_init();
-  wemos_motor_initMotor(M1, M1_addr, M1_freq);
-  mgos_set_timer(200, MGOS_TIMER_REPEAT, motor_timer_cb, NULL);
+  wemos_motor_initMotor(M1, motorAddress, motorFrequency);
+  mgos_set_timer(100, MGOS_TIMER_REPEAT, motor_timer_cb, NULL);
   return MGOS_APP_INIT_SUCCESS;
 }