diff options
Diffstat (limited to 'src/drivers')
-rw-r--r-- | src/drivers/md25/BlockSysIdent.cpp | 8 | ||||
-rw-r--r-- | src/drivers/md25/BlockSysIdent.hpp | 10 | ||||
-rw-r--r-- | src/drivers/md25/md25.cpp | 33 |
3 files changed, 48 insertions, 3 deletions
diff --git a/src/drivers/md25/BlockSysIdent.cpp b/src/drivers/md25/BlockSysIdent.cpp new file mode 100644 index 000000000..23b0724d8 --- /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 000000000..270635f40 --- /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 4e7e2694a..13d5c7eeb 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; |