From 42f09c4b547052d9fe2ef49f40a2df6910cf75b1 Mon Sep 17 00:00:00 2001
From: James Goppert <james.goppert@gmail.com>
Date: Sat, 22 Jun 2013 13:41:38 -0400
Subject: [PATCH] Working on sysid. Added debug values.

---
 src/drivers/md25/BlockSysIdent.cpp |  8 ++++++++
 src/drivers/md25/BlockSysIdent.hpp | 10 +++++++++
 src/drivers/md25/md25.cpp          | 33 +++++++++++++++++++++++++++---
 3 files changed, 48 insertions(+), 3 deletions(-)
 create mode 100644 src/drivers/md25/BlockSysIdent.cpp
 create mode 100644 src/drivers/md25/BlockSysIdent.hpp

diff --git a/src/drivers/md25/BlockSysIdent.cpp b/src/drivers/md25/BlockSysIdent.cpp
new file mode 100644
index 0000000000..23b0724d8c
--- /dev/null
+++ b/src/drivers/md25/BlockSysIdent.cpp
@@ -0,0 +1,8 @@
+#include "BlockSysIdent.hpp"
+
+BlockSysIdent::BlockSysIdent() :
+	Block(NULL, "SYSID"),
+	_freq(this, "FREQ"),
+	_ampl(this, "AMPL")
+{
+}
diff --git a/src/drivers/md25/BlockSysIdent.hpp b/src/drivers/md25/BlockSysIdent.hpp
new file mode 100644
index 0000000000..270635f407
--- /dev/null
+++ b/src/drivers/md25/BlockSysIdent.hpp
@@ -0,0 +1,10 @@
+#include <controllib/block/Block.hpp>
+#include <controllib/block/BlockParam.hpp>
+
+class BlockSysIdent : public control::Block {
+public:
+	BlockSysIdent();
+private:
+	control::BlockParam<float> _freq;
+	control::BlockParam<float> _ampl;
+};
diff --git a/src/drivers/md25/md25.cpp b/src/drivers/md25/md25.cpp
index 4e7e2694a6..13d5c7eeb3 100644
--- a/src/drivers/md25/md25.cpp
+++ b/src/drivers/md25/md25.cpp
@@ -46,11 +46,16 @@
 #include <poll.h>
 #include <stdio.h>
 #include <math.h>
+#include <string.h>
 
 #include <systemlib/err.h>
 #include <arch/board/board.h>
 #include <mavlink/mavlink_log.h>
 
+#include <controllib/block/UOrbPublication.hpp>
+#include <uORB/topics/debug_key_value.h>
+#include <drivers/drv_hrt.h>
+
 // registers
 enum {
 	// RW: read/write
@@ -572,17 +577,39 @@ int md25Sine(const char *deviceName, uint8_t bus, uint8_t address)
 	md25.setTimeout(true);
 	float dt = 0.1;
 	float amplitude = 0.2;
-	float t = 0;
 	float omega = 0.1;
 
+	// input signal
+	control::UOrbPublication<debug_key_value_s> input_signal(NULL,
+			ORB_ID(debug_key_value));
+	strncpy(input_signal.key, "md25 in   ", 10);
+	input_signal.timestamp_ms = hrt_absolute_time();
+	input_signal.value = 0;
+
+	// output signal
+	control::UOrbPublication<debug_key_value_s> output_signal(NULL,
+			ORB_ID(debug_key_value));
+	strncpy(output_signal.key, "md25 out  ", 10);
+	output_signal.timestamp_ms = hrt_absolute_time();
+	output_signal.value = 0;
+
 	// sine wave for motor 1
 	md25.resetEncoders();
 	while (true) {
 		float prev_revolution = md25.getRevolutions1();
-		md25.setMotor1Speed(amplitude*sinf(omega*t));
 		usleep(1000000 * dt);
-		t += dt;
+
+		// input
+		uint64_t timestamp = hrt_absolute_time();
+		float t = timestamp/1000;
+		input_signal.timestamp_ms = timestamp;
+		input_signal.value = amplitude*sinf(omega*t);
+		md25.setMotor1Speed(input_signal.value);
+
+		// output
+		output_signal.timestamp_ms = timestamp;
 		float speed_rpm = 60*(md25.getRevolutions1() - prev_revolution)/dt;
+		output_signal.value = speed_rpm;
 		mavlink_log_info(mavlink_fd, "rpm: %10.4f\n", (double)speed_rpm);
 		md25.readData();
 		if (t > 2.0f) break;