diff --git a/src/drivers/md25/md25.cpp b/src/drivers/md25/md25.cpp
index 9dac5e5ea8..1079005c05 100644
--- a/src/drivers/md25/md25.cpp
+++ b/src/drivers/md25/md25.cpp
@@ -560,7 +560,7 @@ int md25Test(const char *deviceName, uint8_t bus, uint8_t address)
 	return 0;
 }
 
-int md25Sine(const char *deviceName, uint8_t bus, uint8_t address)
+int md25Sine(const char *deviceName, uint8_t bus, uint8_t address, float amplitude, float frequency)
 {
 	printf("md25 sine: starting\n");
 
@@ -576,9 +576,7 @@ int md25Sine(const char *deviceName, uint8_t bus, uint8_t address)
 	md25.setSpeedRegulation(true);
 	md25.setTimeout(true);
 	float dt = 0.01;
-	float amplitude = 0.5;
-	float frequency = 1.0;
-	float t_final = 120.0;
+	float t_final = 60.0;
 	float prev_revolution = md25.getRevolutions1();
 
 	// debug publication
@@ -599,8 +597,6 @@ int md25Sine(const char *deviceName, uint8_t bus, uint8_t address)
 		// output
 		md25.readData();
 		float current_revolution = md25.getRevolutions1();
-		float output_speed_rpm = 60*(current_revolution - prev_revolution)/dt;
-		mavlink_log_info(mavlink_fd, "rpm: %10.4f\n", (double)output_speed_rpm);
 
 		// send input message
 		//strncpy(debug_msg.key, "md25 in   ", 10);
@@ -611,8 +607,7 @@ int md25Sine(const char *deviceName, uint8_t bus, uint8_t address)
 		// send output message
 		strncpy(debug_msg.key, "md25 out  ", 10);
 		debug_msg.timestamp_ms = 1000*timestamp;
-		debug_msg.value = current_revolution - prev_revolution;
-		//debug_msg.value = output_speed_rpm;
+		debug_msg.value = current_revolution;
 		debug_msg.update();
 
 		if (t > t_final) break;
diff --git a/src/drivers/md25/md25.hpp b/src/drivers/md25/md25.hpp
index cac3ffd293..2fc317f5e4 100644
--- a/src/drivers/md25/md25.hpp
+++ b/src/drivers/md25/md25.hpp
@@ -291,6 +291,6 @@ private:
 int md25Test(const char *deviceName, uint8_t bus, uint8_t address);
 
 // sine testing
-int md25Sine(const char *deviceName, uint8_t bus, uint8_t address);
+int md25Sine(const char *deviceName, uint8_t bus, uint8_t address, float amplitude, float frequency);
 
 // vi:noet:smarttab:autoindent:ts=4:sw=4:tw=78
diff --git a/src/drivers/md25/md25_main.cpp b/src/drivers/md25/md25_main.cpp
index 701452f2d5..b395088a38 100644
--- a/src/drivers/md25/md25_main.cpp
+++ b/src/drivers/md25/md25_main.cpp
@@ -139,7 +139,7 @@ int md25_main(int argc, char *argv[])
 	if (!strcmp(argv[1], "sine")) {
 
 		if (argc < 4) {
-			printf("usage: md25 sine bus address\n");
+			printf("usage: md25 sine bus address amp freq\n");
 			exit(0);
 		}
 
@@ -149,7 +149,11 @@ int md25_main(int argc, char *argv[])
 
 		uint8_t address = strtoul(argv[3], nullptr, 0);
 
-		md25Sine(deviceName, bus, address);
+		float amplitude = atof(argv[4]);
+
+		float frequency = atof(argv[5]);
+
+		md25Sine(deviceName, bus, address, amplitude, frequency);
 
 		exit(0);
 	}