From f3bfbd87b1f6faef6bac75c9f94b590bb8b504b6 Mon Sep 17 00:00:00 2001 From: James Goppert Date: Fri, 7 Jun 2013 14:02:18 -0400 Subject: [PATCH 01/18] Added sine test. --- src/drivers/md25/md25.cpp | 38 ++++++++++++++++++++++++++++++++++ src/drivers/md25/md25.hpp | 3 +++ src/drivers/md25/md25_main.cpp | 18 ++++++++++++++++ 3 files changed, 59 insertions(+) diff --git a/src/drivers/md25/md25.cpp b/src/drivers/md25/md25.cpp index 71932ad65d..f9f5d70ab5 100644 --- a/src/drivers/md25/md25.cpp +++ b/src/drivers/md25/md25.cpp @@ -45,6 +45,7 @@ #include "md25.hpp" #include #include +#include #include #include @@ -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 e77511b163..cac3ffd293 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 e62c46b0d7..701452f2d5 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"); From 764310620837461857d511144738a521e3840f97 Mon Sep 17 00:00:00 2001 From: James Goppert Date: Sat, 15 Jun 2013 16:37:15 -0400 Subject: [PATCH 02/18] Added log print ability to md25 driver. --- src/drivers/md25/md25.cpp | 5 +++++ 1 file changed, 5 insertions(+) diff --git a/src/drivers/md25/md25.cpp b/src/drivers/md25/md25.cpp index f9f5d70ab5..4e7e2694a6 100644 --- a/src/drivers/md25/md25.cpp +++ b/src/drivers/md25/md25.cpp @@ -49,6 +49,7 @@ #include #include +#include // registers enum { @@ -73,6 +74,9 @@ enum { REG_COMMAND_RW, }; +// File descriptors +static int mavlink_fd; + MD25::MD25(const char *deviceName, int bus, uint16_t address, uint32_t speed) : I2C("MD25", deviceName, bus, address, speed), @@ -579,6 +583,7 @@ int md25Sine(const char *deviceName, uint8_t bus, uint8_t address) usleep(1000000 * dt); t += dt; float speed_rpm = 60*(md25.getRevolutions1() - prev_revolution)/dt; + mavlink_log_info(mavlink_fd, "rpm: %10.4f\n", (double)speed_rpm); md25.readData(); if (t > 2.0f) break; } From 42f09c4b547052d9fe2ef49f40a2df6910cf75b1 Mon Sep 17 00:00:00 2001 From: James Goppert Date: Sat, 22 Jun 2013 13:41:38 -0400 Subject: [PATCH 03/18] 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 +#include + +class BlockSysIdent : public control::Block { +public: + BlockSysIdent(); +private: + control::BlockParam _freq; + control::BlockParam _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 #include #include +#include #include #include #include +#include +#include +#include + // 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 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 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; From e7cc6e71ad5d53d940a0e5c6961e5ea6c3a59e27 Mon Sep 17 00:00:00 2001 From: James Goppert Date: Sat, 22 Jun 2013 13:45:12 -0400 Subject: [PATCH 04/18] Added pub update. --- src/drivers/md25/md25.cpp | 2 ++ 1 file changed, 2 insertions(+) diff --git a/src/drivers/md25/md25.cpp b/src/drivers/md25/md25.cpp index 13d5c7eeb3..582b871c78 100644 --- a/src/drivers/md25/md25.cpp +++ b/src/drivers/md25/md25.cpp @@ -605,11 +605,13 @@ int md25Sine(const char *deviceName, uint8_t bus, uint8_t address) input_signal.timestamp_ms = timestamp; input_signal.value = amplitude*sinf(omega*t); md25.setMotor1Speed(input_signal.value); + input_signal.update(); // output output_signal.timestamp_ms = timestamp; float speed_rpm = 60*(md25.getRevolutions1() - prev_revolution)/dt; output_signal.value = speed_rpm; + output_signal.update(); mavlink_log_info(mavlink_fd, "rpm: %10.4f\n", (double)speed_rpm); md25.readData(); if (t > 2.0f) break; From f2a0cce958db1c97eb70d43c3151992ccaed4cab Mon Sep 17 00:00:00 2001 From: James Goppert Date: Sat, 29 Jun 2013 15:21:17 -0400 Subject: [PATCH 05/18] Fixed timing issues. --- src/drivers/md25/md25.cpp | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/src/drivers/md25/md25.cpp b/src/drivers/md25/md25.cpp index 582b871c78..7a1e7b7f47 100644 --- a/src/drivers/md25/md25.cpp +++ b/src/drivers/md25/md25.cpp @@ -577,7 +577,7 @@ int md25Sine(const char *deviceName, uint8_t bus, uint8_t address) md25.setTimeout(true); float dt = 0.1; float amplitude = 0.2; - float omega = 0.1; + float frequency = 0.3; // input signal control::UOrbPublication input_signal(NULL, @@ -601,9 +601,9 @@ int md25Sine(const char *deviceName, uint8_t bus, uint8_t address) // input uint64_t timestamp = hrt_absolute_time(); - float t = timestamp/1000; + float t = timestamp/1000000; input_signal.timestamp_ms = timestamp; - input_signal.value = amplitude*sinf(omega*t); + input_signal.value = amplitude*sinf(2*M_PI*frequency*t); md25.setMotor1Speed(input_signal.value); input_signal.update(); From 78ef6f5265d5f4e84d4e36be37fc7329587df0a3 Mon Sep 17 00:00:00 2001 From: James Goppert Date: Sat, 29 Jun 2013 15:31:23 -0400 Subject: [PATCH 06/18] Changed final time. --- src/drivers/md25/md25.cpp | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/src/drivers/md25/md25.cpp b/src/drivers/md25/md25.cpp index 7a1e7b7f47..375072bb03 100644 --- a/src/drivers/md25/md25.cpp +++ b/src/drivers/md25/md25.cpp @@ -578,6 +578,7 @@ int md25Sine(const char *deviceName, uint8_t bus, uint8_t address) float dt = 0.1; float amplitude = 0.2; float frequency = 0.3; + float t_final = 30.0; // input signal control::UOrbPublication input_signal(NULL, @@ -614,7 +615,7 @@ int md25Sine(const char *deviceName, uint8_t bus, uint8_t address) output_signal.update(); mavlink_log_info(mavlink_fd, "rpm: %10.4f\n", (double)speed_rpm); md25.readData(); - if (t > 2.0f) break; + if (t > t_final) break; } md25.setMotor1Speed(0); From 76d086e0773bda8252b9a59b737b902ddc52b59f Mon Sep 17 00:00:00 2001 From: James Goppert Date: Sat, 29 Jun 2013 15:58:58 -0400 Subject: [PATCH 07/18] Working with debug messages. --- src/drivers/md25/md25.cpp | 49 +++++++++++++++++++++------------------ 1 file changed, 26 insertions(+), 23 deletions(-) diff --git a/src/drivers/md25/md25.cpp b/src/drivers/md25/md25.cpp index 375072bb03..4cb005721c 100644 --- a/src/drivers/md25/md25.cpp +++ b/src/drivers/md25/md25.cpp @@ -579,43 +579,46 @@ int md25Sine(const char *deviceName, uint8_t bus, uint8_t address) float amplitude = 0.2; float frequency = 0.3; float t_final = 30.0; + float prev_revolution = md25.getRevolutions1(); - // input signal - control::UOrbPublication input_signal(NULL, + // debug publication + control::UOrbPublication debug_msg(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 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(); - usleep(1000000 * dt); // input uint64_t timestamp = hrt_absolute_time(); float t = timestamp/1000000; - input_signal.timestamp_ms = timestamp; - input_signal.value = amplitude*sinf(2*M_PI*frequency*t); - md25.setMotor1Speed(input_signal.value); - input_signal.update(); + + float input_value = amplitude*sinf(2*M_PI*frequency*t); + md25.setMotor1Speed(input_value); // output - output_signal.timestamp_ms = timestamp; - float speed_rpm = 60*(md25.getRevolutions1() - prev_revolution)/dt; - output_signal.value = speed_rpm; - output_signal.update(); - mavlink_log_info(mavlink_fd, "rpm: %10.4f\n", (double)speed_rpm); md25.readData(); + float current_revolution = md25.getRevolutions1(); + float output_speed_rpm = 60*(current_revolution - prev_revolution)/dt; + float prev_revolution = current_revolution; + mavlink_log_info(mavlink_fd, "rpm: %10.4f\n", (double)output_speed_rpm); + + // send input message + strncpy(debug_msg.key, "md25 in ", sizeof(10)); + debug_msg.timestamp_ms = 1000*timestamp; + debug_msg.value = input_value; + debug_msg.update(); + + // send output message + strncpy(debug_msg.key, "md25 out ", sizeof(10)); + debug_msg.timestamp_ms = 1000*timestamp; + debug_msg.value = output_speed_rpm;; + debug_msg.update(); + if (t > t_final) break; + + // sleep + usleep(1000000 * dt); } md25.setMotor1Speed(0); From 77c084a4cf6d5ac1131fae493230fcea2b11700c Mon Sep 17 00:00:00 2001 From: James Goppert Date: Sat, 29 Jun 2013 16:00:27 -0400 Subject: [PATCH 08/18] Fixed typo with strncpy. --- src/drivers/md25/md25.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/drivers/md25/md25.cpp b/src/drivers/md25/md25.cpp index 4cb005721c..f265ec4515 100644 --- a/src/drivers/md25/md25.cpp +++ b/src/drivers/md25/md25.cpp @@ -604,13 +604,13 @@ int md25Sine(const char *deviceName, uint8_t bus, uint8_t address) mavlink_log_info(mavlink_fd, "rpm: %10.4f\n", (double)output_speed_rpm); // send input message - strncpy(debug_msg.key, "md25 in ", sizeof(10)); + strncpy(debug_msg.key, "md25 in ", 10); debug_msg.timestamp_ms = 1000*timestamp; debug_msg.value = input_value; debug_msg.update(); // send output message - strncpy(debug_msg.key, "md25 out ", sizeof(10)); + strncpy(debug_msg.key, "md25 out ", 10); debug_msg.timestamp_ms = 1000*timestamp; debug_msg.value = output_speed_rpm;; debug_msg.update(); From 4cfcea176785975590d1d2952eb0b0270a6949b3 Mon Sep 17 00:00:00 2001 From: James Goppert Date: Sat, 29 Jun 2013 16:53:24 -0400 Subject: [PATCH 09/18] Working on debug output. --- src/drivers/md25/md25.cpp | 25 ++++++++++++++----------- 1 file changed, 14 insertions(+), 11 deletions(-) diff --git a/src/drivers/md25/md25.cpp b/src/drivers/md25/md25.cpp index f265ec4515..9dac5e5ea8 100644 --- a/src/drivers/md25/md25.cpp +++ b/src/drivers/md25/md25.cpp @@ -575,10 +575,10 @@ int md25Sine(const char *deviceName, uint8_t bus, uint8_t address) // setup for test md25.setSpeedRegulation(true); md25.setTimeout(true); - float dt = 0.1; - float amplitude = 0.2; - float frequency = 0.3; - float t_final = 30.0; + float dt = 0.01; + float amplitude = 0.5; + float frequency = 1.0; + float t_final = 120.0; float prev_revolution = md25.getRevolutions1(); // debug publication @@ -591,7 +591,7 @@ int md25Sine(const char *deviceName, uint8_t bus, uint8_t address) // input uint64_t timestamp = hrt_absolute_time(); - float t = timestamp/1000000; + float t = timestamp/1000000.0f; float input_value = amplitude*sinf(2*M_PI*frequency*t); md25.setMotor1Speed(input_value); @@ -600,23 +600,26 @@ int md25Sine(const char *deviceName, uint8_t bus, uint8_t address) md25.readData(); float current_revolution = md25.getRevolutions1(); float output_speed_rpm = 60*(current_revolution - prev_revolution)/dt; - float prev_revolution = current_revolution; mavlink_log_info(mavlink_fd, "rpm: %10.4f\n", (double)output_speed_rpm); // send input message - strncpy(debug_msg.key, "md25 in ", 10); - debug_msg.timestamp_ms = 1000*timestamp; - debug_msg.value = input_value; - debug_msg.update(); + //strncpy(debug_msg.key, "md25 in ", 10); + //debug_msg.timestamp_ms = 1000*timestamp; + //debug_msg.value = input_value; + //debug_msg.update(); // send output message strncpy(debug_msg.key, "md25 out ", 10); debug_msg.timestamp_ms = 1000*timestamp; - debug_msg.value = output_speed_rpm;; + debug_msg.value = current_revolution - prev_revolution; + //debug_msg.value = output_speed_rpm; debug_msg.update(); if (t > t_final) break; + // update for next step + prev_revolution = current_revolution; + // sleep usleep(1000000 * dt); } From 308f1dbfa4787e84665a3e822ddf7d1979f023ca Mon Sep 17 00:00:00 2001 From: James Goppert Date: Sat, 29 Jun 2013 17:04:43 -0400 Subject: [PATCH 10/18] Added amplitude frequency to md25sine command. --- src/drivers/md25/md25.cpp | 11 +++-------- src/drivers/md25/md25.hpp | 2 +- src/drivers/md25/md25_main.cpp | 8 ++++++-- 3 files changed, 10 insertions(+), 11 deletions(-) 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); } From 95aa82f586a8c44c53ae48517efdeb5e5673b7b5 Mon Sep 17 00:00:00 2001 From: James Goppert Date: Sat, 29 Jun 2013 17:05:41 -0400 Subject: [PATCH 11/18] Fixed arg number. --- src/drivers/md25/md25_main.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/drivers/md25/md25_main.cpp b/src/drivers/md25/md25_main.cpp index b395088a38..3260705c1c 100644 --- a/src/drivers/md25/md25_main.cpp +++ b/src/drivers/md25/md25_main.cpp @@ -138,7 +138,7 @@ int md25_main(int argc, char *argv[]) if (!strcmp(argv[1], "sine")) { - if (argc < 4) { + if (argc < 6) { printf("usage: md25 sine bus address amp freq\n"); exit(0); } From 1980d9dd63e29390f7c3ba9b31be576c07706f73 Mon Sep 17 00:00:00 2001 From: James Goppert Date: Sun, 28 Jul 2013 01:35:43 -0400 Subject: [PATCH 12/18] Working on segway controller, restructure of fixedwing. --- makefiles/config_px4fmu-v1_default.mk | 1 + src/drivers/md25/md25.cpp | 2 +- src/drivers/md25/md25.hpp | 2 +- .../att_pos_estimator_ekf/KalmanNav.hpp | 4 +- src/modules/controllib/block/Block.cpp | 4 +- src/modules/controllib/blocks.hpp | 1 + src/modules/controllib/module.mk | 8 +- .../{block => uorb}/UOrbPublication.cpp | 0 .../{block => uorb}/UOrbPublication.hpp | 4 +- .../{block => uorb}/UOrbSubscription.cpp | 0 .../{block => uorb}/UOrbSubscription.hpp | 4 +- src/modules/controllib/uorb/blocks.cpp | 64 +++++++ src/modules/controllib/uorb/blocks.hpp | 90 +++++++++ .../fixedwing.cpp | 18 -- .../fixedwing.hpp | 45 +---- .../fixedwing_backside_main.cpp | 3 +- src/modules/fixedwing_backside/module.mk | 1 + src/modules/segway/BlockSegwayController.cpp | 56 ++++++ src/modules/segway/BlockSegwayController.hpp | 18 ++ src/modules/segway/module.mk | 42 +++++ src/modules/segway/params.c | 72 ++++++++ src/modules/segway/segway_main.cpp | 173 ++++++++++++++++++ 22 files changed, 536 insertions(+), 76 deletions(-) rename src/modules/controllib/{block => uorb}/UOrbPublication.cpp (100%) rename src/modules/controllib/{block => uorb}/UOrbPublication.hpp (98%) rename src/modules/controllib/{block => uorb}/UOrbSubscription.cpp (100%) rename src/modules/controllib/{block => uorb}/UOrbSubscription.hpp (98%) create mode 100644 src/modules/controllib/uorb/blocks.cpp create mode 100644 src/modules/controllib/uorb/blocks.hpp rename src/modules/{controllib => fixedwing_backside}/fixedwing.cpp (93%) rename src/modules/{controllib => fixedwing_backside}/fixedwing.hpp (87%) create mode 100644 src/modules/segway/BlockSegwayController.cpp create mode 100644 src/modules/segway/BlockSegwayController.hpp create mode 100644 src/modules/segway/module.mk create mode 100644 src/modules/segway/params.c create mode 100644 src/modules/segway/segway_main.cpp diff --git a/makefiles/config_px4fmu-v1_default.mk b/makefiles/config_px4fmu-v1_default.mk index 74be1cd23f..cbca4f6a69 100644 --- a/makefiles/config_px4fmu-v1_default.mk +++ b/makefiles/config_px4fmu-v1_default.mk @@ -71,6 +71,7 @@ MODULES += examples/flow_position_estimator # # Vehicle Control # +MODULES += modules/segway MODULES += modules/fixedwing_backside MODULES += modules/fixedwing_att_control MODULES += modules/fixedwing_pos_control diff --git a/src/drivers/md25/md25.cpp b/src/drivers/md25/md25.cpp index 1079005c05..d6dd64a094 100644 --- a/src/drivers/md25/md25.cpp +++ b/src/drivers/md25/md25.cpp @@ -52,7 +52,7 @@ #include #include -#include +#include #include #include diff --git a/src/drivers/md25/md25.hpp b/src/drivers/md25/md25.hpp index 2fc317f5e4..780978514e 100644 --- a/src/drivers/md25/md25.hpp +++ b/src/drivers/md25/md25.hpp @@ -46,7 +46,7 @@ #include #include -#include +#include #include #include diff --git a/src/modules/att_pos_estimator_ekf/KalmanNav.hpp b/src/modules/att_pos_estimator_ekf/KalmanNav.hpp index 49d0d157da..f01ee03553 100644 --- a/src/modules/att_pos_estimator_ekf/KalmanNav.hpp +++ b/src/modules/att_pos_estimator_ekf/KalmanNav.hpp @@ -47,8 +47,8 @@ #include #include #include -#include -#include +#include +#include #include #include diff --git a/src/modules/controllib/block/Block.cpp b/src/modules/controllib/block/Block.cpp index 5994d23154..b964d40a31 100644 --- a/src/modules/controllib/block/Block.cpp +++ b/src/modules/controllib/block/Block.cpp @@ -43,8 +43,8 @@ #include "Block.hpp" #include "BlockParam.hpp" -#include "UOrbSubscription.hpp" -#include "UOrbPublication.hpp" +#include "../uorb/UOrbSubscription.hpp" +#include "../uorb/UOrbPublication.hpp" namespace control { diff --git a/src/modules/controllib/blocks.hpp b/src/modules/controllib/blocks.hpp index 7a785d12e2..fefe99702d 100644 --- a/src/modules/controllib/blocks.hpp +++ b/src/modules/controllib/blocks.hpp @@ -42,6 +42,7 @@ #include #include #include +#include #include #include "block/Block.hpp" diff --git a/src/modules/controllib/module.mk b/src/modules/controllib/module.mk index 13d1069c7a..d815a8feb5 100644 --- a/src/modules/controllib/module.mk +++ b/src/modules/controllib/module.mk @@ -37,7 +37,7 @@ SRCS = test_params.c \ block/Block.cpp \ block/BlockParam.cpp \ - block/UOrbPublication.cpp \ - block/UOrbSubscription.cpp \ - blocks.cpp \ - fixedwing.cpp + uorb/UOrbPublication.cpp \ + uorb/UOrbSubscription.cpp \ + uorb/blocks.cpp \ + blocks.cpp diff --git a/src/modules/controllib/block/UOrbPublication.cpp b/src/modules/controllib/uorb/UOrbPublication.cpp similarity index 100% rename from src/modules/controllib/block/UOrbPublication.cpp rename to src/modules/controllib/uorb/UOrbPublication.cpp diff --git a/src/modules/controllib/block/UOrbPublication.hpp b/src/modules/controllib/uorb/UOrbPublication.hpp similarity index 98% rename from src/modules/controllib/block/UOrbPublication.hpp rename to src/modules/controllib/uorb/UOrbPublication.hpp index 0a8ae2ff7d..6f1f3fc1c0 100644 --- a/src/modules/controllib/block/UOrbPublication.hpp +++ b/src/modules/controllib/uorb/UOrbPublication.hpp @@ -39,8 +39,8 @@ #pragma once #include -#include "Block.hpp" -#include "List.hpp" +#include "../block/Block.hpp" +#include "../block/List.hpp" namespace control diff --git a/src/modules/controllib/block/UOrbSubscription.cpp b/src/modules/controllib/uorb/UOrbSubscription.cpp similarity index 100% rename from src/modules/controllib/block/UOrbSubscription.cpp rename to src/modules/controllib/uorb/UOrbSubscription.cpp diff --git a/src/modules/controllib/block/UOrbSubscription.hpp b/src/modules/controllib/uorb/UOrbSubscription.hpp similarity index 98% rename from src/modules/controllib/block/UOrbSubscription.hpp rename to src/modules/controllib/uorb/UOrbSubscription.hpp index 22cc2e1145..d337d89a88 100644 --- a/src/modules/controllib/block/UOrbSubscription.hpp +++ b/src/modules/controllib/uorb/UOrbSubscription.hpp @@ -39,8 +39,8 @@ #pragma once #include -#include "Block.hpp" -#include "List.hpp" +#include "../block/Block.hpp" +#include "../block/List.hpp" namespace control diff --git a/src/modules/controllib/uorb/blocks.cpp b/src/modules/controllib/uorb/blocks.cpp new file mode 100644 index 0000000000..6e5ade5199 --- /dev/null +++ b/src/modules/controllib/uorb/blocks.cpp @@ -0,0 +1,64 @@ +/**************************************************************************** + * + * Copyright (C) 2012 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file uorb_blocks.cpp + * + * uorb block library code + */ + +#include "blocks.hpp" + +namespace control +{ + +BlockUorbEnabledAutopilot::BlockUorbEnabledAutopilot(SuperBlock *parent, const char *name) : + SuperBlock(parent, name), + // subscriptions + _att(&getSubscriptions(), ORB_ID(vehicle_attitude), 20), + _attCmd(&getSubscriptions(), ORB_ID(vehicle_attitude_setpoint), 20), + _ratesCmd(&getSubscriptions(), ORB_ID(vehicle_rates_setpoint), 20), + _pos(&getSubscriptions() , ORB_ID(vehicle_global_position), 20), + _posCmd(&getSubscriptions(), ORB_ID(vehicle_global_position_set_triplet), 20), + _manual(&getSubscriptions(), ORB_ID(manual_control_setpoint), 20), + _status(&getSubscriptions(), ORB_ID(vehicle_status), 20), + _param_update(&getSubscriptions(), ORB_ID(parameter_update), 1000), // limit to 1 Hz + // publications + _actuators(&getPublications(), ORB_ID(actuator_controls_0)) +{ +} + +BlockUorbEnabledAutopilot::~BlockUorbEnabledAutopilot() {}; + +} // namespace control + diff --git a/src/modules/controllib/uorb/blocks.hpp b/src/modules/controllib/uorb/blocks.hpp new file mode 100644 index 0000000000..54f31735ce --- /dev/null +++ b/src/modules/controllib/uorb/blocks.hpp @@ -0,0 +1,90 @@ +/**************************************************************************** + * + * Copyright (C) 2012 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file uorb_blocks.h + * + * uorb block library code + */ + +#pragma once + +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include + +#include +#include + +#include "../blocks.hpp" +#include "UOrbSubscription.hpp" +#include "UOrbPublication.hpp" + +namespace control +{ + +/** + * UorbEnabledAutopilot + */ +class __EXPORT BlockUorbEnabledAutopilot : public SuperBlock +{ +protected: + // subscriptions + UOrbSubscription _att; + UOrbSubscription _attCmd; + UOrbSubscription _ratesCmd; + UOrbSubscription _pos; + UOrbSubscription _posCmd; + UOrbSubscription _manual; + UOrbSubscription _status; + UOrbSubscription _param_update; + // publications + UOrbPublication _actuators; +public: + BlockUorbEnabledAutopilot(SuperBlock *parent, const char *name); + virtual ~BlockUorbEnabledAutopilot(); +}; + +} // namespace control + diff --git a/src/modules/controllib/fixedwing.cpp b/src/modules/fixedwing_backside/fixedwing.cpp similarity index 93% rename from src/modules/controllib/fixedwing.cpp rename to src/modules/fixedwing_backside/fixedwing.cpp index 77b2ac8063..d5dc746e0d 100644 --- a/src/modules/controllib/fixedwing.cpp +++ b/src/modules/fixedwing_backside/fixedwing.cpp @@ -123,24 +123,6 @@ void BlockWaypointGuidance::update(vehicle_global_position_s &pos, _xtYawLimit.update(_xt2Yaw.update(xtrackError.distance))); } -BlockUorbEnabledAutopilot::BlockUorbEnabledAutopilot(SuperBlock *parent, const char *name) : - SuperBlock(parent, name), - // subscriptions - _att(&getSubscriptions(), ORB_ID(vehicle_attitude), 20), - _attCmd(&getSubscriptions(), ORB_ID(vehicle_attitude_setpoint), 20), - _ratesCmd(&getSubscriptions(), ORB_ID(vehicle_rates_setpoint), 20), - _pos(&getSubscriptions() , ORB_ID(vehicle_global_position), 20), - _posCmd(&getSubscriptions(), ORB_ID(vehicle_global_position_set_triplet), 20), - _manual(&getSubscriptions(), ORB_ID(manual_control_setpoint), 20), - _status(&getSubscriptions(), ORB_ID(vehicle_status), 20), - _param_update(&getSubscriptions(), ORB_ID(parameter_update), 1000), // limit to 1 Hz - // publications - _actuators(&getPublications(), ORB_ID(actuator_controls_0)) -{ -} - -BlockUorbEnabledAutopilot::~BlockUorbEnabledAutopilot() {}; - BlockMultiModeBacksideAutopilot::BlockMultiModeBacksideAutopilot(SuperBlock *parent, const char *name) : BlockUorbEnabledAutopilot(parent, name), _stabilization(this, ""), // no name needed, already unique diff --git a/src/modules/controllib/fixedwing.hpp b/src/modules/fixedwing_backside/fixedwing.hpp similarity index 87% rename from src/modules/controllib/fixedwing.hpp rename to src/modules/fixedwing_backside/fixedwing.hpp index e4028c40d7..eb5d383810 100644 --- a/src/modules/controllib/fixedwing.hpp +++ b/src/modules/fixedwing_backside/fixedwing.hpp @@ -39,27 +39,8 @@ #pragma once -#include -#include -#include -#include -#include -#include -#include -#include -#include - -#include -#include -#include -#include - -#include -#include - -#include "blocks.hpp" -#include "block/UOrbSubscription.hpp" -#include "block/UOrbPublication.hpp" +#include +#include extern "C" { #include @@ -269,28 +250,6 @@ public: float getPsiCmd() { return _psiCmd; } }; -/** - * UorbEnabledAutopilot - */ -class __EXPORT BlockUorbEnabledAutopilot : public SuperBlock -{ -protected: - // subscriptions - UOrbSubscription _att; - UOrbSubscription _attCmd; - UOrbSubscription _ratesCmd; - UOrbSubscription _pos; - UOrbSubscription _posCmd; - UOrbSubscription _manual; - UOrbSubscription _status; - UOrbSubscription _param_update; - // publications - UOrbPublication _actuators; -public: - BlockUorbEnabledAutopilot(SuperBlock *parent, const char *name); - virtual ~BlockUorbEnabledAutopilot(); -}; - /** * Multi-mode Autopilot */ diff --git a/src/modules/fixedwing_backside/fixedwing_backside_main.cpp b/src/modules/fixedwing_backside/fixedwing_backside_main.cpp index 4803a526eb..f4ea050880 100644 --- a/src/modules/fixedwing_backside/fixedwing_backside_main.cpp +++ b/src/modules/fixedwing_backside/fixedwing_backside_main.cpp @@ -45,12 +45,13 @@ #include #include #include -#include #include #include #include #include +#include "fixedwing.hpp" + static bool thread_should_exit = false; /**< Deamon exit flag */ static bool thread_running = false; /**< Deamon status flag */ static int deamon_task; /**< Handle of deamon task / thread */ diff --git a/src/modules/fixedwing_backside/module.mk b/src/modules/fixedwing_backside/module.mk index ec958d7cbb..133728781d 100644 --- a/src/modules/fixedwing_backside/module.mk +++ b/src/modules/fixedwing_backside/module.mk @@ -38,4 +38,5 @@ MODULE_COMMAND = fixedwing_backside SRCS = fixedwing_backside_main.cpp \ + fixedwing.cpp \ params.c diff --git a/src/modules/segway/BlockSegwayController.cpp b/src/modules/segway/BlockSegwayController.cpp new file mode 100644 index 0000000000..b7a0bbbccb --- /dev/null +++ b/src/modules/segway/BlockSegwayController.cpp @@ -0,0 +1,56 @@ +#include "BlockSegwayController.hpp" + +void BlockSegwayController::update() { + // wait for a sensor update, check for exit condition every 100 ms + if (poll(&_attPoll, 1, 100) < 0) return; // poll error + + uint64_t newTimeStamp = hrt_absolute_time(); + float dt = (newTimeStamp - _timeStamp) / 1.0e6f; + _timeStamp = newTimeStamp; + + // check for sane values of dt + // to prevent large control responses + if (dt > 1.0f || dt < 0) return; + + // set dt for all child blocks + setDt(dt); + + // check for new updates + if (_param_update.updated()) updateParams(); + + // get new information from subscriptions + updateSubscriptions(); + + // default all output to zero unless handled by mode + for (unsigned i = 2; i < NUM_ACTUATOR_CONTROLS; i++) + _actuators.control[i] = 0.0f; + + // only update guidance in auto mode + if (_status.state_machine == SYSTEM_STATE_AUTO) { + // update guidance + } + + // XXX handle STABILIZED (loiter on spot) as well + // once the system switches from manual or auto to stabilized + // the setpoint should update to loitering around this position + + // handle autopilot modes + if (_status.state_machine == SYSTEM_STATE_AUTO || + _status.state_machine == SYSTEM_STATE_STABILIZED) { + + float spdCmd = phi2spd.update(_att.phi); + + // output + _actuators.control[0] = spdCmd; + _actuators.control[1] = spdCmd; + + } else if (_status.state_machine == SYSTEM_STATE_MANUAL) { + _actuators.control[0] = _manual.roll; + _actuators.control[1] = _manual.roll; + } + + // update all publications + updatePublications(); + +} + diff --git a/src/modules/segway/BlockSegwayController.hpp b/src/modules/segway/BlockSegwayController.hpp new file mode 100644 index 0000000000..b16d38338b --- /dev/null +++ b/src/modules/segway/BlockSegwayController.hpp @@ -0,0 +1,18 @@ +#pragma once + +#include + +using namespace control; + +class BlockSegwayController : public control::BlockUorbEnabledAutopilot { +public: + BlockSegwayController() : + BlockUorbEnabledAutopilot(NULL,"SEG"), + phi2spd(this, "PHI2SPD") + { + } + void update(); +private: + BlockP phi2spd; +}; + diff --git a/src/modules/segway/module.mk b/src/modules/segway/module.mk new file mode 100644 index 0000000000..d5da856010 --- /dev/null +++ b/src/modules/segway/module.mk @@ -0,0 +1,42 @@ +############################################################################ +# +# Copyright (c) 2012, 2013 PX4 Development Team. All rights reserved. +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions +# are met: +# +# 1. Redistributions of source code must retain the above copyright +# notice, this list of conditions and the following disclaimer. +# 2. Redistributions in binary form must reproduce the above copyright +# notice, this list of conditions and the following disclaimer in +# the documentation and/or other materials provided with the +# distribution. +# 3. Neither the name PX4 nor the names of its contributors may be +# used to endorse or promote products derived from this software +# without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS +# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED +# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE. +# +############################################################################ + +# +# segway controller +# + +MODULE_COMMAND = segway + +SRCS = segway_main.cpp \ + BlockSegwayController.cpp \ + params.c diff --git a/src/modules/segway/params.c b/src/modules/segway/params.c new file mode 100644 index 0000000000..db30af4160 --- /dev/null +++ b/src/modules/segway/params.c @@ -0,0 +1,72 @@ +#include + +// currently tuned for easystar from arkhangar in HIL +//https://github.com/arktools/arkhangar + +// 16 is max name length + +// gyro low pass filter +PARAM_DEFINE_FLOAT(FWB_P_LP, 300.0f); // roll rate low pass cut freq +PARAM_DEFINE_FLOAT(FWB_Q_LP, 300.0f); // pitch rate low pass cut freq +PARAM_DEFINE_FLOAT(FWB_R_LP, 300.0f); // yaw rate low pass cut freq + +// yaw washout +PARAM_DEFINE_FLOAT(FWB_R_HP, 1.0f); // yaw rate high pass + +// stabilization mode +PARAM_DEFINE_FLOAT(FWB_P2AIL, 0.3f); // roll rate 2 aileron +PARAM_DEFINE_FLOAT(FWB_Q2ELV, 0.1f); // pitch rate 2 elevator +PARAM_DEFINE_FLOAT(FWB_R2RDR, 0.1f); // yaw rate 2 rudder + +// psi -> phi -> p +PARAM_DEFINE_FLOAT(FWB_PSI2PHI, 0.5f); // heading 2 roll +PARAM_DEFINE_FLOAT(FWB_PHI2P, 1.0f); // roll to roll rate +PARAM_DEFINE_FLOAT(FWB_PHI_LIM_MAX, 0.3f); // roll limit, 28 deg + +// velocity -> theta +PARAM_DEFINE_FLOAT(FWB_V2THE_P, 1.0f); // velocity to pitch angle PID, prop gain +PARAM_DEFINE_FLOAT(FWB_V2THE_I, 0.0f); // integral gain +PARAM_DEFINE_FLOAT(FWB_V2THE_D, 0.0f); // derivative gain +PARAM_DEFINE_FLOAT(FWB_V2THE_D_LP, 0.0f); // derivative low-pass +PARAM_DEFINE_FLOAT(FWB_V2THE_I_MAX, 0.0f); // integrator wind up guard +PARAM_DEFINE_FLOAT(FWB_THE_MIN, -0.5f); // the max commanded pitch angle +PARAM_DEFINE_FLOAT(FWB_THE_MAX, 0.5f); // the min commanded pitch angle + + +// theta -> q +PARAM_DEFINE_FLOAT(FWB_THE2Q_P, 1.0f); // pitch angle to pitch-rate PID +PARAM_DEFINE_FLOAT(FWB_THE2Q_I, 0.0f); +PARAM_DEFINE_FLOAT(FWB_THE2Q_D, 0.0f); +PARAM_DEFINE_FLOAT(FWB_THE2Q_D_LP, 0.0f); +PARAM_DEFINE_FLOAT(FWB_THE2Q_I_MAX, 0.0f); + +// h -> thr +PARAM_DEFINE_FLOAT(FWB_H2THR_P, 0.01f); // altitude to throttle PID +PARAM_DEFINE_FLOAT(FWB_H2THR_I, 0.0f); +PARAM_DEFINE_FLOAT(FWB_H2THR_D, 0.0f); +PARAM_DEFINE_FLOAT(FWB_H2THR_D_LP, 0.0f); +PARAM_DEFINE_FLOAT(FWB_H2THR_I_MAX, 0.0f); + +// crosstrack +PARAM_DEFINE_FLOAT(FWB_XT2YAW_MAX, 1.57f); // cross-track to yaw angle limit 90 deg +PARAM_DEFINE_FLOAT(FWB_XT2YAW, 0.005f); // cross-track to yaw angle gain + +// speed command +PARAM_DEFINE_FLOAT(FWB_V_MIN, 10.0f); // minimum commanded velocity +PARAM_DEFINE_FLOAT(FWB_V_CMD, 12.0f); // commanded velocity +PARAM_DEFINE_FLOAT(FWB_V_MAX, 16.0f); // maximum commanded velocity + +// rate of climb +// this is what rate of climb is commanded (in m/s) +// when the pitch stick is fully defelcted in simple mode +PARAM_DEFINE_FLOAT(FWB_CR_MAX, 1.0f); + +// climb rate -> thr +PARAM_DEFINE_FLOAT(FWB_CR2THR_P, 0.01f); // rate of climb to throttle PID +PARAM_DEFINE_FLOAT(FWB_CR2THR_I, 0.0f); +PARAM_DEFINE_FLOAT(FWB_CR2THR_D, 0.0f); +PARAM_DEFINE_FLOAT(FWB_CR2THR_D_LP, 0.0f); +PARAM_DEFINE_FLOAT(FWB_CR2THR_I_MAX, 0.0f); + +PARAM_DEFINE_FLOAT(FWB_TRIM_THR, 0.8f); // trim throttle (0,1) +PARAM_DEFINE_FLOAT(FWB_TRIM_V, 12.0f); // trim velocity, m/s diff --git a/src/modules/segway/segway_main.cpp b/src/modules/segway/segway_main.cpp new file mode 100644 index 0000000000..8be1cc7aaf --- /dev/null +++ b/src/modules/segway/segway_main.cpp @@ -0,0 +1,173 @@ +/**************************************************************************** + * + * Copyright (c) 2012, 2013 PX4 Development Team. All rights reserved. + * Author: James Goppert + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file segway_main.cpp + * @author James Goppert + * + * Segway controller using control library + */ + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include "BlockSegwayController.hpp" + +static bool thread_should_exit = false; /**< Deamon exit flag */ +static bool thread_running = false; /**< Deamon status flag */ +static int deamon_task; /**< Handle of deamon task / thread */ + +/** + * Deamon management function. + */ +extern "C" __EXPORT int segway_main(int argc, char *argv[]); + +/** + * Mainloop of deamon. + */ +int control_demo_thread_main(int argc, char *argv[]); + +/** + * Test function + */ +void test(); + +/** + * Print the correct usage. + */ +static void usage(const char *reason); + +static void +usage(const char *reason) +{ + if (reason) + fprintf(stderr, "%s\n", reason); + + fprintf(stderr, "usage: segway {start|stop|status} [-p ]\n\n"); + exit(1); +} + +/** + * The deamon app only briefly exists to start + * the background job. The stack size assigned in the + * Makefile does only apply to this management task. + * + * The actual stack size should be set in the call + * to task_create(). + */ +int segway_main(int argc, char *argv[]) +{ + + if (argc < 1) + usage("missing command"); + + if (!strcmp(argv[1], "start")) { + + if (thread_running) { + warnx("already running"); + /* this is not an error */ + exit(0); + } + + thread_should_exit = false; + + deamon_task = task_spawn_cmd("segway", + SCHED_DEFAULT, + SCHED_PRIORITY_MAX - 10, + 5120, + control_demo_thread_main, + (argv) ? (const char **)&argv[2] : (const char **)NULL); + exit(0); + } + + if (!strcmp(argv[1], "test")) { + test(); + exit(0); + } + + if (!strcmp(argv[1], "stop")) { + thread_should_exit = true; + exit(0); + } + + if (!strcmp(argv[1], "status")) { + if (thread_running) { + warnx("is running"); + + } else { + warnx("not started"); + } + + exit(0); + } + + usage("unrecognized command"); + exit(1); +} + +int control_demo_thread_main(int argc, char *argv[]) +{ + + warnx("starting"); + + using namespace control; + + BlockSegwayController autopilot; + + thread_running = true; + + while (!thread_should_exit) { + autopilot.update(); + } + + warnx("exiting."); + + thread_running = false; + + return 0; +} + +void test() +{ + warnx("beginning control lib test"); + control::basicBlocksTest(); +} From dc542b2a4818bfc1c3c1cd24c5ee513d5b5ea0c4 Mon Sep 17 00:00:00 2001 From: James Goppert Date: Sun, 28 Jul 2013 22:27:05 -0400 Subject: [PATCH 13/18] Segway stabilized. --- src/drivers/md25/md25.cpp | 17 +++-- src/drivers/md25/md25.hpp | 13 ++++ src/drivers/md25/md25_main.cpp | 27 +++++++- src/modules/controllib/uorb/blocks.cpp | 37 ++++++++++ src/modules/controllib/uorb/blocks.hpp | 23 +++++++ src/modules/fixedwing_backside/fixedwing.cpp | 37 ---------- src/modules/fixedwing_backside/fixedwing.hpp | 23 ------- src/modules/segway/BlockSegwayController.cpp | 19 +++--- src/modules/segway/BlockSegwayController.hpp | 13 +++- src/modules/segway/params.c | 72 ++------------------ src/modules/segway/segway_main.cpp | 22 +----- 11 files changed, 138 insertions(+), 165 deletions(-) diff --git a/src/drivers/md25/md25.cpp b/src/drivers/md25/md25.cpp index d6dd64a094..d43e3aef9e 100644 --- a/src/drivers/md25/md25.cpp +++ b/src/drivers/md25/md25.cpp @@ -116,7 +116,8 @@ MD25::MD25(const char *deviceName, int bus, setMotor2Speed(0); resetEncoders(); _setMode(MD25::MODE_UNSIGNED_SPEED); - setSpeedRegulation(true); + setSpeedRegulation(false); + setMotorAccel(10); setTimeout(true); } @@ -308,6 +309,12 @@ int MD25::setDeviceAddress(uint8_t address) return OK; } +int MD25::setMotorAccel(uint8_t accel) +{ + return _writeUint8(REG_ACCEL_RATE_RW, + accel); +} + int MD25::setMotor1Speed(float value) { return _writeUint8(REG_SPEED1_RW, @@ -461,12 +468,12 @@ int md25Test(const char *deviceName, uint8_t bus, uint8_t address) MD25 md25("/dev/md25", bus, address); // print status - char buf[200]; + char buf[400]; md25.status(buf, sizeof(buf)); printf("%s\n", buf); // setup for test - md25.setSpeedRegulation(true); + md25.setSpeedRegulation(false); md25.setTimeout(true); float dt = 0.1; float speed = 0.2; @@ -568,12 +575,12 @@ int md25Sine(const char *deviceName, uint8_t bus, uint8_t address, float amplitu MD25 md25("/dev/md25", bus, address); // print status - char buf[200]; + char buf[400]; md25.status(buf, sizeof(buf)); printf("%s\n", buf); // setup for test - md25.setSpeedRegulation(true); + md25.setSpeedRegulation(false); md25.setTimeout(true); float dt = 0.01; float t_final = 60.0; diff --git a/src/drivers/md25/md25.hpp b/src/drivers/md25/md25.hpp index 780978514e..1661f67f9a 100644 --- a/src/drivers/md25/md25.hpp +++ b/src/drivers/md25/md25.hpp @@ -212,6 +212,19 @@ public: */ int setDeviceAddress(uint8_t address); + /** + * set motor acceleration + * @param accel + * controls motor speed change (1-10) + * accel rate | time for full fwd. to full rev. + * 1 | 6.375 s + * 2 | 1.6 s + * 3 | 0.675 s + * 5(default) | 1.275 s + * 10 | 0.65 s + */ + int setMotorAccel(uint8_t accel); + /** * set motor 1 speed * @param normSpeed normalize speed between -1 and 1 diff --git a/src/drivers/md25/md25_main.cpp b/src/drivers/md25/md25_main.cpp index 3260705c1c..7e5904d050 100644 --- a/src/drivers/md25/md25_main.cpp +++ b/src/drivers/md25/md25_main.cpp @@ -82,7 +82,7 @@ usage(const char *reason) if (reason) fprintf(stderr, "%s\n", reason); - fprintf(stderr, "usage: md25 {start|stop|status|search|test|change_address}\n\n"); + fprintf(stderr, "usage: md25 {start|stop|read|status|search|test|change_address}\n\n"); exit(1); } @@ -184,6 +184,29 @@ int md25_main(int argc, char *argv[]) exit(0); } + if (!strcmp(argv[1], "read")) { + if (argc < 4) { + printf("usage: md25 read 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); + + MD25 md25(deviceName, bus, address); + + // print status + char buf[400]; + md25.status(buf, sizeof(buf)); + printf("%s\n", buf); + + exit(0); + } + + if (!strcmp(argv[1], "search")) { if (argc < 3) { printf("usage: md25 search bus\n"); @@ -268,7 +291,7 @@ int md25_thread_main(int argc, char *argv[]) uint8_t address = strtoul(argv[4], nullptr, 0); // start - MD25 md25("/dev/md25", bus, address); + MD25 md25(deviceName, bus, address); thread_running = true; diff --git a/src/modules/controllib/uorb/blocks.cpp b/src/modules/controllib/uorb/blocks.cpp index 6e5ade5199..448a42a996 100644 --- a/src/modules/controllib/uorb/blocks.cpp +++ b/src/modules/controllib/uorb/blocks.cpp @@ -42,6 +42,43 @@ namespace control { +BlockWaypointGuidance::BlockWaypointGuidance(SuperBlock *parent, const char *name) : + SuperBlock(parent, name), + _xtYawLimit(this, "XT2YAW"), + _xt2Yaw(this, "XT2YAW"), + _psiCmd(0) +{ +} + +BlockWaypointGuidance::~BlockWaypointGuidance() {}; + +void BlockWaypointGuidance::update(vehicle_global_position_s &pos, + vehicle_attitude_s &att, + vehicle_global_position_setpoint_s &posCmd, + vehicle_global_position_setpoint_s &lastPosCmd) +{ + + // heading to waypoint + float psiTrack = get_bearing_to_next_waypoint( + (double)pos.lat / (double)1e7d, + (double)pos.lon / (double)1e7d, + (double)posCmd.lat / (double)1e7d, + (double)posCmd.lon / (double)1e7d); + + // cross track + struct crosstrack_error_s xtrackError; + get_distance_to_line(&xtrackError, + (double)pos.lat / (double)1e7d, + (double)pos.lon / (double)1e7d, + (double)lastPosCmd.lat / (double)1e7d, + (double)lastPosCmd.lon / (double)1e7d, + (double)posCmd.lat / (double)1e7d, + (double)posCmd.lon / (double)1e7d); + + _psiCmd = _wrap_2pi(psiTrack - + _xtYawLimit.update(_xt2Yaw.update(xtrackError.distance))); +} + BlockUorbEnabledAutopilot::BlockUorbEnabledAutopilot(SuperBlock *parent, const char *name) : SuperBlock(parent, name), // subscriptions diff --git a/src/modules/controllib/uorb/blocks.hpp b/src/modules/controllib/uorb/blocks.hpp index 54f31735ce..9c0720aa5e 100644 --- a/src/modules/controllib/uorb/blocks.hpp +++ b/src/modules/controllib/uorb/blocks.hpp @@ -57,6 +57,10 @@ #include #include +extern "C" { +#include +} + #include "../blocks.hpp" #include "UOrbSubscription.hpp" #include "UOrbPublication.hpp" @@ -64,6 +68,25 @@ namespace control { +/** + * Waypoint Guidance block + */ +class __EXPORT BlockWaypointGuidance : public SuperBlock +{ +private: + BlockLimitSym _xtYawLimit; + BlockP _xt2Yaw; + float _psiCmd; +public: + BlockWaypointGuidance(SuperBlock *parent, const char *name); + virtual ~BlockWaypointGuidance(); + void update(vehicle_global_position_s &pos, + vehicle_attitude_s &att, + vehicle_global_position_setpoint_s &posCmd, + vehicle_global_position_setpoint_s &lastPosCmd); + float getPsiCmd() { return _psiCmd; } +}; + /** * UorbEnabledAutopilot */ diff --git a/src/modules/fixedwing_backside/fixedwing.cpp b/src/modules/fixedwing_backside/fixedwing.cpp index d5dc746e0d..f655a13bd7 100644 --- a/src/modules/fixedwing_backside/fixedwing.cpp +++ b/src/modules/fixedwing_backside/fixedwing.cpp @@ -86,43 +86,6 @@ void BlockStabilization::update(float pCmd, float qCmd, float rCmd, _yawDamper.update(rCmd, r, outputScale); } -BlockWaypointGuidance::BlockWaypointGuidance(SuperBlock *parent, const char *name) : - SuperBlock(parent, name), - _xtYawLimit(this, "XT2YAW"), - _xt2Yaw(this, "XT2YAW"), - _psiCmd(0) -{ -} - -BlockWaypointGuidance::~BlockWaypointGuidance() {}; - -void BlockWaypointGuidance::update(vehicle_global_position_s &pos, - vehicle_attitude_s &att, - vehicle_global_position_setpoint_s &posCmd, - vehicle_global_position_setpoint_s &lastPosCmd) -{ - - // heading to waypoint - float psiTrack = get_bearing_to_next_waypoint( - (double)pos.lat / (double)1e7d, - (double)pos.lon / (double)1e7d, - (double)posCmd.lat / (double)1e7d, - (double)posCmd.lon / (double)1e7d); - - // cross track - struct crosstrack_error_s xtrackError; - get_distance_to_line(&xtrackError, - (double)pos.lat / (double)1e7d, - (double)pos.lon / (double)1e7d, - (double)lastPosCmd.lat / (double)1e7d, - (double)lastPosCmd.lon / (double)1e7d, - (double)posCmd.lat / (double)1e7d, - (double)posCmd.lon / (double)1e7d); - - _psiCmd = _wrap_2pi(psiTrack - - _xtYawLimit.update(_xt2Yaw.update(xtrackError.distance))); -} - BlockMultiModeBacksideAutopilot::BlockMultiModeBacksideAutopilot(SuperBlock *parent, const char *name) : BlockUorbEnabledAutopilot(parent, name), _stabilization(this, ""), // no name needed, already unique diff --git a/src/modules/fixedwing_backside/fixedwing.hpp b/src/modules/fixedwing_backside/fixedwing.hpp index eb5d383810..3876e46301 100644 --- a/src/modules/fixedwing_backside/fixedwing.hpp +++ b/src/modules/fixedwing_backside/fixedwing.hpp @@ -42,10 +42,6 @@ #include #include -extern "C" { -#include -} - namespace control { @@ -231,25 +227,6 @@ public: * than frontside at high speeds. */ -/** - * Waypoint Guidance block - */ -class __EXPORT BlockWaypointGuidance : public SuperBlock -{ -private: - BlockLimitSym _xtYawLimit; - BlockP _xt2Yaw; - float _psiCmd; -public: - BlockWaypointGuidance(SuperBlock *parent, const char *name); - virtual ~BlockWaypointGuidance(); - void update(vehicle_global_position_s &pos, - vehicle_attitude_s &att, - vehicle_global_position_setpoint_s &posCmd, - vehicle_global_position_setpoint_s &lastPosCmd); - float getPsiCmd() { return _psiCmd; } -}; - /** * Multi-mode Autopilot */ diff --git a/src/modules/segway/BlockSegwayController.cpp b/src/modules/segway/BlockSegwayController.cpp index b7a0bbbccb..682758a14b 100644 --- a/src/modules/segway/BlockSegwayController.cpp +++ b/src/modules/segway/BlockSegwayController.cpp @@ -30,23 +30,24 @@ void BlockSegwayController::update() { // update guidance } - // XXX handle STABILIZED (loiter on spot) as well - // once the system switches from manual or auto to stabilized - // the setpoint should update to loitering around this position + // compute speed command + float spdCmd = -theta2spd.update(_att.pitch) - q2spd.update(_att.pitchspeed); // handle autopilot modes if (_status.state_machine == SYSTEM_STATE_AUTO || _status.state_machine == SYSTEM_STATE_STABILIZED) { - - float spdCmd = phi2spd.update(_att.phi); - - // output _actuators.control[0] = spdCmd; _actuators.control[1] = spdCmd; } else if (_status.state_machine == SYSTEM_STATE_MANUAL) { - _actuators.control[0] = _manual.roll; - _actuators.control[1] = _manual.roll; + if (_status.manual_control_mode == VEHICLE_MANUAL_CONTROL_MODE_DIRECT) { + _actuators.control[CH_LEFT] = _manual.throttle; + _actuators.control[CH_RIGHT] = _manual.pitch; + + } else if (_status.manual_control_mode == VEHICLE_MANUAL_CONTROL_MODE_SAS) { + _actuators.control[0] = spdCmd; + _actuators.control[1] = spdCmd; + } } // update all publications diff --git a/src/modules/segway/BlockSegwayController.hpp b/src/modules/segway/BlockSegwayController.hpp index b16d38338b..e2faa49169 100644 --- a/src/modules/segway/BlockSegwayController.hpp +++ b/src/modules/segway/BlockSegwayController.hpp @@ -8,11 +8,20 @@ class BlockSegwayController : public control::BlockUorbEnabledAutopilot { public: BlockSegwayController() : BlockUorbEnabledAutopilot(NULL,"SEG"), - phi2spd(this, "PHI2SPD") + theta2spd(this, "THETA2SPD"), + q2spd(this, "Q2SPD"), + _attPoll(), + _timeStamp(0) { + _attPoll.fd = _att.getHandle(); + _attPoll.events = POLLIN; } void update(); private: - BlockP phi2spd; + enum {CH_LEFT, CH_RIGHT}; + BlockPI theta2spd; + BlockP q2spd; + struct pollfd _attPoll; + uint64_t _timeStamp; }; diff --git a/src/modules/segway/params.c b/src/modules/segway/params.c index db30af4160..1669785d31 100644 --- a/src/modules/segway/params.c +++ b/src/modules/segway/params.c @@ -1,72 +1,8 @@ #include -// currently tuned for easystar from arkhangar in HIL -//https://github.com/arktools/arkhangar - // 16 is max name length +PARAM_DEFINE_FLOAT(SEG_THETA2SPD_P, 10.0f); // pitch to speed +PARAM_DEFINE_FLOAT(SEG_THETA2SPD_I, 0.0f); // pitch integral to speed +PARAM_DEFINE_FLOAT(SEG_THETA2SPD_I_MAX, 0.0f); // integral limiter +PARAM_DEFINE_FLOAT(SEG_Q2SPD, 1.0f); // pitch rate to speed -// gyro low pass filter -PARAM_DEFINE_FLOAT(FWB_P_LP, 300.0f); // roll rate low pass cut freq -PARAM_DEFINE_FLOAT(FWB_Q_LP, 300.0f); // pitch rate low pass cut freq -PARAM_DEFINE_FLOAT(FWB_R_LP, 300.0f); // yaw rate low pass cut freq - -// yaw washout -PARAM_DEFINE_FLOAT(FWB_R_HP, 1.0f); // yaw rate high pass - -// stabilization mode -PARAM_DEFINE_FLOAT(FWB_P2AIL, 0.3f); // roll rate 2 aileron -PARAM_DEFINE_FLOAT(FWB_Q2ELV, 0.1f); // pitch rate 2 elevator -PARAM_DEFINE_FLOAT(FWB_R2RDR, 0.1f); // yaw rate 2 rudder - -// psi -> phi -> p -PARAM_DEFINE_FLOAT(FWB_PSI2PHI, 0.5f); // heading 2 roll -PARAM_DEFINE_FLOAT(FWB_PHI2P, 1.0f); // roll to roll rate -PARAM_DEFINE_FLOAT(FWB_PHI_LIM_MAX, 0.3f); // roll limit, 28 deg - -// velocity -> theta -PARAM_DEFINE_FLOAT(FWB_V2THE_P, 1.0f); // velocity to pitch angle PID, prop gain -PARAM_DEFINE_FLOAT(FWB_V2THE_I, 0.0f); // integral gain -PARAM_DEFINE_FLOAT(FWB_V2THE_D, 0.0f); // derivative gain -PARAM_DEFINE_FLOAT(FWB_V2THE_D_LP, 0.0f); // derivative low-pass -PARAM_DEFINE_FLOAT(FWB_V2THE_I_MAX, 0.0f); // integrator wind up guard -PARAM_DEFINE_FLOAT(FWB_THE_MIN, -0.5f); // the max commanded pitch angle -PARAM_DEFINE_FLOAT(FWB_THE_MAX, 0.5f); // the min commanded pitch angle - - -// theta -> q -PARAM_DEFINE_FLOAT(FWB_THE2Q_P, 1.0f); // pitch angle to pitch-rate PID -PARAM_DEFINE_FLOAT(FWB_THE2Q_I, 0.0f); -PARAM_DEFINE_FLOAT(FWB_THE2Q_D, 0.0f); -PARAM_DEFINE_FLOAT(FWB_THE2Q_D_LP, 0.0f); -PARAM_DEFINE_FLOAT(FWB_THE2Q_I_MAX, 0.0f); - -// h -> thr -PARAM_DEFINE_FLOAT(FWB_H2THR_P, 0.01f); // altitude to throttle PID -PARAM_DEFINE_FLOAT(FWB_H2THR_I, 0.0f); -PARAM_DEFINE_FLOAT(FWB_H2THR_D, 0.0f); -PARAM_DEFINE_FLOAT(FWB_H2THR_D_LP, 0.0f); -PARAM_DEFINE_FLOAT(FWB_H2THR_I_MAX, 0.0f); - -// crosstrack -PARAM_DEFINE_FLOAT(FWB_XT2YAW_MAX, 1.57f); // cross-track to yaw angle limit 90 deg -PARAM_DEFINE_FLOAT(FWB_XT2YAW, 0.005f); // cross-track to yaw angle gain - -// speed command -PARAM_DEFINE_FLOAT(FWB_V_MIN, 10.0f); // minimum commanded velocity -PARAM_DEFINE_FLOAT(FWB_V_CMD, 12.0f); // commanded velocity -PARAM_DEFINE_FLOAT(FWB_V_MAX, 16.0f); // maximum commanded velocity - -// rate of climb -// this is what rate of climb is commanded (in m/s) -// when the pitch stick is fully defelcted in simple mode -PARAM_DEFINE_FLOAT(FWB_CR_MAX, 1.0f); - -// climb rate -> thr -PARAM_DEFINE_FLOAT(FWB_CR2THR_P, 0.01f); // rate of climb to throttle PID -PARAM_DEFINE_FLOAT(FWB_CR2THR_I, 0.0f); -PARAM_DEFINE_FLOAT(FWB_CR2THR_D, 0.0f); -PARAM_DEFINE_FLOAT(FWB_CR2THR_D_LP, 0.0f); -PARAM_DEFINE_FLOAT(FWB_CR2THR_I_MAX, 0.0f); - -PARAM_DEFINE_FLOAT(FWB_TRIM_THR, 0.8f); // trim throttle (0,1) -PARAM_DEFINE_FLOAT(FWB_TRIM_V, 12.0f); // trim velocity, m/s diff --git a/src/modules/segway/segway_main.cpp b/src/modules/segway/segway_main.cpp index 8be1cc7aaf..061fbf9b9d 100644 --- a/src/modules/segway/segway_main.cpp +++ b/src/modules/segway/segway_main.cpp @@ -64,12 +64,7 @@ extern "C" __EXPORT int segway_main(int argc, char *argv[]); /** * Mainloop of deamon. */ -int control_demo_thread_main(int argc, char *argv[]); - -/** - * Test function - */ -void test(); +int segway_thread_main(int argc, char *argv[]); /** * Print the correct usage. @@ -114,16 +109,11 @@ int segway_main(int argc, char *argv[]) SCHED_DEFAULT, SCHED_PRIORITY_MAX - 10, 5120, - control_demo_thread_main, + segway_thread_main, (argv) ? (const char **)&argv[2] : (const char **)NULL); exit(0); } - if (!strcmp(argv[1], "test")) { - test(); - exit(0); - } - if (!strcmp(argv[1], "stop")) { thread_should_exit = true; exit(0); @@ -144,7 +134,7 @@ int segway_main(int argc, char *argv[]) exit(1); } -int control_demo_thread_main(int argc, char *argv[]) +int segway_thread_main(int argc, char *argv[]) { warnx("starting"); @@ -165,9 +155,3 @@ int control_demo_thread_main(int argc, char *argv[]) return 0; } - -void test() -{ - warnx("beginning control lib test"); - control::basicBlocksTest(); -} From a569cd834c13052ef3210ab9b8c1afd03b81fef6 Mon Sep 17 00:00:00 2001 From: James Goppert Date: Sun, 28 Jul 2013 22:28:04 -0400 Subject: [PATCH 14/18] Added segway rc script. --- ROMFS/px4fmu_common/init.d/40_io_segway | 122 ++++++++++++++++++++++++ 1 file changed, 122 insertions(+) create mode 100644 ROMFS/px4fmu_common/init.d/40_io_segway diff --git a/ROMFS/px4fmu_common/init.d/40_io_segway b/ROMFS/px4fmu_common/init.d/40_io_segway new file mode 100644 index 0000000000..5742d685aa --- /dev/null +++ b/ROMFS/px4fmu_common/init.d/40_io_segway @@ -0,0 +1,122 @@ +#!nsh +# +# Flight startup script for PX4FMU+PX4IO +# + +# disable USB and autostart +set USB no +set MODE custom + +# +# Start the ORB (first app to start) +# +uorb start + +# +# Load microSD params +# +echo "[init] loading microSD params" +param select /fs/microsd/params +if [ -f /fs/microsd/params ] +then + param load /fs/microsd/params +fi + +# +# Load default params for this platform +# +if param compare SYS_AUTOCONFIG 1 +then + # Set all params here, then disable autoconfig + param set SYS_AUTOCONFIG 0 + param save /fs/microsd/params +fi + +# +# Force some key parameters to sane values +# MAV_TYPE 1 = fixed wing, 2 = quadrotor, 13 = hexarotor +# see https://pixhawk.ethz.ch/mavlink/ +# +param set MAV_TYPE 10 + +# +# Check if PX4IO Firmware should be upgraded (from Andrew Tridgell) +# +if [ -f /fs/microsd/px4io.bin ] +then + echo "PX4IO Firmware found. Checking Upgrade.." + if cmp /fs/microsd/px4io.bin /fs/microsd/px4io.bin.current + then + echo "No newer version, skipping upgrade." + else + echo "Loading /fs/microsd/px4io.bin" + if px4io update /fs/microsd/px4io.bin > /fs/microsd/px4io_update.log + then + cp /fs/microsd/px4io.bin /fs/microsd/px4io.bin.current + echo "Flashed /fs/microsd/px4io.bin OK" >> /fs/microsd/px4io_update.log + else + echo "Failed flashing /fs/microsd/px4io.bin" >> /fs/microsd/px4io_update.log + echo "Failed to upgrade PX4IO firmware - check if PX4IO is in bootloader mode" + fi + fi +fi + +# +# Start MAVLink (depends on orb) +# +mavlink start -d /dev/ttyS1 -b 57600 +usleep 5000 + +# +# Start the commander (depends on orb, mavlink) +# +commander start + +# +# Start PX4IO interface (depends on orb, commander) +# +px4io start + +# +# Allow PX4IO to recover from midair restarts. +# this is very unlikely, but quite safe and robust. +px4io recovery + +# +# Disable px4io topic limiting +# +px4io limit 200 + +# +# Start the sensors (depends on orb, px4io) +# +sh /etc/init.d/rc.sensors + +# +# Start GPS interface (depends on orb) +# +gps start + +# +# Start the attitude estimator (depends on orb) +# +attitude_estimator_ekf start + +# +# Load mixer and start controllers (depends on px4io) +# +md25 start 3 0x58 +segway start + +# +# Start logging +# +sdlog2 start -r 50 -a -b 14 + +# +# Start system state +# +if blinkm start +then + blinkm systemstate +fi From 97b75951b13b9f4a79058d11f4c8923444ebc544 Mon Sep 17 00:00:00 2001 From: James Goppert Date: Sat, 3 Aug 2013 14:33:43 -0400 Subject: [PATCH 15/18] Shortened segway param names. --- src/modules/segway/BlockSegwayController.cpp | 2 +- src/modules/segway/BlockSegwayController.hpp | 8 ++++---- src/modules/segway/params.c | 8 ++++---- 3 files changed, 9 insertions(+), 9 deletions(-) diff --git a/src/modules/segway/BlockSegwayController.cpp b/src/modules/segway/BlockSegwayController.cpp index 682758a14b..b1dc39445f 100644 --- a/src/modules/segway/BlockSegwayController.cpp +++ b/src/modules/segway/BlockSegwayController.cpp @@ -31,7 +31,7 @@ void BlockSegwayController::update() { } // compute speed command - float spdCmd = -theta2spd.update(_att.pitch) - q2spd.update(_att.pitchspeed); + float spdCmd = -th2v.update(_att.pitch) - q2v.update(_att.pitchspeed); // handle autopilot modes if (_status.state_machine == SYSTEM_STATE_AUTO || diff --git a/src/modules/segway/BlockSegwayController.hpp b/src/modules/segway/BlockSegwayController.hpp index e2faa49169..4a01f785c5 100644 --- a/src/modules/segway/BlockSegwayController.hpp +++ b/src/modules/segway/BlockSegwayController.hpp @@ -8,8 +8,8 @@ class BlockSegwayController : public control::BlockUorbEnabledAutopilot { public: BlockSegwayController() : BlockUorbEnabledAutopilot(NULL,"SEG"), - theta2spd(this, "THETA2SPD"), - q2spd(this, "Q2SPD"), + th2v(this, "TH2V"), + q2v(this, "Q2V"), _attPoll(), _timeStamp(0) { @@ -19,8 +19,8 @@ public: void update(); private: enum {CH_LEFT, CH_RIGHT}; - BlockPI theta2spd; - BlockP q2spd; + BlockPI th2v; + BlockP q2v; struct pollfd _attPoll; uint64_t _timeStamp; }; diff --git a/src/modules/segway/params.c b/src/modules/segway/params.c index 1669785d31..d729237174 100644 --- a/src/modules/segway/params.c +++ b/src/modules/segway/params.c @@ -1,8 +1,8 @@ #include // 16 is max name length -PARAM_DEFINE_FLOAT(SEG_THETA2SPD_P, 10.0f); // pitch to speed -PARAM_DEFINE_FLOAT(SEG_THETA2SPD_I, 0.0f); // pitch integral to speed -PARAM_DEFINE_FLOAT(SEG_THETA2SPD_I_MAX, 0.0f); // integral limiter -PARAM_DEFINE_FLOAT(SEG_Q2SPD, 1.0f); // pitch rate to speed +PARAM_DEFINE_FLOAT(SEG_TH2V_P, 10.0f); // pitch to voltage +PARAM_DEFINE_FLOAT(SEG_TH2V_I, 0.0f); // pitch integral to voltage +PARAM_DEFINE_FLOAT(SEG_TH2V_I_MAX, 0.0f); // integral limiter +PARAM_DEFINE_FLOAT(SEG_Q2V, 1.0f); // pitch rate to voltage From 518e95ce44ca586a930609fabf551f12001a7b52 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 12 Aug 2013 21:49:49 +0200 Subject: [PATCH 16/18] Hotfix: Fixed Windows execution of sdlog2 dump script --- Tools/sdlog2_dump.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/Tools/sdlog2_dump.py b/Tools/sdlog2_dump.py index ebc04f4d03..838da9c489 100644 --- a/Tools/sdlog2_dump.py +++ b/Tools/sdlog2_dump.py @@ -105,7 +105,7 @@ class SDLog2Parser: for msg_name, show_fields in self.__msg_filter: self.__msg_filter_map[msg_name] = show_fields first_data_msg = True - f = open(fn, "r") + f = open(fn, "rb") bytes_read = 0 while True: chunk = f.read(self.BLOCK_SIZE) From d3d9d059c0c0780541d9dfd9c44b1f521298f5b6 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 12 Aug 2013 22:03:57 +0200 Subject: [PATCH 17/18] First stab at Python 2 and 3 compatibilty --- Tools/sdlog2_dump.py | 72 +++++++++++++++++++++++++++++++++++++++++++- 1 file changed, 71 insertions(+), 1 deletion(-) diff --git a/Tools/sdlog2_dump.py b/Tools/sdlog2_dump.py index 838da9c489..0ffb381766 100644 --- a/Tools/sdlog2_dump.py +++ b/Tools/sdlog2_dump.py @@ -21,6 +21,17 @@ __version__ = "1.2" import struct, sys +from pypreprocessor import pypreprocessor + +#exclude +if sys.version[:3].split('.')[0] == '2': + pypreprocessor.defines.append('python2') +if sys.version[:3].split('.')[0] == '3': + pypreprocessor.defines.append('python3') + +pypreprocessor.parse() +#endexclude + class SDLog2Parser: BLOCK_SIZE = 8192 MSG_HEADER_LEN = 3 @@ -65,7 +76,11 @@ class SDLog2Parser: self.__msg_descrs = {} # message descriptions by message type map self.__msg_labels = {} # message labels by message name map self.__msg_names = [] # message names in the same order as FORMAT messages - self.__buffer = "" # buffer for input binary data + #ifdef python2 + self.__buffer = "" # buffer, python2 + #else + self.__buffer = bytearray() # buffer for input binary data + #endif self.__ptr = 0 # read pointer in buffer self.__csv_columns = [] # CSV file columns in correct order in format "MSG.label" self.__csv_data = {} # current values for all columns @@ -114,15 +129,24 @@ class SDLog2Parser: self.__buffer = self.__buffer[self.__ptr:] + chunk self.__ptr = 0 while self.__bytesLeft() >= self.MSG_HEADER_LEN: + #ifdef python2 head1 = ord(self.__buffer[self.__ptr]) head2 = ord(self.__buffer[self.__ptr+1]) + #else + head1 = self.__buffer[self.__ptr] + head2 = self.__buffer[self.__ptr+1] + #endif if (head1 != self.MSG_HEAD1 or head2 != self.MSG_HEAD2): if self.__correct_errors: self.__ptr += 1 continue else: raise Exception("Invalid header at %i (0x%X): %02X %02X, must be %02X %02X" % (bytes_read + self.__ptr, bytes_read + self.__ptr, head1, head2, self.MSG_HEAD1, self.MSG_HEAD2)) + #ifdef python2 msg_type = ord(self.__buffer[self.__ptr+2]) + #else + msg_type = self.__buffer[self.__ptr+2] + #endif if msg_type == self.MSG_TYPE_FORMAT: # parse FORMAT message if self.__bytesLeft() < self.MSG_FORMAT_PACKET_LEN: @@ -170,7 +194,11 @@ class SDLog2Parser: if self.__file != None: print >> self.__file, self.__csv_delim.join(self.__csv_columns) else: + #ifdef python2 print self.__csv_delim.join(self.__csv_columns) + #else + print(self.__csv_delim.join(self.__csv_columns)) + #endif def __printCSVRow(self): s = [] @@ -185,16 +213,26 @@ class SDLog2Parser: if self.__file != None: print >> self.__file, self.__csv_delim.join(s) else: + #ifdef python2 print self.__csv_delim.join(s) + #else + print(self.__csv_delim.join(s)) + #endif def __parseMsgDescr(self): data = struct.unpack(self.MSG_FORMAT_STRUCT, self.__buffer[self.__ptr + 3 : self.__ptr + self.MSG_FORMAT_PACKET_LEN]) msg_type = data[0] if msg_type != self.MSG_TYPE_FORMAT: msg_length = data[1] + #ifdef python2 msg_name = data[2].strip("\0") msg_format = data[3].strip("\0") msg_labels = data[4].strip("\0").split(",") + #else + msg_name = str(data[2], 'ascii').strip("\0") + msg_format = str(data[3], 'ascii').strip("\0") + msg_labels = str(data[4], 'ascii').strip("\0").split(",") + #endif # Convert msg_format to struct.unpack format string msg_struct = "" msg_mults = [] @@ -211,8 +249,13 @@ class SDLog2Parser: self.__msg_names.append(msg_name) if self.__debug_out: if self.__filterMsg(msg_name) != None: + #ifdef python2 print "MSG FORMAT: type = %i, length = %i, name = %s, format = %s, labels = %s, struct = %s, mults = %s" % ( msg_type, msg_length, msg_name, msg_format, str(msg_labels), msg_struct, msg_mults) + #else + print("MSG FORMAT: type = %i, length = %i, name = %s, format = %s, labels = %s, struct = %s, mults = %s" % ( + msg_type, msg_length, msg_name, msg_format, str(msg_labels), msg_struct, msg_mults)) + #endif self.__ptr += self.MSG_FORMAT_PACKET_LEN def __parseMsg(self, msg_descr): @@ -223,7 +266,11 @@ class SDLog2Parser: show_fields = self.__filterMsg(msg_name) if (show_fields != None): data = list(struct.unpack(msg_struct, self.__buffer[self.__ptr+self.MSG_HEADER_LEN:self.__ptr+msg_length])) + #ifdef python2 for i in xrange(len(data)): + #else + for i in range(len(data)): + #endif if type(data[i]) is str: data[i] = data[i].strip("\0") m = msg_mults[i] @@ -231,14 +278,26 @@ class SDLog2Parser: data[i] = data[i] * m if self.__debug_out: s = [] + #ifdef python2 for i in xrange(len(data)): + #else + for i in range(len(data)): + #endif label = msg_labels[i] if show_fields == "*" or label in show_fields: s.append(label + "=" + str(data[i])) + #ifdef python2 print "MSG %s: %s" % (msg_name, ", ".join(s)) + #else + print("MSG %s: %s" % (msg_name, ", ".join(s))) + #endif else: # update CSV data buffer + #ifdef python2 for i in xrange(len(data)): + #else + for i in range(len(data)): + #endif label = msg_labels[i] if label in show_fields: self.__csv_data[msg_name + "_" + label] = data[i] @@ -250,6 +309,7 @@ class SDLog2Parser: def _main(): if len(sys.argv) < 2: + #ifdef python2 print "Usage: python sdlog2_dump.py [-v] [-e] [-d delimiter] [-n null] [-m MSG[.field1,field2,...]] [-t TIME_MSG_NAME]\n" print "\t-v\tUse plain debug output instead of CSV.\n" print "\t-e\tRecover from errors.\n" @@ -258,6 +318,16 @@ def _main(): print "\t-m MSG[.field1,field2,...]\n\t\tDump only messages of specified type, and only specified fields.\n\t\tMultiple -m options allowed." print "\t-t\tSpecify TIME message name to group data messages by time and significantly reduce duplicate output.\n" print "\t-fPrint to file instead of stdout" + #else + print("Usage: python sdlog2_dump.py [-v] [-e] [-d delimiter] [-n null] [-m MSG[.field1,field2,...]] [-t TIME_MSG_NAME]\n") + print("\t-v\tUse plain debug output instead of CSV.\n") + print("\t-e\tRecover from errors.\n") + print("\t-d\tUse \"delimiter\" in CSV. Default is \",\".\n") + print("\t-n\tUse \"null\" as placeholder for empty values in CSV. Default is empty.\n") + print("\t-m MSG[.field1,field2,...]\n\t\tDump only messages of specified type, and only specified fields.\n\t\tMultiple -m options allowed.") + print("\t-t\tSpecify TIME message name to group data messages by time and significantly reduce duplicate output.\n") + print("\t-fPrint to file instead of stdout") + #endif return fn = sys.argv[1] debug_out = False From 487497d66eab5846f133ed2da1a1f72356c24668 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 12 Aug 2013 23:50:29 +0200 Subject: [PATCH 18/18] Added Python 2 / 3 Windows / Linux / Mac OS converter script. So much for cross-platform / version agnostic --- Tools/README.txt | 13 +++++++ Tools/sdlog2_dump.py | 86 +++++--------------------------------------- 2 files changed, 22 insertions(+), 77 deletions(-) create mode 100644 Tools/README.txt diff --git a/Tools/README.txt b/Tools/README.txt new file mode 100644 index 0000000000..abeb9a4c71 --- /dev/null +++ b/Tools/README.txt @@ -0,0 +1,13 @@ +====== PX4 LOG CONVERSION ====== + +On each log session (commonly started and stopped by arming and disarming the vehicle) a new file logxxx.bin is created. In many cases there will be only one logfile named log001.bin (only one flight). + +There are two conversion scripts in this ZIP file: + +logconv.m: This is a MATLAB script which will automatically convert and display the flight data with a GUI. If running this script, the second script can be ignored. + +sdlog2_dump.py: This is a Python script (compatible with v2 and v3) which converts the self-describing binary log format to a CSV file. To export a CSV file from within a shell (Windows CMD or BASH on Linux / Mac OS), run: + +python sdlog2_dump.py log001.bin -f "export.csv" -t "TIME" -d "," -n "" + +Python can be downloaded from http://python.org, but is available as default on Mac OS and Linux. \ No newline at end of file diff --git a/Tools/sdlog2_dump.py b/Tools/sdlog2_dump.py index 0ffb381766..bb109d6f3f 100644 --- a/Tools/sdlog2_dump.py +++ b/Tools/sdlog2_dump.py @@ -1,6 +1,8 @@ #!/usr/bin/env python -"""Dump binary log generated by sdlog2 or APM as CSV +from __future__ import print_function + +"""Dump binary log generated by PX4's sdlog2 or APM as CSV Usage: python sdlog2_dump.py [-v] [-e] [-d delimiter] [-n null] [-m MSG[.field1,field2,...]] @@ -21,17 +23,6 @@ __version__ = "1.2" import struct, sys -from pypreprocessor import pypreprocessor - -#exclude -if sys.version[:3].split('.')[0] == '2': - pypreprocessor.defines.append('python2') -if sys.version[:3].split('.')[0] == '3': - pypreprocessor.defines.append('python3') - -pypreprocessor.parse() -#endexclude - class SDLog2Parser: BLOCK_SIZE = 8192 MSG_HEADER_LEN = 3 @@ -76,11 +67,7 @@ class SDLog2Parser: self.__msg_descrs = {} # message descriptions by message type map self.__msg_labels = {} # message labels by message name map self.__msg_names = [] # message names in the same order as FORMAT messages - #ifdef python2 - self.__buffer = "" # buffer, python2 - #else - self.__buffer = bytearray() # buffer for input binary data - #endif + self.__buffer = "" # buffer for input binary data self.__ptr = 0 # read pointer in buffer self.__csv_columns = [] # CSV file columns in correct order in format "MSG.label" self.__csv_data = {} # current values for all columns @@ -129,24 +116,15 @@ class SDLog2Parser: self.__buffer = self.__buffer[self.__ptr:] + chunk self.__ptr = 0 while self.__bytesLeft() >= self.MSG_HEADER_LEN: - #ifdef python2 head1 = ord(self.__buffer[self.__ptr]) head2 = ord(self.__buffer[self.__ptr+1]) - #else - head1 = self.__buffer[self.__ptr] - head2 = self.__buffer[self.__ptr+1] - #endif if (head1 != self.MSG_HEAD1 or head2 != self.MSG_HEAD2): if self.__correct_errors: self.__ptr += 1 continue else: raise Exception("Invalid header at %i (0x%X): %02X %02X, must be %02X %02X" % (bytes_read + self.__ptr, bytes_read + self.__ptr, head1, head2, self.MSG_HEAD1, self.MSG_HEAD2)) - #ifdef python2 msg_type = ord(self.__buffer[self.__ptr+2]) - #else - msg_type = self.__buffer[self.__ptr+2] - #endif if msg_type == self.MSG_TYPE_FORMAT: # parse FORMAT message if self.__bytesLeft() < self.MSG_FORMAT_PACKET_LEN: @@ -192,13 +170,9 @@ class SDLog2Parser: self.__csv_columns.append(full_label) self.__csv_data[full_label] = None if self.__file != None: - print >> self.__file, self.__csv_delim.join(self.__csv_columns) + print(self.__csv_delim.join(self.__csv_columns), file=self.__file) else: - #ifdef python2 - print self.__csv_delim.join(self.__csv_columns) - #else print(self.__csv_delim.join(self.__csv_columns)) - #endif def __printCSVRow(self): s = [] @@ -211,28 +185,18 @@ class SDLog2Parser: s.append(v) if self.__file != None: - print >> self.__file, self.__csv_delim.join(s) + print(self.__csv_delim.join(s), file=self.__file) else: - #ifdef python2 - print self.__csv_delim.join(s) - #else print(self.__csv_delim.join(s)) - #endif def __parseMsgDescr(self): data = struct.unpack(self.MSG_FORMAT_STRUCT, self.__buffer[self.__ptr + 3 : self.__ptr + self.MSG_FORMAT_PACKET_LEN]) msg_type = data[0] if msg_type != self.MSG_TYPE_FORMAT: msg_length = data[1] - #ifdef python2 - msg_name = data[2].strip("\0") - msg_format = data[3].strip("\0") - msg_labels = data[4].strip("\0").split(",") - #else - msg_name = str(data[2], 'ascii').strip("\0") - msg_format = str(data[3], 'ascii').strip("\0") - msg_labels = str(data[4], 'ascii').strip("\0").split(",") - #endif + msg_name = str(data[2]).strip("\0") + msg_format = str(data[3]).strip("\0") + msg_labels = str(data[4]).strip("\0").split(",") # Convert msg_format to struct.unpack format string msg_struct = "" msg_mults = [] @@ -249,13 +213,8 @@ class SDLog2Parser: self.__msg_names.append(msg_name) if self.__debug_out: if self.__filterMsg(msg_name) != None: - #ifdef python2 - print "MSG FORMAT: type = %i, length = %i, name = %s, format = %s, labels = %s, struct = %s, mults = %s" % ( - msg_type, msg_length, msg_name, msg_format, str(msg_labels), msg_struct, msg_mults) - #else print("MSG FORMAT: type = %i, length = %i, name = %s, format = %s, labels = %s, struct = %s, mults = %s" % ( msg_type, msg_length, msg_name, msg_format, str(msg_labels), msg_struct, msg_mults)) - #endif self.__ptr += self.MSG_FORMAT_PACKET_LEN def __parseMsg(self, msg_descr): @@ -266,11 +225,7 @@ class SDLog2Parser: show_fields = self.__filterMsg(msg_name) if (show_fields != None): data = list(struct.unpack(msg_struct, self.__buffer[self.__ptr+self.MSG_HEADER_LEN:self.__ptr+msg_length])) - #ifdef python2 - for i in xrange(len(data)): - #else for i in range(len(data)): - #endif if type(data[i]) is str: data[i] = data[i].strip("\0") m = msg_mults[i] @@ -278,26 +233,14 @@ class SDLog2Parser: data[i] = data[i] * m if self.__debug_out: s = [] - #ifdef python2 - for i in xrange(len(data)): - #else for i in range(len(data)): - #endif label = msg_labels[i] if show_fields == "*" or label in show_fields: s.append(label + "=" + str(data[i])) - #ifdef python2 - print "MSG %s: %s" % (msg_name, ", ".join(s)) - #else print("MSG %s: %s" % (msg_name, ", ".join(s))) - #endif else: # update CSV data buffer - #ifdef python2 - for i in xrange(len(data)): - #else for i in range(len(data)): - #endif label = msg_labels[i] if label in show_fields: self.__csv_data[msg_name + "_" + label] = data[i] @@ -309,16 +252,6 @@ class SDLog2Parser: def _main(): if len(sys.argv) < 2: - #ifdef python2 - print "Usage: python sdlog2_dump.py [-v] [-e] [-d delimiter] [-n null] [-m MSG[.field1,field2,...]] [-t TIME_MSG_NAME]\n" - print "\t-v\tUse plain debug output instead of CSV.\n" - print "\t-e\tRecover from errors.\n" - print "\t-d\tUse \"delimiter\" in CSV. Default is \",\".\n" - print "\t-n\tUse \"null\" as placeholder for empty values in CSV. Default is empty.\n" - print "\t-m MSG[.field1,field2,...]\n\t\tDump only messages of specified type, and only specified fields.\n\t\tMultiple -m options allowed." - print "\t-t\tSpecify TIME message name to group data messages by time and significantly reduce duplicate output.\n" - print "\t-fPrint to file instead of stdout" - #else print("Usage: python sdlog2_dump.py [-v] [-e] [-d delimiter] [-n null] [-m MSG[.field1,field2,...]] [-t TIME_MSG_NAME]\n") print("\t-v\tUse plain debug output instead of CSV.\n") print("\t-e\tRecover from errors.\n") @@ -327,7 +260,6 @@ def _main(): print("\t-m MSG[.field1,field2,...]\n\t\tDump only messages of specified type, and only specified fields.\n\t\tMultiple -m options allowed.") print("\t-t\tSpecify TIME message name to group data messages by time and significantly reduce duplicate output.\n") print("\t-fPrint to file instead of stdout") - #endif return fn = sys.argv[1] debug_out = False