diff --git a/arduino/mads/mads.ino b/arduino/mads/mads.ino
index c619433..f5ca61f 100644
--- a/arduino/mads/mads.ino
+++ b/arduino/mads/mads.ino
@@ -6,12 +6,16 @@
 #define CURRENT_Z A2
 #define CURRENT_B A3
 #define CURRENT_C A4
-#define DELAY 50UL     // microseconds
+#define DELAY 40UL     // microseconds
 #define TIMESTEP 160UL // milliseconds
 #define DATA_FIELD "data"
 
+#define limit(v, t, fV, fA) (((v * fV) < t ? 0 : v * fV) * fA)
+
 JsonDocument doc;
 String out;
+const double to_V = 5.0 / 1024.0;
+const double to_A = 20.0 / 2.8;
 
 void setup() {
   // put your setup code here, to run once:
@@ -22,24 +26,84 @@ void setup() {
 
 void loop() {
   static unsigned long prev_time = 0;
-  static const unsigned long timestep_us = TIMESTEP * 1000;
-  static const float factor = 1.0 / (1024.0 * 20.0);
-  static bool onoff = LOW;
+  static unsigned long timestep_us = TIMESTEP * 1000;
+  static unsigned long delay = DELAY;
+  static unsigned int threshold_mV = 280;
+  static bool onoff = LOW, pause = false, raw = false;
   unsigned long now = micros();
+  static unsigned long v = 0; // accumulator for serial values
+  char ch;
+
+  // Read serial in
+  if (Serial.available()) {
+    ch = Serial.read();
+    switch (ch) {
+      case '0'...'9':
+        v = v * 10 + ch - '0';
+        break;
+      case 'p':
+        timestep_us = constrain(v * 1000, 1000, 1E6);
+        v = 0;
+        break;
+      case 'd':
+        delay = constrain(v, 1, timestep_us / 10.0);
+        v = 0;
+        break;
+      case 't':
+        threshold_mV = constrain(v, 0, 5000);
+        v = 0;
+        break;
+      case 'x':
+        pause = !pause;
+        break;
+      case 'r':
+        raw = !raw;
+        break;
+      case '?':
+        Serial.print("Usage:\n");
+        Serial.print("- 10p  set sampling period to 10 milliseconds (now ");
+        Serial.print(timestep_us / 1000);
+        Serial.print(" ms)\n- 30d  set loop delay to 30 microseconds (now ");
+        Serial.print(delay);
+        Serial.print(" us)\n- 280t set threshold to 0.28V (now ");
+        Serial.print(threshold_mV);
+        Serial.print(" mV)\n");
+        Serial.print("- x    toggle pause\n");
+        Serial.print("- r    toggle raw output\n");
+        break;
+      default:
+        v = 0;
+    }
+  }
+
+  if (pause) return;
   if (now - prev_time >= timestep_us) {
     digitalWrite(LED_BUILTIN, onoff);
     onoff = !onoff;
     doc["millis"] = millis();
-    doc[DATA_FIELD]["AX"] = analogRead(CURRENT_X) * factor;
-    doc[DATA_FIELD]["AY"] = analogRead(CURRENT_Y) * factor;
-    doc[DATA_FIELD]["AZ"] = analogRead(CURRENT_Z) * factor;
-    doc[DATA_FIELD]["AB"] = analogRead(CURRENT_B) * factor;
-    doc[DATA_FIELD]["AC"] = analogRead(CURRENT_C) * factor;
-    serializeJson(doc, out);
-    Serial.print(out);
-    Serial.print("\n");
+    doc[DATA_FIELD]["AX"] = limit(analogRead(CURRENT_X), threshold_mV / 1000.0, to_V, to_A);
+    doc[DATA_FIELD]["AY"] = limit(analogRead(CURRENT_Y), threshold_mV / 1000.0, to_V, to_A);
+    doc[DATA_FIELD]["AZ"] = limit(analogRead(CURRENT_Z), threshold_mV / 1000.0, to_V, to_A);
+    doc[DATA_FIELD]["AB"] = limit(analogRead(CURRENT_B), threshold_mV / 1000.0, to_V, to_A);
+    doc[DATA_FIELD]["AC"] = limit(analogRead(CURRENT_C), threshold_mV / 1000.0, to_V, to_A);
+    if (raw) {
+      Serial.print(doc[DATA_FIELD]["AX"].as<double>());
+      Serial.print(" "); 
+      Serial.print(doc[DATA_FIELD]["AY"].as<double>());
+      Serial.print(" "); 
+      Serial.print(doc[DATA_FIELD]["AZ"].as<double>());
+      Serial.print(" "); 
+      Serial.print(doc[DATA_FIELD]["AB"].as<double>());
+      Serial.print(" "); 
+      Serial.print(doc[DATA_FIELD]["AC"].as<double>());
+      Serial.print("\n");
+    } else {
+      serializeJson(doc, out);
+      Serial.print(out);
+      Serial.print("\n");
+    }
     prev_time = now;
   }
-  delayMicroseconds(DELAY);
+  delayMicroseconds(delay);
 }
 
diff --git a/src/plugin/serial_reader.cpp b/src/plugin/serial_reader.cpp
index 99691a6..074aca4 100644
--- a/src/plugin/serial_reader.cpp
+++ b/src/plugin/serial_reader.cpp
@@ -51,9 +51,6 @@ class SerialReader : public Source<json> {
     string line;
     bool success = false;
     out.clear();
-    if (setup() != return_type::success) {
-      return return_type::critical;
-    }
     do {
       line.clear();
       _serialPort->readLine(line);
@@ -73,12 +70,20 @@ class SerialReader : public Source<json> {
     _params["port"] = "/dev/ttyUSB0";
     _params["baudrate"] = 115200;
     _params.merge_patch(*(json *)params);
+    if (setup() != return_type::success) {
+      throw std::runtime_error("Error setting up serial port");
+    }
+    if (_params.find("cfg_cmd") != _params.end()) {
+      _serialPort->write(_params["cfg_cmd"].get<string>().c_str());
+      _serialPort->write("\n");
+    }
   }
 
   map<string, string> info() override {
     return {
       {"port", _params["port"].get<string>()},
-      {"baudrate", to_string(_params["baudrate"].get<unsigned>())}
+      {"baudrate", to_string(_params["baudrate"].get<unsigned>())},
+      {"cfg_cmd", _params["cfg_cmd"].get<string>()}
     };
   };