diff options
author | James Goppert <james.goppert@gmail.com> | 2013-06-07 14:02:18 -0400 |
---|---|---|
committer | James Goppert <james.goppert@gmail.com> | 2013-07-28 00:05:56 -0400 |
commit | f3bfbd87b1f6faef6bac75c9f94b590bb8b504b6 (patch) | |
tree | 4ef34188976c763c865abd8a237a4ce0dba606a6 /src | |
parent | a8ac56b9e5eb8c1501ea592b4417aa8becd7236c (diff) | |
download | px4-firmware-f3bfbd87b1f6faef6bac75c9f94b590bb8b504b6.tar.gz px4-firmware-f3bfbd87b1f6faef6bac75c9f94b590bb8b504b6.tar.bz2 px4-firmware-f3bfbd87b1f6faef6bac75c9f94b590bb8b504b6.zip |
Added sine test.
Diffstat (limited to 'src')
-rw-r--r-- | src/drivers/md25/md25.cpp | 38 | ||||
-rw-r--r-- | src/drivers/md25/md25.hpp | 3 | ||||
-rw-r--r-- | src/drivers/md25/md25_main.cpp | 18 |
3 files changed, 59 insertions, 0 deletions
diff --git a/src/drivers/md25/md25.cpp b/src/drivers/md25/md25.cpp index 71932ad65..f9f5d70ab 100644 --- a/src/drivers/md25/md25.cpp +++ b/src/drivers/md25/md25.cpp @@ -45,6 +45,7 @@ #include "md25.hpp" #include <poll.h> #include <stdio.h> +#include <math.h> #include <systemlib/err.h> #include <arch/board/board.h> @@ -550,4 +551,41 @@ int md25Test(const char *deviceName, uint8_t bus, uint8_t address) return 0; } +int md25Sine(const char *deviceName, uint8_t bus, uint8_t address) +{ + printf("md25 sine: starting\n"); + + // setup + MD25 md25("/dev/md25", bus, address); + + // print status + char buf[200]; + md25.status(buf, sizeof(buf)); + printf("%s\n", buf); + + // setup for test + md25.setSpeedRegulation(true); + md25.setTimeout(true); + float dt = 0.1; + float amplitude = 0.2; + float t = 0; + float omega = 0.1; + + // 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; + float speed_rpm = 60*(md25.getRevolutions1() - prev_revolution)/dt; + md25.readData(); + if (t > 2.0f) break; + } + md25.setMotor1Speed(0); + + printf("md25 sine complete\n"); + return 0; +} + // vi:noet:smarttab:autoindent:ts=4:sw=4:tw=78 diff --git a/src/drivers/md25/md25.hpp b/src/drivers/md25/md25.hpp index e77511b16..cac3ffd29 100644 --- a/src/drivers/md25/md25.hpp +++ b/src/drivers/md25/md25.hpp @@ -290,4 +290,7 @@ private: // unit testing int md25Test(const char *deviceName, uint8_t bus, uint8_t address); +// sine testing +int md25Sine(const char *deviceName, uint8_t bus, uint8_t address); + // 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 e62c46b0d..701452f2d 100644 --- a/src/drivers/md25/md25_main.cpp +++ b/src/drivers/md25/md25_main.cpp @@ -136,6 +136,24 @@ int md25_main(int argc, char *argv[]) exit(0); } + if (!strcmp(argv[1], "sine")) { + + if (argc < 4) { + printf("usage: md25 sine bus address\n"); + exit(0); + } + + const char *deviceName = "/dev/md25"; + + uint8_t bus = strtoul(argv[2], nullptr, 0); + + uint8_t address = strtoul(argv[3], nullptr, 0); + + md25Sine(deviceName, bus, address); + + exit(0); + } + if (!strcmp(argv[1], "probe")) { if (argc < 4) { printf("usage: md25 probe bus address\n"); |