From f3bfbd87b1f6faef6bac75c9f94b590bb8b504b6 Mon Sep 17 00:00:00 2001 From: James Goppert Date: Fri, 7 Jun 2013 14:02:18 -0400 Subject: [PATCH 01/25] 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/25] 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/25] 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/25] 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/25] 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/25] 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/25] 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/25] 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/25] 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/25] 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/25] 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/25] 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/25] 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/25] 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/25] 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/25] 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/25] 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/25] 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 From b3d2efc90af671f13f8f473230e3af6e8d91153c Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 13 Aug 2013 07:26:05 +0200 Subject: [PATCH 19/25] WIP --- Tools/sdlog2_dump.py | 21 +++++++++++++++------ 1 file changed, 15 insertions(+), 6 deletions(-) diff --git a/Tools/sdlog2_dump.py b/Tools/sdlog2_dump.py index bb109d6f3f..8746a99479 100644 --- a/Tools/sdlog2_dump.py +++ b/Tools/sdlog2_dump.py @@ -23,6 +23,11 @@ __version__ = "1.2" import struct, sys +if sys.hexversion >= 0x030000F0: + runningPython3 = True +else: + runningPython3 = False + class SDLog2Parser: BLOCK_SIZE = 8192 MSG_HEADER_LEN = 3 @@ -67,7 +72,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 - self.__buffer = "" # buffer for input binary data + self.__buffer = bytearray() # 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 @@ -116,15 +121,15 @@ class SDLog2Parser: self.__buffer = self.__buffer[self.__ptr:] + chunk self.__ptr = 0 while self.__bytesLeft() >= self.MSG_HEADER_LEN: - head1 = ord(self.__buffer[self.__ptr]) - head2 = ord(self.__buffer[self.__ptr+1]) + head1 = self.__buffer[self.__ptr] + head2 = self.__buffer[self.__ptr+1] 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)) - msg_type = ord(self.__buffer[self.__ptr+2]) + msg_type = self.__buffer[self.__ptr+2] if msg_type == self.MSG_TYPE_FORMAT: # parse FORMAT message if self.__bytesLeft() < self.MSG_FORMAT_PACKET_LEN: @@ -190,8 +195,12 @@ class SDLog2Parser: print(self.__csv_delim.join(s)) def __parseMsgDescr(self): - data = struct.unpack(self.MSG_FORMAT_STRUCT, self.__buffer[self.__ptr + 3 : self.__ptr + self.MSG_FORMAT_PACKET_LEN]) + if runningPython3: + data = struct.unpack(self.MSG_FORMAT_STRUCT, self.__buffer[self.__ptr + 3 : self.__ptr + self.MSG_FORMAT_PACKET_LEN]) + else: + data = struct.unpack(self.MSG_FORMAT_STRUCT, self.__buffer[self.__ptr + 3 : self.__ptr + self.MSG_FORMAT_PACKET_LEN]) msg_type = data[0] + print(msg_type) if msg_type != self.MSG_TYPE_FORMAT: msg_length = data[1] msg_name = str(data[2]).strip("\0") @@ -224,7 +233,7 @@ class SDLog2Parser: self.__csv_updated = False 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])) + data = list(struct.unpack(msg_struct, str(bytearray(self.__buffer[self.__ptr+self.MSG_HEADER_LEN:self.__ptr+msg_length])))) for i in range(len(data)): if type(data[i]) is str: data[i] = data[i].strip("\0") From da9d9781f999361009b2c7b8e9b18d0d500c072c Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 13 Aug 2013 07:33:32 +0200 Subject: [PATCH 20/25] Final version of log conversion script, runs with Python 2 or 3 on Windows, Linux and MacOS. Tested on Mac with 2 and 3 --- Tools/sdlog2_dump.py | 18 +++++++++++++----- 1 file changed, 13 insertions(+), 5 deletions(-) diff --git a/Tools/sdlog2_dump.py b/Tools/sdlog2_dump.py index 8746a99479..a376e03d39 100644 --- a/Tools/sdlog2_dump.py +++ b/Tools/sdlog2_dump.py @@ -198,14 +198,19 @@ class SDLog2Parser: if runningPython3: data = struct.unpack(self.MSG_FORMAT_STRUCT, self.__buffer[self.__ptr + 3 : self.__ptr + self.MSG_FORMAT_PACKET_LEN]) else: - data = struct.unpack(self.MSG_FORMAT_STRUCT, self.__buffer[self.__ptr + 3 : self.__ptr + self.MSG_FORMAT_PACKET_LEN]) + data = struct.unpack(self.MSG_FORMAT_STRUCT, str(self.__buffer[self.__ptr + 3 : self.__ptr + self.MSG_FORMAT_PACKET_LEN])) msg_type = data[0] print(msg_type) if msg_type != self.MSG_TYPE_FORMAT: msg_length = data[1] - msg_name = str(data[2]).strip("\0") - msg_format = str(data[3]).strip("\0") - msg_labels = str(data[4]).strip("\0").split(",") + if runningPython3: + 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(",") + else: + 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 = [] @@ -233,7 +238,10 @@ class SDLog2Parser: self.__csv_updated = False show_fields = self.__filterMsg(msg_name) if (show_fields != None): - data = list(struct.unpack(msg_struct, str(bytearray(self.__buffer[self.__ptr+self.MSG_HEADER_LEN:self.__ptr+msg_length])))) + if runningPython3: + data = list(struct.unpack(msg_struct, self.__buffer[self.__ptr+self.MSG_HEADER_LEN:self.__ptr+msg_length])) + else: + data = list(struct.unpack(msg_struct, str(self.__buffer[self.__ptr+self.MSG_HEADER_LEN:self.__ptr+msg_length]))) for i in range(len(data)): if type(data[i]) is str: data[i] = data[i].strip("\0") From 0025e9ca909d4e1c8bc63841cd0e61265a328e30 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 13 Aug 2013 07:57:39 +0200 Subject: [PATCH 21/25] Hotfix: Copy a current version of the log conversion tools to each log directory --- ROMFS/px4fmu_common/logging/log_converter.zip | Bin 0 -> 10090 bytes ROMFS/px4fmu_common/logging/logconv.m | 586 ------------------ src/modules/sdlog2/sdlog2.c | 9 + 3 files changed, 9 insertions(+), 586 deletions(-) create mode 100644 ROMFS/px4fmu_common/logging/log_converter.zip delete mode 100644 ROMFS/px4fmu_common/logging/logconv.m diff --git a/ROMFS/px4fmu_common/logging/log_converter.zip b/ROMFS/px4fmu_common/logging/log_converter.zip new file mode 100644 index 0000000000000000000000000000000000000000..2641e671068b1ea6867fc6b9fef17ba3889e5262 GIT binary patch literal 10090 zcmaKSV~i&3w&hzkx@_CFZQHhuF1xy1UGB1NTV1wo`z?F&ok?cS+%xw+$xfahd#_|C z>(AP2DanF?qX7T_Xuu{ff@mT8c8nK103gi*03ZVJ0Ip_sj+QKjX72V*3{KwaYA^t> z_#qgTx4$qd?p|;J5b#G30N`Kq-8i3)8!f<-w^}1~@Mg_n+07nfxKyYj`3`j3^(tw? zCk)(R64e$1bD4z*Rnyh)uWPC3A}SgAWfPIzRle=JYp*Uw=bO7$ckQ3PS#^%o8%aMk zt64d4hQ(Wmg^^d(^=FBQ#wcc@qYuRgXt2zcQ+C;A?x)Y?6#)En9%NG*X5!fN1JMj) zS8;k@Pao3K*}m^JX;u))w~nx4Z*l z3*1ZIOFfxvCPh|J0mPSC=#q8x4#(mCd1;pQpIJBW!}|c%0G@0H3={efu2mFZei51C zU?@obhL3nd$M{7wSyh!)8<}>@r98xRA=uj~e{2IiJ+vkaeRLI7Ddcc*-YInrAJMRM z0xM30J8Y5c{%cOu5E%;aI$t6%KeDFLEebBE;FO})!wk2ConHTeYqMyTmLKY1kdwyr zEY8j<%7;;TnWa=2bRn&1{b-SrST|Z4r!Pu6jO(-!oG%@4TX+VaP!yJ;t}L0NIIa(4%KVjE zb4D2@^yMdlD2Ok>8+x*|$81?%hAq2#p0qK`O5~-#=q9$1^weh<2EdIiV55_Daesoe zyJn%2(TRLz-0t#ym34Z2dv;BK|C~c&6@Xnw@D~u|=M5n2;tqgIu&mxA4UWlffwKI0 z8xc_6uX(d=_{RV$t&}l4PCFX{AjaXZ#HHH5a?UormSP)BAnu-1Z zHmV$vvxa(iq6XbkWIlxbcFzd1VfzPvFHfwaQuheFeSQQl4j6r7<)M^%L+wyKW`AS|2%{Hwna>UhC*-09UBGOCWY=fDRt;DS{s zHNey`w%Y|?IZKhK85tm@Geb~@4QiYtTvaM1meiCu*HJ)M%{hr|k0gq|m$lUw7|4c2 zjROzx%&H3xae7?%T{-WnESZXMMfz9|DJE?dFw4UCmMEyWWd51)z|@*0{is&dMVOto)>D1@l~!;THgX_Q;W zy3uTY&sBNDkm+b_9fC*T!O4CGnOTdkgL!n0j;XD(VB?(2b|mr;k2q!PL?fiBVy7pU zmb3TspBBs0=6&Gtn{B?#}{@DVg=px5GGTHfY zVws9~GU)~7iW?INl#3rgEid^fI}J zI#yk6gC=^uYeaC&qLH4>$=2CeiP<>!UoNVEr zBjA7HTkiK58f}qZtpba~P)#`k?;gs2f!W~xN+HnwiErFaEw!GvRy0vVcFcEqy_O1^ zo0Cwu?XCQ`A{1M~Lf|dEBZovTPl@jvO*yASsAY`kfn1gWw*&@#O>y+)cHzr0*|Z^m z>peCg5UiG%U`vJnFwu{NL++)#zemvrM6Y{)XR_7paw9t1?XYifRJAH7Hk+GM!xZP; zQcchy`zjI7mYde|ZPGsr@~hCGO10+EQT5r5Ly?ry1)~2Jeqq*GQ}I({R%$3YpV(Us zw938Obw5iYTVG#RAM4bj&zT>;@S{(l;WCaGAx9~MoWja+szBcxH@yF%ny8p(Zej=T zso`{!LH_8*4k^CLg71_vxZFL7!8>^4uw|8D&spIz4t-}*5*U{p6gUW)S&EvoZe1B- zVXA9dJjmgGZpTfSJ?rBCS!SG#e&OhERpJW3D3034xYbb=^4Ix$0pO5H2$Ly zZ*k+I3feo*?llR9yRMjJG-^`lBwyag$bw7a7^E1;nNyB!+U`*R(m#v1U@xLjbp{Kh z8V;e)%C6N?XVHl({w;v0uN|7q(FrAH~;LeyKI%n|+nO3Af_Q6i?Krsr_>vpXEP0e0yDYk~a zxn`xV;s}@D)7V27=L*NX??8ZYCjMfTEN1ECAwYZJuRaB3K_54>lr6)S@>LL=oWx&h zCcmgHsCfkXW?dh`rbq0ijV}9U_Vx$stn(H@C7s`D!~9j4aq8#?)#i1v_;$qnR}98&?Y|>)hFsFRCn;&CIAXtt_F`HHK(`U`%cW zRsekto2mrG6#ar!@=%FeGCk`r|DU;Z9Vv-->Tx_4f=%`a2y071;!ZS;VTw7S=D*1l zDO9oKf%Tk%51U89mk2ngGX%jknJuuN-bimvlM6*SzCGe&_OA}@UpZU>iqXEzy^;nN zcCx?5^}5E_OEw-16A5(P<%A}e91-MJOB84)7WPB7IYcNcHLoh_XgsbTcpN^8KXEQ7 zTrj~P(-P+Y`mu+YXoaaTSxMdv(_#bRw(fF>p0<4}2qlkHm$`e|hcJsp%IA3K@&NlV ziVH^(h6kT!1GcxLoG4`OR+M0!x1<&be~}%PEXBoO(uGpy!I0@;v<)*&H3NlQsuVk> zViN{9t7bWupPChX7ifZ(WXd0$2vHP;vh2@~M_zumoUG;)p$3T82rBH8Zc_CmCtQV? zlHgza@1q`J-bJ{9zvtn)_uNJBw7iJg6Mb1tPx+6<=CyvX(hGGZeSc`=$Is&AJY1zM zTXZ@v5e8i1h^c3G{6={!(Kyv)qHDyx9cHia*r3ezHq;x$L4`DAZ->Q*@Xj2FlP9B| zEmCFhW5Al|f-qEw-5C2-{<=N=r*g?Q@X>&g;u$9Y`Ot#M&j(pDSs_$rmJ*X#ca9lj zYIJ634gqp_e-71|Q(Nx#FM`w26OM~YG86f)y;X+TEL&O^)-)mlnPs-$5mPBtd+VEzsz3{ueic_G_W1NrUL6<2RARO>a_{>sQy>cxch)u=Lf--xfyT54Q*Au^`O`yX_pms2L9fX>cr+TDejFb&ahTPJ=mlJ&^`XY_x;!|G z6s1C|(Ug!Z7s9E*Nc$RydZPFtZS`UAql>FV$H$GIUiXjE_o2ad3ysitQHgHV#69ZV z=Xd+n?sc3?E8zVjchjI*7S*088da@wV$hBe0Y+$7kSWe}?pA|MUkbPT9crhG&7V)q zukZ+IpD^5gY*4p)C6sro$T<@LvTY0K8S@&HTAYDK`K7*J-O8vbv8YgiubX<+N1Vql z%m!lxoy?4Z6GcLOB~R7-eulNYzjg*yIdAFOdUP$javG1>JdYOXx#4QAf1hobEyKHu zD(u8IzS?d1Hac`=xMoclF=yS%zJM4aJ6?o9g5~^x2wJS0jFupRE?4}vjvG=*J5cH0vyD(;Dk5V( zs|q9${WfOYb2aqXNq^)tY_ZtHr8kv|;*bPge810}`o4NA&!N_eq@olK@esL;N#-uE z&nwO=&VAUp$yu~@1lni8bbHB72OF0`nB5{aGszyNuwnE}s+Vgp6O62rbSLJzB~3bi zK{Rchd04#-Wt(o)_Wm9{B8;%a)Esg#T9iX_Ypa#qU2eezXOi)}&22 z2f8Q{#tpsR4o)4tIQ?fe9#$zImq)B{ZsFefFCb+cD} zj`X(liSM6x!RY-mS%vi9v)^jDn}3w|Xg6GxG26_NdkaB2_#7xskf%Vl<<}~R;p@nk z{E+;fW&>TR+#>0v-yoA+02}L@ki9Jb0by2Z;An(&L^drwpQfhV;Nwr$J=Q z1R{eJCQ$;|;6zx-cwOoJ0u8B)*AOKBP|cL8tGfyWg?U)Q0hyu;u)m0>nezjnCTvLZ za6@-WmbkNc^7n1tgPE)!kGp}o2pLsj?&`gX9_yjhDCSHoqpD_1UuL=-EyBwol`)Nd z&aaC@A=SXR$^tU3(^*HH}!Ka07)9!jy zgv*eQ+jPr44p2G=p}$Kim)NJ-^z~({IkccxwCf0NpJPjRcB=GuMD}`N@M(=gERIQV zFz!TbA1X$ekd1W0PokF!!S;(cTOXcHJ0ZrSBqnxl(Ck1dQ+XR${4vC&Bu0tf{O2*rdoQv z@ezr1B$TTD5>=y*yKMG=aJ zZ~3-92lKV$9XKI@Uz66%ZA;C$u3Q_fQJW7zlNc2R8i~t7=Vo-)@+e>G`{Q!djz~NB zAqab!Uon78{gCmReRemrQn#*zm5CIw%m_=%(@5G$<}+v}!Ygv@1=b-O1( z?B#-bsM7JJCbd1f2}F;5w?ojIh=Bz6)0-ljlPd>sdkrm8`=_IRU%tbvv!oylv4okJU16HBJ~=KsZ1Ir_T?XkRyEnZ_8&756PhMl9k}8$@S}aZuspoPVC42xWqfm*>gBx z`ARmt4us4xqz1aLxi830;G|`aZV45Hs@TsZ1x_YHR}2WwYSpu7i7Fj z#eKwq1iEI~R|gCxiCMpYS5~C$%T|W9E-hYlwErShrfv!U;Ku1<6vo4eqg5n0!i(h8 z?Tr|z-ZmF;0;v5y!F$W`kaI}GtI?}4cW^!mM8I#Eu*ZRTq>%nx_(}DQ7h5EbF>h^c zQDj(>-I>6i<0(45YTFA*U}@4frAqewPT=R{YJ7-bv<$UI6q~7B<~Xz_X%lVHKq91| z3N)rsPDJymh-M%p56%Ek&JW`mgNWLGKruY7-!A8}jO2T58NKtz*SvXR-)4zS=<3MT z@6-fWx&^RO0_GRi_A3NzL0F{zu)e2*L6(T`24jtQ&bMln&ySD;9||UaW%NOVqywK# z`ksNZM!wkPj~HcmS_O;zQdV2_%w}0u706N^c)8xGkh5h1mS=3#E5*?)s?Nvz%j*vWC7KuzkOqQ4WElLzjyd z#>ET;;*y?6ggT~^MeYdxb8L!Cem)T+F&SDWVGXvy;_GyetUOc)@SLg6^*8#_%w?

A{m!XO)hr8(xGBB;%}2kAZgk#{>7 zxQr$=%zt8~Q9CWqPJ`7Et%#oBcZ>Vl3hE~sTp(CZL#_;Uk@{r1c45H9vj>2vAClZ7 z*iO|nV7IV&rku}uOgDO#!q%2n2pm{!=^&!@;$5=C2^$(uEa{My(94y0E`6Y{C7gWeO58d%oM z5A*;U4gf8OVNGg_Nl>ca9o3^nWUJ*gg!(gYkggXVP%SQ&;=a@^+(=}`M;@k_9aXq` zp?4%jY+*WhL44$)gK^a5OE&e7N_|mEYKb|j8a#kQZXWe~a+30;S^IYl&K{ccTw~rs zf$+AJ2@qkjwgy{!W5^=}8}%bli)yud91vlUp_%IeaAWv!2s*39b0{us?tRP^=G=1a z?P2rF--kA`$2Mk1*uK=G@XTZR;q^^PI-xs3A#h0d_4+Fw&S5uc80z0Q=H4>mUYYF0 z_#fAZS6Y;-`w}z0w(sSA9CQ2*BlJVsA!E0y)6dZDT$`~|# zINHR7U3@pp)LpdxVY3XyFVTh%{-ILwb(aRQ%q_o6sn)h!4Y4wMO3|`M$p*>%ZG{2V zFrSb%{%H5Ve{V%iy0d`;Cf&j=!u=Kt-)jAFn|Rl1Msi+4e!Zb1qT`tPu@Gn@RV2(y zy3hGZW(Fkhy&+)(QLcK9Vd_M#sfV`J+hi}b2EM*>y)|nBRUE4p)g0$Z3bUKkko9za zHivBuFb)pycB+^e8Cm}B^J-!eAYAP!w3{?`aKcGn-^8wF_kN-+!4Gx_w~(aCCWn~h z{6Dsa)C5f(AS%lQN$ZnFgT>T4%z$Y}23qw=6xvFRKXocciqR^2=eui5B0THZusict zvk*!i=TN@%bCzMmTyFi`;)6L z?I>8@ycJ_)Op*ue91RFvZ4--z@k!cr?wypD%%O~EE9>WocL&+=X-aYaXI}QSjwWw2 zR|Nd`#B2xApIXu;3J<(npeJJ-g+RkKB+aAPWCy0>V?Ctt{Xk7!yxR&jnwtxB;64Pk zY$A}$<(iSfYx}FR!JqkRWa;ncsnvP*rFMssdXFHoOQY6u0Nr@sUnIuc_Y45QuObtPJ<8Z(hqg;0Wr!b~BrVP}Nj&EABO`~)!|H}v{g#y^=u z?x4SWsI|>nVUxF*V@S+ITmPwjvsNV-crX$V#sXjW-EU8Vwd!m$LJW%c!NL=aeB5_* zV!U_s(huY-l$cutPlV2k#RW^K_Da9D-Tg0+-FJ446*Q1DbDQ&O(^0D< zE*^!&wuRO_(%0e;Og_=&wZ(SbLO)D8L$l3BCr|G1r$r!OAlHC&q_84oDdz7Y`x;MY zvHF21lKcokPwzCp#H+a!7JN0#f z>gi^0e);DXZx=l^+?5#cS}oy3#1^(@tRqU%29w>^gviB{l;3j=fq_~%ARvo9uH2-g ztMgHynOg)TtJlIcR_G>ZZWT>cxz4cB zs8^%kKLX+%AN|r(uWsjJfaG2RBV%F)vj|>Tt$9DZp9kLPKj7Z z%fHmZq-h`!zF`e<*$1uf>-=Z=cu2|(c_vs(&N?&;l^vNt)^6f>EK%i@pZs;GDZjDF zdtzMSh87ZJ@?JuVUx}8ZCMv_XPMI&BGr5!&`gDR%pc=qB7(X&b(*0QCXTvD>d@#H{ ze6uw!AQ!RsB*T)%vU0YFAETg5`Z0oBJM^YWr|FG zLv=yYhIeBAM#W(?R#G4iDAQ-z_`8*LFR%NURhko2gg_@b%tpFm+e@3UsY8T;!U1u<-za2di2ISi8 z>SO`ExL&)J!hx%C`~nYlU-|6o5!iVpK*P1sC@t(2gDHtJWVHSsLhwPY#}FYgw46}Y z=&n|JWLAu$sW@@$=vlae`jCKEZQFYauzj^J+7J|Kv+3iJ|Y}48n(cra0P3bc*t+8e5Q!Ega#?h5qb|Q|S82-nqG~B!v{mghV^{ z4c#f?*R7T+>YI>z#st^qOgJ(s-v*WLWN?IPX!WUC-%cr%3XiI6X#ZBkAg%Jwfa1L= z9PJd5@w(ho2O}hOs}HV44G2@dv2RP)MLNTwA@H5;DeU-qm~Z`{Rx$|)he;Yviv@kB zKONw%J76U^RJ*-Ijv9Es>_XC4{_*E;ZF4`zNLT5Rwf$RBI&B22;(7K_i2eu(n2pTE zy)#5NP{B56GriLNwyk(Jjxu(=I+{W%d0eU})Ul=jAXKtqyNl%#*gX_>7&vGthw4=p zJ5C#E!)X~gi9c>hR&psX!RH)aiibM80g#v9vUb?)SD~$WP0xLH*BdFCBg%mZ{-yIE z>Da<#J(rMke9So_1S%!NZ=D$`FC7UbkC&1`(*Pf2DlprM7D-U2l87ADDB&gMZt)fl z8NLLdR@zNHRBVXB%2YS3G1K|VcF<+@h;E`I^i0_y6M7sP(u^hV{F8vvDX+ACVA|ML zriX9{*7Wk&G$az|H#UZ1_uK?$8%U#-F=;@V^B&!b=Pz2*EilpII)+;N;l@ai67h-} zaa6v0>*>s1$;$29u4x8T$!`aWxg)OXNy{c$aYeCX3dXlI17Ab@l`T@1;Zex1$&h35 z`;RY#UvAiyS@(8Hdkfm)eKNh;!SNFSIbLRg!f%4P^WCRN3_Jn5x^z;luKr)XrSB`b zo-Bd~*l9o)uXm)SE-;Qkf_um770=K9rI!!CCi(41P5xs(mwV~78S7b)55HJ$q}*XJ zF>0aQthz>Q*@c($*0rgPoD*wT{3nJzHO_Ui@f*{%`(?5lse%}gOr=z0e@0m3!`yE& zbC`P10s$3iS;HOx2oHm=TdaxEDPoRF_poLo14c=NqnJ~ z3LnfX-;;TZgSp{;v55J4ignpg+7yRJ`@|ISv0odnwez z*fNZ#K>mzv`)Wu}N_aQ-2$I`vY|;(<@uXP_XK)UFK4SwAj_W5k+HE)4gk_AF(Fkeb zh7ffB>K#`zc<_Rk1$W*7vfb7}r_c?RrjRlJXU|_&tO3U&M@92mfn$e=J zGTdAgw)u5B3cP({ZpK_Q(NkFgddur1%usG~|3lQP9oln)*snCbf`?A*mUq!HAJ2iu zKIhC9;=&|nv=+}!I@wMEfiB`ciU$!;nG`~kDT<*#N5dHC6{A@YyT8D8qC`m<4U~jP z6xGMJ+3J3Ev6A7TjuUj8_^oif4kN&9n$-4`wrqji6*|iY@AM|h&U7_y&RV{MT!SNP zVz}4rZ_%Bp7b7!9y6JeeMIEk6Cwe2Bsz|RTj%`0rDvT*rV;~uD%OtR< zvlVq4c6fa3E-M_|**Pz02RMzJP_~_V`X1z3l9Vi8f}n{G6MpVxT9J+5O$7NA%HLah zGcu^*qHE*6u9c6!kJgggsZ8+@x8{@w@Jn->?C>W$&E2}c#gN#qyvU<*)E)Qa1hygA z?m;y4{~jNO`7tVP|2D`vt`_i7?z%1bxr+TK=h;7p@6HV&XH=HL;S@=io@w^7Ioq@2 zCHz2XY{t?C+buL+_5^T3I1N1bH-N7 zA9t+T@Nl|QplmF2?ZlTMV;;f7H>T*$x_93V*%v0Q+s8NH|K8fyy5U6&Xkj<+KmY(0 zFaQ7+fCo?!7Z#HjXK?d!``593O!NPn6H-^vbzEmc^Pj1G`AwNXkg}F2B$*W!c1~;;^Q^(t7 z`FD!aF(~DqWC_!xj0zSVcc+{xOm3+YJ`VS(gC?*wnR8jKsbXBq9yBX)YF6?RA}eQc z;*`@w$5!1W8HBS0ZQ+=7qe|t(>x3T*Ps19!G$|r9$ZfHR%^;$x!;^fzUN0yK6wjn# zNon(n$y7X0@cSc%2rPxV#vz1h+S6e-81menKEX`a9GUD@H}Qn1Q4Mu=)ZdGP&W3bsZo zqP-d3_M6g%85{IpFkwyCsJo2grq1{eQ+Dl$9GcJMcQlo?R3SK~({^S0rRDvlv$HA( zExz&p?pyo4^f@~(MqD+X(eOQnu@oI3B$7+UE zMEMVCKtKt>{=0}77T});0ssJtg8upa*T5OX{|=n}m$2EtWB#Y@^Y55_@PL0efgk|? tGJ*aFxc_AKzr)dy0sbGJLjeB8^Z&qufc$3}FaQ|f9smFsQ~a~#{{rgbAVmNG literal 0 HcmV?d00001 diff --git a/ROMFS/px4fmu_common/logging/logconv.m b/ROMFS/px4fmu_common/logging/logconv.m deleted file mode 100644 index 3750ddae2a..0000000000 --- a/ROMFS/px4fmu_common/logging/logconv.m +++ /dev/null @@ -1,586 +0,0 @@ -% This Matlab Script can be used to import the binary logged values of the -% PX4FMU into data that can be plotted and analyzed. - -%% ************************************************************************ -% PX4LOG_PLOTSCRIPT: Main function -% ************************************************************************ -function PX4Log_Plotscript - -% Clear everything -clc -clear all -close all - -% ************************************************************************ -% SETTINGS -% ************************************************************************ - -% Set the path to your sysvector.bin file here -filePath = 'sysvector.bin'; - -% Set the minimum and maximum times to plot here [in seconds] -mintime=0; %The minimum time/timestamp to display, as set by the user [0 for first element / start] -maxtime=0; %The maximum time/timestamp to display, as set by the user [0 for last element / end] - -%Determine which data to plot. Not completely implemented yet. -bDisplayGPS=true; - -%conversion factors -fconv_gpsalt=1E-3; %[mm] to [m] -fconv_gpslatlong=1E-7; %[gps_raw_position_unit] to [deg] -fconv_timestamp=1E-6; % [microseconds] to [seconds] - -% ************************************************************************ -% Import the PX4 logs -% ************************************************************************ -ImportPX4LogData(); - -%Translate min and max plot times to indices -time=double(sysvector.timestamp) .*fconv_timestamp; -mintime_log=time(1); %The minimum time/timestamp found in the log -maxtime_log=time(end); %The maximum time/timestamp found in the log -CurTime=mintime_log; %The current time at which to draw the aircraft position - -[imintime,imaxtime]=FindMinMaxTimeIndices(); - -% ************************************************************************ -% PLOT & GUI SETUP -% ************************************************************************ -NrFigures=5; -NrAxes=10; -h.figures(1:NrFigures)=0.0; % Temporary initialization of figure handle array - these are numbered consecutively -h.axes(1:NrAxes)=0.0; % Temporary initialization of axes handle array - these are numbered consecutively -h.pathpoints=[]; % Temporary initiliazation of path points - -% Setup the GUI to control the plots -InitControlGUI(); -% Setup the plotting-GUI (figures, axes) itself. -InitPlotGUI(); - -% ************************************************************************ -% DRAW EVERYTHING -% ************************************************************************ -DrawRawData(); -DrawCurrentAircraftState(); - -%% ************************************************************************ -% *** END OF MAIN SCRIPT *** -% NESTED FUNCTION DEFINTIONS FROM HERE ON -% ************************************************************************ - - -%% ************************************************************************ -% IMPORTPX4LOGDATA (nested function) -% ************************************************************************ -% Attention: This is the import routine for firmware from ca. 03/2013. -% Other firmware versions might require different import -% routines. - -function ImportPX4LogData() - % Work around a Matlab bug (not related to PX4) - % where timestamps from 1.1.1970 do not allow to - % read the file's size - if ismac - system('touch -t 201212121212.12 sysvector.bin'); - end - - % ************************************************************************ - % RETRIEVE SYSTEM VECTOR - % ************************************************************************* - % //All measurements in NED frame - % - % uint64_t timestamp; //[us] - % float gyro[3]; //[rad/s] - % float accel[3]; //[m/s^2] - % float mag[3]; //[gauss] - % float baro; //pressure [millibar] - % float baro_alt; //altitude above MSL [meter] - % float baro_temp; //[degree celcius] - % float control[4]; //roll, pitch, yaw [-1..1], thrust [0..1] - % float actuators[8]; //motor 1-8, in motor units (PWM: 1000-2000,AR.Drone: 0-512) - % float vbat; //battery voltage in [volt] - % float bat_current - current drawn from battery at this time instant - % float bat_discharged - discharged energy in mAh - % float adc[4]; //remaining auxiliary ADC ports [volt] - % float local_position[3]; //tangent plane mapping into x,y,z [m] - % int32_t gps_raw_position[3]; //latitude [degrees] north, longitude [degrees] east, altitude above MSL [millimeter] - % float attitude[3]; //pitch, roll, yaw [rad] - % float rotMatrix[9]; //unitvectors - % float actuator_control[4]; //unitvector - % float optical_flow[4]; //roll, pitch, yaw [-1..1], thrust [0..1] - % float diff_pressure; - pressure difference in millibar - % float ind_airspeed; - % float true_airspeed; - - % Definition of the logged values - logFormat{1} = struct('name', 'timestamp', 'bytes', 8, 'array', 1, 'precision', 'uint64', 'machineformat', 'ieee-le.l64'); - logFormat{2} = struct('name', 'gyro', 'bytes', 4, 'array', 3, 'precision', 'float', 'machineformat', 'ieee-le'); - logFormat{3} = struct('name', 'accel', 'bytes', 4, 'array', 3, 'precision', 'float', 'machineformat', 'ieee-le'); - logFormat{4} = struct('name', 'mag', 'bytes', 4, 'array', 3, 'precision', 'float', 'machineformat', 'ieee-le'); - logFormat{5} = struct('name', 'baro', 'bytes', 4, 'array', 1, 'precision', 'float', 'machineformat', 'ieee-le'); - logFormat{6} = struct('name', 'baro_alt', 'bytes', 4, 'array', 1, 'precision', 'float', 'machineformat', 'ieee-le'); - logFormat{7} = struct('name', 'baro_temp', 'bytes', 4, 'array', 1, 'precision', 'float', 'machineformat', 'ieee-le'); - logFormat{8} = struct('name', 'control', 'bytes', 4, 'array', 4, 'precision', 'float', 'machineformat', 'ieee-le'); - logFormat{9} = struct('name', 'actuators', 'bytes', 4, 'array', 8, 'precision', 'float', 'machineformat', 'ieee-le'); - logFormat{10} = struct('name', 'vbat', 'bytes', 4, 'array', 1, 'precision', 'float', 'machineformat', 'ieee-le'); - logFormat{11} = struct('name', 'bat_current', 'bytes', 4, 'array', 1, 'precision', 'float', 'machineformat', 'ieee-le'); - logFormat{12} = struct('name', 'bat_discharged', 'bytes', 4, 'array', 1, 'precision', 'float', 'machineformat', 'ieee-le'); - logFormat{13} = struct('name', 'adc', 'bytes', 4, 'array', 4, 'precision', 'float', 'machineformat', 'ieee-le'); - logFormat{14} = struct('name', 'local_position', 'bytes', 4, 'array', 3, 'precision', 'float', 'machineformat', 'ieee-le'); - logFormat{15} = struct('name', 'gps_raw_position', 'bytes', 4, 'array', 3, 'precision', 'uint32', 'machineformat', 'ieee-le'); - logFormat{16} = struct('name', 'attitude', 'bytes', 4, 'array', 3, 'precision', 'float', 'machineformat', 'ieee-le'); - logFormat{17} = struct('name', 'rot_matrix', 'bytes', 4, 'array', 9, 'precision', 'float', 'machineformat', 'ieee-le'); - logFormat{18} = struct('name', 'vicon_position', 'bytes', 4, 'array', 6, 'precision', 'float', 'machineformat', 'ieee-le'); - logFormat{19} = struct('name', 'actuator_control', 'bytes', 4, 'array', 4, 'precision', 'float', 'machineformat', 'ieee-le'); - logFormat{20} = struct('name', 'optical_flow', 'bytes', 4, 'array', 6, 'precision', 'float', 'machineformat', 'ieee-le'); - logFormat{21} = struct('name', 'diff_pressure', 'bytes', 4, 'array', 1, 'precision', 'float', 'machineformat', 'ieee-le'); - logFormat{22} = struct('name', 'ind_airspeed', 'bytes', 4, 'array', 1, 'precision', 'float', 'machineformat', 'ieee-le'); - logFormat{23} = struct('name', 'true_airspeed', 'bytes', 4, 'array', 1, 'precision', 'float', 'machineformat', 'ieee-le'); - - % First get length of one line - columns = length(logFormat); - lineLength = 0; - - for i=1:columns - lineLength = lineLength + logFormat{i}.bytes * logFormat{i}.array; - end - - - if exist(filePath, 'file') - - fileInfo = dir(filePath); - fileSize = fileInfo.bytes; - - elements = int64(fileSize./(lineLength)); - - fid = fopen(filePath, 'r'); - offset = 0; - for i=1:columns - % using fread with a skip speeds up the import drastically, do not - % import the values one after the other - sysvector.(genvarname(logFormat{i}.name)) = transpose(fread(... - fid, ... - [logFormat{i}.array, elements], [num2str(logFormat{i}.array),'*',logFormat{i}.precision,'=>',logFormat{i}.precision], ... - lineLength - logFormat{i}.bytes*logFormat{i}.array, ... - logFormat{i}.machineformat) ... - ); - offset = offset + logFormat{i}.bytes*logFormat{i}.array; - fseek(fid, offset,'bof'); - end - - % shot the flight time - time_us = sysvector.timestamp(end) - sysvector.timestamp(1); - time_s = time_us*1e-6; - time_m = time_s/60; - - % close the logfile - fclose(fid); - - disp(['end log2matlab conversion' char(10)]); - else - disp(['file: ' filePath ' does not exist' char(10)]); - end -end - -%% ************************************************************************ -% INITCONTROLGUI (nested function) -% ************************************************************************ -%Setup central control GUI components to control current time where data is shown -function InitControlGUI() - %********************************************************************** - % GUI size definitions - %********************************************************************** - dxy=5; %margins - %Panel: Plotctrl - dlabels=120; - dsliders=200; - dedits=80; - hslider=20; - - hpanel1=40; %panel1 - hpanel2=220;%panel2 - hpanel3=3*hslider+4*dxy+3*dxy;%panel3. - - width=dlabels+dsliders+dedits+4*dxy+2*dxy; %figure width - height=hpanel1+hpanel2+hpanel3+4*dxy; %figure height - - %********************************************************************** - % Create GUI - %********************************************************************** - h.figures(1)=figure('Units','pixels','position',[200 200 width height],'Name','Control GUI'); - h.guistatepanel=uipanel('Title','Current GUI state','Units','pixels','Position',[dxy dxy width-2*dxy hpanel1],'parent',h.figures(1)); - h.aircraftstatepanel=uipanel('Title','Current aircraft state','Units','pixels','Position',[dxy hpanel1+2*dxy width-2*dxy hpanel2],'parent',h.figures(1)); - h.plotctrlpanel=uipanel('Title','Plot Control','Units','pixels','Position',[dxy hpanel1+hpanel2+3*dxy width-2*dxy hpanel3],'parent',h.figures(1)); - - %%Control GUI-elements - %Slider: Current time - h.labels.CurTime=uicontrol(gcf,'style','text','Position',[dxy dxy dlabels hslider],'String','Current time t[s]:','parent',h.plotctrlpanel,'HorizontalAlignment','left'); - h.sliders.CurTime=uicontrol(gcf,'style','slider','units','pix','position',[2*dxy+dlabels dxy dsliders hslider],... - 'min',mintime,'max',maxtime,'value',mintime,'callback',@curtime_callback,'parent',h.plotctrlpanel); - temp=get(h.sliders.CurTime,'Max')-get(h.sliders.CurTime,'Min'); - set(h.sliders.CurTime,'SliderStep',[1.0/temp 5.0/temp]); - h.edits.CurTime=uicontrol(gcf,'style','edit','position',[3*dxy+dlabels+dsliders dxy dedits hslider],'String',get(h.sliders.CurTime,'value'),... - 'BackgroundColor','white','callback',@curtime_callback,'parent',h.plotctrlpanel); - - %Slider: MaxTime - h.labels.MaxTime=uicontrol(gcf,'style','text','position',[dxy 2*dxy+hslider dlabels hslider],'String','Max. time t[s] to display:','parent',h.plotctrlpanel,'HorizontalAlignment','left'); - h.sliders.MaxTime=uicontrol(gcf,'style','slider','units','pix','position',[2*dxy+dlabels 2*dxy+hslider dsliders hslider],... - 'min',mintime_log,'max',maxtime_log,'value',maxtime,'callback',@minmaxtime_callback,'parent',h.plotctrlpanel); - h.edits.MaxTime=uicontrol(gcf,'style','edit','position',[3*dxy+dlabels+dsliders 2*dxy+hslider dedits hslider],'String',get(h.sliders.MaxTime,'value'),... - 'BackgroundColor','white','callback',@minmaxtime_callback,'parent',h.plotctrlpanel); - - %Slider: MinTime - h.labels.MinTime=uicontrol(gcf,'style','text','position',[dxy 3*dxy+2*hslider dlabels hslider],'String','Min. time t[s] to dispay :','parent',h.plotctrlpanel,'HorizontalAlignment','left'); - h.sliders.MinTime=uicontrol(gcf,'style','slider','units','pix','position',[2*dxy+dlabels 3*dxy+2*hslider dsliders hslider],... - 'min',mintime_log,'max',maxtime_log,'value',mintime,'callback',@minmaxtime_callback,'parent',h.plotctrlpanel); - h.edits.MinTime=uicontrol(gcf,'style','edit','position',[3*dxy+dlabels+dsliders 3*dxy+2*hslider dedits hslider],'String',get(h.sliders.MinTime,'value'),... - 'BackgroundColor','white','callback',@minmaxtime_callback,'parent',h.plotctrlpanel); - - %%Current data/state GUI-elements (Multiline-edit-box) - h.edits.AircraftState=uicontrol(gcf,'style','edit','Units','normalized','position',[.02 .02 0.96 0.96],'Min',1,'Max',10,'String','This shows the current aircraft state',... - 'HorizontalAlignment','left','parent',h.aircraftstatepanel); - - h.labels.GUIState=uicontrol(gcf,'style','text','Units','pixels','position',[dxy dxy width-4*dxy hslider],'String','Current state of this GUI',... - 'HorizontalAlignment','left','parent',h.guistatepanel); - -end - -%% ************************************************************************ -% INITPLOTGUI (nested function) -% ************************************************************************ -function InitPlotGUI() - - % Setup handles to lines and text - h.markertext=[]; - templinehandle=0.0;%line([0 1],[0 5]); % Just a temporary handle to init array - h.markerline(1:NrAxes)=templinehandle; % the actual handle-array to the lines - these are numbered consecutively - h.markerline(1:NrAxes)=0.0; - - % Setup all other figures and axes for plotting - % PLOT WINDOW 1: GPS POSITION - h.figures(2)=figure('units','normalized','Toolbar','figure', 'Name', 'GPS Position'); - h.axes(1)=axes(); - set(h.axes(1),'Parent',h.figures(2)); - - % PLOT WINDOW 2: IMU, baro altitude - h.figures(3)=figure('Name', 'IMU / Baro Altitude'); - h.axes(2)=subplot(4,1,1); - h.axes(3)=subplot(4,1,2); - h.axes(4)=subplot(4,1,3); - h.axes(5)=subplot(4,1,4); - set(h.axes(2:5),'Parent',h.figures(3)); - - % PLOT WINDOW 3: ATTITUDE ESTIMATE, ACTUATORS/CONTROLS, AIRSPEEDS,... - h.figures(4)=figure('Name', 'Attitude Estimate / Actuators / Airspeeds'); - h.axes(6)=subplot(4,1,1); - h.axes(7)=subplot(4,1,2); - h.axes(8)=subplot(4,1,3); - h.axes(9)=subplot(4,1,4); - set(h.axes(6:9),'Parent',h.figures(4)); - - % PLOT WINDOW 4: LOG STATS - h.figures(5) = figure('Name', 'Log Statistics'); - h.axes(10)=subplot(1,1,1); - set(h.axes(10:10),'Parent',h.figures(5)); - -end - -%% ************************************************************************ -% DRAWRAWDATA (nested function) -% ************************************************************************ -%Draws the raw data from the sysvector, but does not add any -%marker-lines or so -function DrawRawData() - % ************************************************************************ - % PLOT WINDOW 1: GPS POSITION & GUI - % ************************************************************************ - figure(h.figures(2)); - % Only plot GPS data if available - if (sum(double(sysvector.gps_raw_position(imintime:imaxtime,1)))>0) && (bDisplayGPS) - %Draw data - plot3(h.axes(1),double(sysvector.gps_raw_position(imintime:imaxtime,1))*fconv_gpslatlong, ... - double(sysvector.gps_raw_position(imintime:imaxtime,2))*fconv_gpslatlong, ... - double(sysvector.gps_raw_position(imintime:imaxtime,3))*fconv_gpsalt,'r.'); - title(h.axes(1),'GPS Position Data(if available)'); - xlabel(h.axes(1),'Latitude [deg]'); - ylabel(h.axes(1),'Longitude [deg]'); - zlabel(h.axes(1),'Altitude above MSL [m]'); - grid on - - %Reset path - h.pathpoints=0; - end - - % ************************************************************************ - % PLOT WINDOW 2: IMU, baro altitude - % ************************************************************************ - figure(h.figures(3)); - plot(h.axes(2),time(imintime:imaxtime),sysvector.mag(imintime:imaxtime,:)); - title(h.axes(2),'Magnetometers [Gauss]'); - legend(h.axes(2),'x','y','z'); - plot(h.axes(3),time(imintime:imaxtime),sysvector.accel(imintime:imaxtime,:)); - title(h.axes(3),'Accelerometers [m/s²]'); - legend(h.axes(3),'x','y','z'); - plot(h.axes(4),time(imintime:imaxtime),sysvector.gyro(imintime:imaxtime,:)); - title(h.axes(4),'Gyroscopes [rad/s]'); - legend(h.axes(4),'x','y','z'); - plot(h.axes(5),time(imintime:imaxtime),sysvector.baro_alt(imintime:imaxtime),'color','blue'); - if(bDisplayGPS) - hold on; - plot(h.axes(5),time(imintime:imaxtime),double(sysvector.gps_raw_position(imintime:imaxtime,3)).*fconv_gpsalt,'color','red'); - hold off - legend('Barometric Altitude [m]','GPS Altitude [m]'); - else - legend('Barometric Altitude [m]'); - end - title(h.axes(5),'Altitude above MSL [m]'); - - % ************************************************************************ - % PLOT WINDOW 3: ATTITUDE ESTIMATE, ACTUATORS/CONTROLS, AIRSPEEDS,... - % ************************************************************************ - figure(h.figures(4)); - %Attitude Estimate - plot(h.axes(6),time(imintime:imaxtime), sysvector.attitude(imintime:imaxtime,:).*180./3.14159); - title(h.axes(6),'Estimated attitude [deg]'); - legend(h.axes(6),'roll','pitch','yaw'); - %Actuator Controls - plot(h.axes(7),time(imintime:imaxtime), sysvector.actuator_control(imintime:imaxtime,:)); - title(h.axes(7),'Actuator control [-]'); - legend(h.axes(7),'0','1','2','3'); - %Actuator Controls - plot(h.axes(8),time(imintime:imaxtime), sysvector.actuators(imintime:imaxtime,1:8)); - title(h.axes(8),'Actuator PWM (raw-)outputs [µs]'); - legend(h.axes(8),'CH1','CH2','CH3','CH4','CH5','CH6','CH7','CH8'); - set(h.axes(8), 'YLim',[800 2200]); - %Airspeeds - plot(h.axes(9),time(imintime:imaxtime), sysvector.ind_airspeed(imintime:imaxtime)); - hold on - plot(h.axes(9),time(imintime:imaxtime), sysvector.true_airspeed(imintime:imaxtime)); - hold off - %add GPS total airspeed here - title(h.axes(9),'Airspeed [m/s]'); - legend(h.axes(9),'Indicated Airspeed (IAS)','True Airspeed (TAS)','GPS Airspeed'); - %calculate time differences and plot them - intervals = zeros(0,imaxtime - imintime); - for k = imintime+1:imaxtime - intervals(k) = time(k) - time(k-1); - end - plot(h.axes(10), time(imintime:imaxtime), intervals); - - %Set same timescale for all plots - for i=2:NrAxes - set(h.axes(i),'XLim',[mintime maxtime]); - end - - set(h.labels.GUIState,'String','OK','BackgroundColor',[240/255 240/255 240/255]); -end - -%% ************************************************************************ -% DRAWCURRENTAIRCRAFTSTATE(nested function) -% ************************************************************************ -function DrawCurrentAircraftState() - %find current data index - i=find(time>=CurTime,1,'first'); - - %********************************************************************** - % Current aircraft state label update - %********************************************************************** - acstate{1,:}=[sprintf('%s \t\t','GPS Pos:'),'[lat=',num2str(double(sysvector.gps_raw_position(i,1))*fconv_gpslatlong),'°, ',... - 'lon=',num2str(double(sysvector.gps_raw_position(i,2))*fconv_gpslatlong),'°, ',... - 'alt=',num2str(double(sysvector.gps_raw_position(i,3))*fconv_gpsalt),'m]']; - acstate{2,:}=[sprintf('%s \t\t','Mags[gauss]'),'[x=',num2str(sysvector.mag(i,1)),... - ', y=',num2str(sysvector.mag(i,2)),... - ', z=',num2str(sysvector.mag(i,3)),']']; - acstate{3,:}=[sprintf('%s \t\t','Accels[m/s²]'),'[x=',num2str(sysvector.accel(i,1)),... - ', y=',num2str(sysvector.accel(i,2)),... - ', z=',num2str(sysvector.accel(i,3)),']']; - acstate{4,:}=[sprintf('%s \t\t','Gyros[rad/s]'),'[x=',num2str(sysvector.gyro(i,1)),... - ', y=',num2str(sysvector.gyro(i,2)),... - ', z=',num2str(sysvector.gyro(i,3)),']']; - acstate{5,:}=[sprintf('%s \t\t','Altitude[m]'),'[Barometric: ',num2str(sysvector.baro_alt(i)),'m, GPS: ',num2str(double(sysvector.gps_raw_position(i,3))*fconv_gpsalt),'m]']; - acstate{6,:}=[sprintf('%s \t','Est. attitude[deg]:'),'[Roll=',num2str(sysvector.attitude(i,1).*180./3.14159),... - ', Pitch=',num2str(sysvector.attitude(i,2).*180./3.14159),... - ', Yaw=',num2str(sysvector.attitude(i,3).*180./3.14159),']']; - acstate{7,:}=sprintf('%s \t[','Actuator Ctrls [-]:'); - for j=1:4 - acstate{7,:}=[acstate{7,:},num2str(sysvector.actuator_control(i,j)),',']; - end - acstate{7,:}=[acstate{7,:},']']; - acstate{8,:}=sprintf('%s \t[','Actuator Outputs [PWM/µs]:'); - for j=1:8 - acstate{8,:}=[acstate{8,:},num2str(sysvector.actuators(i,j)),',']; - end - acstate{8,:}=[acstate{8,:},']']; - acstate{9,:}=[sprintf('%s \t','Airspeed[m/s]:'),'[IAS: ',num2str(sysvector.ind_airspeed(i)),', TAS: ',num2str(sysvector.true_airspeed(i)),']']; - - set(h.edits.AircraftState,'String',acstate); - - %********************************************************************** - % GPS Plot Update - %********************************************************************** - %Plot traveled path, and and time. - figure(h.figures(2)); - hold on; - if(CurTime>mintime+1) %the +1 is only a small bugfix - h.pathline=plot3(h.axes(1),double(sysvector.gps_raw_position(imintime:i,1))*fconv_gpslatlong, ... - double(sysvector.gps_raw_position(imintime:i,2))*fconv_gpslatlong, ... - double(sysvector.gps_raw_position(imintime:i,3))*fconv_gpsalt,'b','LineWidth',2); - end; - hold off - %Plot current position - newpoint=[double(sysvector.gps_raw_position(i,1))*fconv_gpslatlong double(sysvector.gps_raw_position(i,2))*fconv_gpslatlong double(sysvector.gps_raw_position(i,3))*fconv_gpsalt]; - if(numel(h.pathpoints)<=3) %empty path - h.pathpoints(1,1:3)=newpoint; - else %Not empty, append new point - h.pathpoints(size(h.pathpoints,1)+1,:)=newpoint; - end - axes(h.axes(1)); - line(h.pathpoints(:,1),h.pathpoints(:,2),h.pathpoints(:,3),'LineStyle','none','Marker','.','MarkerEdge','black','MarkerSize',20); - - - % Plot current time (small label next to current gps position) - textdesc=strcat(' t=',num2str(time(i)),'s'); - if(isvalidhandle(h.markertext)) - delete(h.markertext); %delete old text - end - h.markertext=text(double(sysvector.gps_raw_position(i,1))*fconv_gpslatlong,double(sysvector.gps_raw_position(i,2))*fconv_gpslatlong,... - double(sysvector.gps_raw_position(i,3))*fconv_gpsalt,textdesc); - set(h.edits.CurTime,'String',CurTime); - - %********************************************************************** - % Plot the lines showing the current time in the 2-d plots - %********************************************************************** - for i=2:NrAxes - if(isvalidhandle(h.markerline(i))) delete(h.markerline(i)); end - ylims=get(h.axes(i),'YLim'); - h.markerline(i)=line([CurTime CurTime] ,get(h.axes(i),'YLim'),'Color','black'); - set(h.markerline(i),'parent',h.axes(i)); - end - - set(h.labels.GUIState,'String','OK','BackgroundColor',[240/255 240/255 240/255]); -end - -%% ************************************************************************ -% MINMAXTIME CALLBACK (nested function) -% ************************************************************************ -function minmaxtime_callback(hObj,event) %#ok - new_mintime=get(h.sliders.MinTime,'Value'); - new_maxtime=get(h.sliders.MaxTime,'Value'); - - %Safety checks: - bErr=false; - %1: mintime must be < maxtime - if((new_mintime>maxtime) || (new_maxtimeCurTime) - set(h.labels.GUIState,'String','Error: Mintime cannot be bigger than CurTime! CurTime set to new mintime.','BackgroundColor','red'); - mintime=new_mintime; - CurTime=mintime; - bErr=true; - end - %3: MaxTime must be >CurTime - if(new_maxtime - %find current time - if(hObj==h.sliders.CurTime) - CurTime=get(h.sliders.CurTime,'Value'); - elseif (hObj==h.edits.CurTime) - temp=str2num(get(h.edits.CurTime,'String')); - if(tempmintime) - CurTime=temp; - else - %Error - set(h.labels.GUIState,'String','Error: You tried to set an invalid current time! Previous value restored.','BackgroundColor','red'); - end - else - %Error - set(h.labels.GUIState,'String','Error: curtime_callback','BackgroundColor','red'); - end - - set(h.sliders.CurTime,'Value',CurTime); - set(h.edits.CurTime,'String',num2str(CurTime)); - - %Redraw time markers, but don't have to redraw the whole raw data - DrawCurrentAircraftState(); -end - -%% ************************************************************************ -% FINDMINMAXINDICES (nested function) -% ************************************************************************ -function [idxmin,idxmax] = FindMinMaxTimeIndices() - for i=1:size(sysvector.timestamp,1) - if time(i)>=mintime; idxmin=i; break; end - end - for i=1:size(sysvector.timestamp,1) - if maxtime==0; idxmax=size(sysvector.timestamp,1); break; end - if time(i)>=maxtime; idxmax=i; break; end - end - mintime=time(idxmin); - maxtime=time(idxmax); -end - -%% ************************************************************************ -% ISVALIDHANDLE (nested function) -% ************************************************************************ -function isvalid = isvalidhandle(handle) - if(exist(varname(handle))>0 && length(ishandle(handle))>0) - if(ishandle(handle)>0) - if(handle>0.0) - isvalid=true; - return; - end - end - end - isvalid=false; -end - -%% ************************************************************************ -% JUST SOME SMALL HELPER FUNCTIONS (nested function) -% ************************************************************************ -function out = varname(var) - out = inputname(1); -end - -%This is the end of the matlab file / the main function -end diff --git a/src/modules/sdlog2/sdlog2.c b/src/modules/sdlog2/sdlog2.c index 3713e0b306..0da8ec568e 100644 --- a/src/modules/sdlog2/sdlog2.c +++ b/src/modules/sdlog2/sdlog2.c @@ -604,6 +604,15 @@ int sdlog2_thread_main(int argc, char *argv[]) errx(1, "unable to create logging folder, exiting."); } + const char *converter_in = "/etc/logging/log_converter.zip"; + char* converter_out = malloc(200); + sprintf(converter_out, "%s/log_converter.zip", folder_path); + + if (file_copy(converter_in, converter_out)) { + errx(1, "unable to copy conversion scripts, exiting."); + } + free(converter_out); + /* only print logging path, important to find log file later */ warnx("logging to directory: %s", folder_path); From 53b373dd829baec5858212ab966cb4f74d219b59 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 13 Aug 2013 08:50:40 +0200 Subject: [PATCH 22/25] Minor fixes to log conversion scripts --- Tools/logconv.m | 4 ++-- Tools/sdlog2_dump.py | 1 - 2 files changed, 2 insertions(+), 3 deletions(-) diff --git a/Tools/logconv.m b/Tools/logconv.m index f2ae6e5f37..c416b8095e 100644 --- a/Tools/logconv.m +++ b/Tools/logconv.m @@ -111,9 +111,9 @@ function ImportPX4LogData() time_us = sysvector.TIME_StartTime(end) - sysvector.TIME_StartTime(1); time_s = uint64(time_us*1e-6); time_m = uint64(time_s/60); - time_s = time_s - time_m * 60 + time_s = time_s - time_m * 60; - disp([sprintf('Flight log duration: %d:%d (minutes:seconds)', time_m, time_s)]); + disp([sprintf('Flight log duration: %d:%d (minutes:seconds)', time_m, time_s) char(10)]); disp(['logfile conversion finished.' char(10)]); else diff --git a/Tools/sdlog2_dump.py b/Tools/sdlog2_dump.py index a376e03d39..7fefc5908f 100644 --- a/Tools/sdlog2_dump.py +++ b/Tools/sdlog2_dump.py @@ -200,7 +200,6 @@ class SDLog2Parser: else: data = struct.unpack(self.MSG_FORMAT_STRUCT, str(self.__buffer[self.__ptr + 3 : self.__ptr + self.MSG_FORMAT_PACKET_LEN])) msg_type = data[0] - print(msg_type) if msg_type != self.MSG_TYPE_FORMAT: msg_length = data[1] if runningPython3: From 8c1f7a3706a15b5df2749f8ea076e457cd796f5f Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 13 Aug 2013 08:52:03 +0200 Subject: [PATCH 23/25] Hotfix: Updated log converter --- ROMFS/px4fmu_common/logging/log_converter.zip | Bin 10090 -> 10087 bytes 1 file changed, 0 insertions(+), 0 deletions(-) diff --git a/ROMFS/px4fmu_common/logging/log_converter.zip b/ROMFS/px4fmu_common/logging/log_converter.zip index 2641e671068b1ea6867fc6b9fef17ba3889e5262..7cb837e5666f51eca7ed803dfef4581f01bec1f3 100644 GIT binary patch delta 7944 zcmZXZRZ|>*vM?79L4pJi4vYKZ?gV!yKp?m~K^Auj4vV|HYj9ZHo#5{7aO<4=^2t=q z+sqH>?qkIRMX<6Q9Q;=R0PqQbEsiFR)f#N*iV6U*u>%0;fNub2QyY78c0*HFTL)$b zPYrcs032`{Syf~aS=H481poto2Lk~7_rFuu$9|Oy`=kB+YehiLeVIr2s`*mDFOii3 zz1pI2$Zb#7KuFGTVx>kAsFQ~?#OL84ra&el4?Jf|41|vv-+A*8Ah`(M{gjLnMgv0sg9RjP}Oo@s(nG8HK>?rOh zvJ$SgP?Bn0q`4X-I#YNS*^5`S9{|Gg;AUFxEp(356 zG-7~k%l4jKbwmRd@#P^8%|_2A?!7s_OpR2Zi=wu1h1wnx0DjGc22;ORuPf6v| zqK(GuXLiD*x1&bJ>eI^<22@YQFseuXqvMvv)L_Jy}gX(Y&#|TYmCD`r}8w*^ds7A0$3%zBe7W z>cuA?23ez7-`-cn72`V=FF-HZRs$V+7pYNOvo!V`QZ!vf9h-@G=pzud1@sZ6jO&EJ zcn8_-oR0m(MReIQy4t{A9t7Y9b>|+{RCM4yQL)d?Zsc46dU2>UaIX*yNmR(SQsvsy zR~RYkVDo&Or7*m$8Y0cO4h_6UDSn+PdL0lBB%^PJc@VMS>N0Q0i|iRJvQ?`J;Q8L8 zB6>0|)}#pyp+H{2Flcf@BJc}TZqkR zX?xc_V7u!G@@0Bi>!6z*sO3{3!wkE^RF&pm=1~1vC&?Dn;N!r5xHxpR5)1D_2-H^l zlqDa3a)SVW5Gjih(a6?;6-Jc$Z#;WtxB7>)b-6wgu~s5KHLwXClQFy?wfbbPUAsqN zmW2z~VQ(1S+tBIzur&%*^{7OVJE(zbB{ZR0KumZKgPzUCjB1c68Tc!BU82Dzoe7`G zVy^;<(E{@#aVEK3#gx9P)CpQVv* z2s1u!%JvTqxLlzg%zB?0W#%&c*3!a~_bg&g_zRB*UZ3hklALL-NuX|U3d6dK3jz@r zJ~RKOs8$=!a3gnUoW+}(dksO5rhaAELby_Xwp8`jLqi2j_1ZY1Ib z*V07*Y^<(csJ@R~-Ew!EF+G_K;~ujK(zsbCY>XhYMb38}7usG?j0?7QD@z#tUd~d& z5zNLr8k7O>c5iA1+|`ibPWF&uwmXY)s)VkYYRhTbT%L z@2xT(k%psS%J^0f6H_d0K%%^=9d)}DfAVP*tN>$~V3ttTf;HBTi=EdMKCdZI=pEHx(w|=~H_7s~7`)-#XQLhe7WM z@oBTLn#If|!gQLu{~q80#8gwNM*r$5Oo*Kz=O7;`-Nwn{Ub_*jWn2^ol+ zrpG&de%jb^q}Ep)1C@FB`K1$JT_ka7=kxB#@*M#Kd`ul)fgJTMgn@Wm<@^HuSj=4MRB%L`lGfrZop< z2Mn4?QOPEBg-T*txW@Z>nh^t|Q(mUpv#g=J7Q7@U9cZ_|c-~o{M-GwcguB*T7-dSM zd9?PHOQF3k+w3Dq!07Qg5031J^SLAp1~KyJTO0iyrEoltOK9t}x~=+rbQP!WqP6z? zb@X{VtCAk!8R4()9U+#<<`eJ|{NS=I|g#x2-xgh0GC8G~ns~#9t;m!D3ec&-W z{yWMq)0s8)Xb^NPnRJ6j&+gZcZ)YVY|HMaUjfQ7YGFYY4n7ps481tz-+0(?V<6}FY zau1kMb_sHN-cQL-j~z5?g)fY=lRY8oCX>y7)^N|W7B|u-Hqr+=gElBxIGPG2f1&^Q zWs7Y)p@SJ)$p<8RK5hoX0d3WallD}uM5_sNIp*`-G_-N}-3f`geW!g5T(Mgde8pvD z40gI!M35`QfcOph!`nD0Pig0m%cNPg59+)zpNlJ;y~V>>B!X4*UA0AW`>@mZL8QLX z4++TCjg%WQLF&{X0`1@nHhxyfqo|9fM4}(EI-Q_jDK+_toH_VZ_6yn2@8Z>&FrCK` zoae!%t_4f`XQ~$6(geG6lkIS`diiw${OrjrrY8O)c*CB%A#raB)IRqw*QuMSGDB_I zj}lLkujIr%LWfdk_OH23Oz+AlVZsofdP675L#7*f6pZSB!hh8dDH}p)d`=1*mxXeU zV>#R^bLnLb*u%kX*DW|PT~jqs;!+Nr*l;D(4Ra_ay$DW=$YV5nv6cXG7hq*2pf`fT4K>x zf2-ahMzg{Vclk2RGPV94%Z{qP5Lt6-H*89HqPYngd}0kq;!;sq?AG$b=*}P#ku=U_ zu~pKG_B9EnS0?GRV|~-V;18l>@0{(5&rQy+If_UHBwle1o@iccL&UMGESc1~_XU+H z4sckgB#PiKd{=I)b0nsqZBcvd0G;7IEA)9cqu>Fn$e}R~!Muh#@WC|&27}Yv^R|G9 z{p0=Zwl|x3NMBsWyA~dwgT8J2K2@qlvQw-C_MFA+3<14U00|fsF#UMHO+KD zB!<=$M@D1n7aC4)T?ikWm=-l|rxNuQweQ}d=H=!vYjmA1YM@SU0EqIQ5w33m3yRr%eV$8Osfc zHh}5q0D4#bMrMLzPTPSUNr8*XLuB?gS3>NZT*IwTr>*TzkOEIJnAHp&_?Ui~wgrrh ztZ#Q7R|`BIT=VXq0%CX8;Ynf|{|Rc>{$s~v^W=XG%(37ui3+R~Cn~yg55QY2{8?PJ z*W3&>XVt%_*@r3O?LJ@^gbsx~>LZdpae>9XnVplo=P|Nv%McAuqah_Wr#xh12m2OG zf7wz3uViu8qdlkffCvFxjrIpeUY_eeUb`~eEX}9yi1~InGvDhT=I3wjp*UxZ=&55q`_ zqlxp_CgXVraIRw?;9noI3r z^*)m^pSY_{kI`gr*DV6D&6DASS!Cu(AUl?~s3Xg@aC};M(PTeVYU}GU1ql?}Z93ai zjlF@ojXc|kc`Ly@oQO|V;!J6~6TpBHF9=Sq6=BXCD3#RI*u74dmFMbJ{5L zB|$l9yi;cp@j7z=OYksGv_Wzb!yi#nH~+>vKt(@FmaOs|9iwm$IMF!+X0ePN@B!d+ zGg(&)Dg^5+;F2$q<@ndBqA)${X#zEkW<~XhO9_pHTbliQe?`GE!jG_v=gOw2f5JDa zTfj`s8%Z1}2|Ja+^v;u)RVfHMWliLowmz4zA8%wu;!s!Z^?Zf*4q|L}y0kj$jvO`k zuDeb@+4`A*y=Y|tXaH^aoR=!KAYsSQ&_c}25(RE8d+ebsWMU-^ICb5pXF5e8<3~5L z{~;qM$>hm;SG^h`FB_0qc0GMpVb9uZ=We9ruGx_CGyO&cOVf_qEqAo7M-rb!OjfOA z0CQMPjOR2pUbhlg?+Zc)vLsG8->*VAk*rEJ99_nEU9|Nkd)J-vymx{oj$@lJz1D9S z5&>Yf_61xs<$pPv8q7uuvYgeJ7+*zRPnqR026Obg1EeZ_{Sx9ReI1E!(ja>V`h$w! ztC~!!q%^Z?Qv${?624jC%Q2KnJ`(e^T(WYky4vDtJ^EUz+;hs(N|mPxj3JS zaaH9{4RZ`=K0+3$9+0J&*Ss%g;s4}O)>QzO@e0^Cnh_`!JdK$m%T<|i>Xn&ptieJ? z{wL~gjI4;MFQ5FhD@--IA?~ePz(gZgu)JOz?h^T8DH*il#2%yzrtXsg-TZsk!m&cY$8lrx(CrE2tj{XYcEKv2`3QbI?^^JDrrT$_)0j zg8UuFzGh9!%JVb_L*V*#p<^QQeN)YHzvooCdo^sN30bCKOekr zx;&AhZ`<~3cO@!*;nr*ikVNN^M*+s~DU?_e4taX_Fm0!391oNg4 z0@H!q+Z@eWrxRym9Uo+te#=s&4(rj<9ohZ4H!TfVfP+nO87R8z_ZpMW=dDhtK(_J4 zKF)tnye2VP-I-x|O`ek!n66g}-p@C^F-By8oaM{gHOz>*##2^u9mxaVp!1fow}T1q zlIAmN-;4QMkL5%1uV5z2Vzz#OL0zYJibJ(*k>yE-&RQ51rtZW0Y)NCkm)u|uhs zE7z}$mJ_can%HLN>pM5auH^}6b33o+nr}K3+IN;AV3TmOg~Xoa0^9+Eykh7Z9urNc zTHDE5IQ-d~qtKW^$81GtX=rt?i>p&iD2w?c^%p1}yD;xByh(Zj*WKSx1zaVZ4;mOF z-Se=%9J=E72Y>PjIneCUOioR!V88inHsM}a33P0u&|P`k>ROGjOOREWd{Z1RWVR() z))yc0H9c6vAk~ad#eM@THg|j0jhA3(-}h;F?u-#dwel_o*6|$A<4SkZCyl)P4({*} z9hqsBDqZ-#m@t92Ar8`w{{83{S#n?xjXi$1F{=1|3{#?^s&g-;Di zdf@X+r&2fCn=hk66jpxFB0=jgK}^efl6Uc4J4FrJzk6?**0TXeNc?rZkauDe8k#K$ z7e$$bUhuD)g&O6~}zLq3N4l#g^K6?j#P zYa3t|xbdF1V=L@T^H)=TuD>TwGmTUL`@jAPB6HkI^>6RRo)${;of6-O4esH>tBG;~ zj~NN{{GW8XgO-?$X$aVFz@FZr^~$0FqD=y$VF4Iczm~iH zv?TfbHo?p3{~jG3EVN>G0FKaj&$alq(PEo=#v?*&3oo1=@0-D*B{AcW^I*2Sa*sYR zV3(q4#E5eHlDA=A%3a!N(^0yJuG=_hymM$<)C4yZL5AFMLN=Q(b1Z;<`dhR}mGnNLMH?xl1F4yek%b}?w-W>f$s9%a9al4_l$GPLwvZ=6j zd*TnA;&G`Mkv(k-W%#d;Y{Syjo&s zw&n;FtQ*dOap99q-Z$9~-`kkYI_u0(A*<+M0 zTi;u8r|65xyQNdponGqngQXt+^wihogtP)he3l*|3drI<|8dGIm z29fR`BLXS&K>eB!&wW!;iJRRKp_2Wl6q~wOrxZx&K8OUVY=`WxLY$|1`4s_!Jk>t?|gRIFU_KRRT*dqGg7_p@ERg24sLUXWDs~(dcUb5x_sfShb zjSK@bm}V=H;=&fCKT6WG-b5vY;x@qIbkopxVWk7=$CLCik|Xgst^VnXiZ;4a#PS?L z)|y2b{Q-8*f<-yz0T#S>XBR&1mqK1^U$0OVhHFVXQo$sVx=bWak5TLq2VMB5Dv-I> zxNAhWlk@W%tK+L0665`mT8cK5IDfPJ;CfZ=tM}%Sdl6bL4lb((p?CePB}Uzx8}44h zz$fb;`#lpcwAWRCut>Z-=DUZG3@SEH`VEceyoi|h7sT5_B8I>u4HqR@y*mPw>}7YNEPZ- z65AD+Br2I{UAg|ooFbpUwopGMFj9t?&8IM=C4-As7oxzK6ilN-27B(Gl?BJ?VK~EA_{Z;1ETk@}nUkF#7X8S?t2{92hx4 z`Y41lbC!|)@$cyU?jUn)(qpi&1psz7ndmdW0;qhRIsl&949rtQ2uyY z6F*=gbdMIAqpfx=V*hdb zU2HF{6TvP9*cXDSwr+P9(@$H2Xlju`<7`GDUbK%@Nw6{0Z zGIZj^_E3_b66Rxbh`lt#sU#*8>yuWgx18e9shsBai;({67eDM%IYTxYu@N<)4xj?! z|2H(fJU0;Ai^eiZS}AiTLH-*3k4fg7y5t;q!z`^IE+Ibnrzp+Vq@&hyH=}Y~ey?cZ zYhuO(-i!&Py-jw~L#GbgG>bTLsrxX{c$U{MOq350(jS9Yw;;_w2a+jsH?jo-i z`U#s4_PaA27mVDu%f5d6F#SoAndKhba=$g``cWt{akH3cKMm{krqI|@@Vcp*lJuvl ztGb!iaSPY0v8$I41Zu@#70BbDSze@65ev68&Uy&wt+_0aX4EYqGe58Ydpt-3hm~@A z0S1(>q`@sfLMYpiQ_eq&*;k!^45;%`k_%G4Ze`a(*Vf4C>h+4lmFCBWN~$qS?vqsd zAn#hCe3?~FU+iGCiq>ixGP^!q4}TlhLUt!Hcqf(g_sb~uc~bCk#)Fcu8)ET6XEu$J zfI%S5b$mF{bDt&(D#*+KAoCo|`%G_D(NUdW+oWKQ$vB#v{Y^G|O|CE`UNA>|Mqp<` zGX3A8^%2*~gtELe!uC#@=pk=uNKk5+aDO#`HsHlvclED+G>xeMeD+Uo7)6AqCH}vs z(Rthk9}+iP0iN?=+R~4|^v#-?%!v^ja>-HJUVod6oz#7tpp+oJ2@g%7HoK8r@`ryc ze<* delta 7942 zcmZXZRa2Y|pri+P2?Pl4?l!o)JA`1t9fDin#Vt6&!{F|c!5xAJ3liKRID`AC?_BKG z)^=6jb^m~V`au0i9ne%kKzt7ZflxuKB3Lp-{OfVPm>`f64+w+}A_I9?Il0>Mm|1x_ zf8qGzr>BbsLP#7$(|-JgrtRg60fIxkfdhg5x8Ftb;<^Hb9X!^3CP4gYFr>2DZH|$S zQmoca;J93^M0ra{8cL%BHMLfri_)=NdV4)nj4ftT2GkZTq&JsD*Dud}J2~CXFI&6} zzXs&gyE3n&d^M=y1(OWPL8+y17xhf0si;O7reb4v<@#AZSZk$i@=aa+K2lQ$iLrUp zP3l|85wrKla*UoP7{A^=D=Cf9gKX$nNB-?0p{c-NM-sH~Q-?CdwTBJHdcQH!CEc>m zM-nd_0rc;0YA)DNjQjli20E2&hMeL;q*tr(dHdK6@b30eS&sdWX-~i1D}Wae&gCGq zV1E`|!h;nQ(`gKZ!xgOf%Qd!-9mmpDSKGDH8OEKcA^k2wcsvw~Z)9gjH9%vJt!64i z9xBN{WUdvZ8d6H)C5iGvFIL%pfW!>a;la*;0{P&A=-MXF7>tm@LxwtUE7DSac9Uzt z)#4>qF_fJF0T#=D2~Kt~{+wD1JY`z&a~Z|Udw&?IjbfEZdSjHbF55s9DWWBeie`c?ivD3H6-T52 zdRhu;8e=ACmfWv-wf`7nBwquiu_egGMZyo}x43O9%86B$j#5^pd8vF&{&>pHrQG(K zMSw`-3;Ebo+`Vp*oz8gJls`p3aISX-Jg9uRdA#$;e0rI|;T1<;#tIUb5EB6?JB0v@ zB-@%T+R(UMD2naZizuL?&)|IB?1w3Ul2OJ92=%0UcE9x@o0;^yUBF2AueOJ-V22&B zj)Rgi29vitnDOu3-5!Zus+fQm^`f^5?}?6^gwYS;fiSS8=eFYr2DaZ;aJ7)tY4Gy| zwS)*2S`;1jh$FV&Y49K!o(NT(jO0DXAv;Ql3t9b!(1{46X>qvl4+@H`qzpiriBvjG z6TEATW-YUxDfnz3!s&mvgM z&HfY?)L-;!JEIMFt@*eC9|HgvDV0xu@h=Z_QK7}wgT#+loNz0SKghT8W!tNa_7K}v zXmhxHl2rzbE@;)=@vLySY3^{PKaSq0IZ4AKa&&*v(WZ&7?qvc=NkL%0Ojg2Oj|KNT zd^^HM^xki|h{E-|e<)Jy=V~0??8K~^W=J-O1{6O{QIr$In?t0k%M=0Gl;-4__CmHA zfeBn^92tVGoVDKIV7?E`B$yzdochqPFE?}Ft7bj46wZDrm8%`3X_e6$$_&Tl zihpN_l5^MmS*CEKfL{`ATc$`>kZp`j#>*FPQ>o`Mz{1egM5do2*_MC%T z9^anybt3kp;};f518pZ`)eOki*Z(56hq3GPX>_HcIvLsjn*ZEsskru&OtsxPG%vc_ zW0Nm))QL?O;=~raZqcdCj!`RDCekU*X;q$E$m1QqgMIW>ivgT05G>Ml!1^A6u-&tN zsWF)qI#i@FB3sbdE1GY;Q+#Z^MhN{^>VK$8ZPL5>BLyoD7h(c?KY3hSgU{Y+cYApg zMel?i)LJ?@KXxhHmlm{wl(4eVMbu*(x4Y#?$TdC?=rX5046?e2vGYFu<=OoG?nfy} zw^5OPvSQAFoe9vC*xI5ayOo`LwCg^bjd7%Eu9m<<%_!N;ddi;kutf3`LF)J}{`PVh z1EyQQBb_0YkA%m=f30~Yz!&*4jQArFdbP#T?Ga*Th7{2_AsYF2jc`+gF9zSQ=TD41m3^0H?eUQ7f4HY*p=O@0D#2?087I&E?Yr zPcXTb_z?>>Y<(hEFZq<4QpVmDk^NaYu159CcX~)c6RI!a8Ht&YJSb4}|7;^DzYkXe z6M$3@Tef)@!u3sK&pAu0SN}tYF%5;W=j7IX)Z_V-Q1V?I+r%(D%hiyXkW=P7*<5Xa z3`i;3^R2!DPiJi1_t2FM(7HGzokxUKh_iny>?|>-M4yWpXQIsH0~cPJbHqaa?BTLX z_r+g38SiMs5cyt=HazM&>(0m5^8c(|tgqZ774tFoHX=DCvF_aw=bTDBUZRVeKez@A z`+u1*Fc$U-aVt7GRKn1`Y!h_a)da0fgL$s$cW`f*DtiYFt_0gp!nhsNr z|84{DRRkTTM%-9cC>75LWeO$Y@+v<7><~U3d4@^$ImOh$QqNR&-fux)^Vr(cfaFWP z1YsMAW@jv{zw?rEUs#$VG$7&D-{~|MbUvuT8U!S+SNB3sut*N4C_-zqq3ADuIFHQ} zbHyY9-EyPO4=!!55J5mAHh{ZF!PLe{<=dEX=h$-T%C%WCg^`!4$RB3$F+!WmPT_Khl^BMxb_t=n zT@R?FRM^v55bAClBr1`vm=R{n2W_Ki%G|RR$2$poO)@YxMPwE$_o)%cR{iz->#}YEHTygycpv2RV%yB;2 zAHM$zJy?Pi;{$TF66)Kut4!UgNvC0!G~{PN+xUA#mr(h({n+~^mO5lVE4Za9dWYkmgF7MZVld~l z5<);&#SYE0^_Q~;tluU|Ldz!YL zN#0JF#?J3QO7E`v(Yl{Kc+@4LI!5PH?3;a&X^aZ0`*ctb>wKK8N(mj?6jLHH2=*9K zI=%k|btRNSYhh^=cI`shCQ7IP)Z?c0V${xcl)lE8 zJ$Mm1F%g$)>8nWcC&lF$au0;dQ$&>-ZgzvgU+gEm7;6H2CcTCyTZOj^eh-#kp_4d; zJfv=rvFNV`f}f`nh-$D23I$~v!p$*_gh~D|lu0hdG9vRFOQnb6Z0lni%aDh)Hbgv) z{8^&f-*0*We6MbluEIl|=9*B+Vv;>;sJr!qjxM)ryy^w!7ZLl0FMq@5*wpyo=~uU? z%A&eP0iWNWBB#04d)W;%zp7tsx9cAM`T26o{eX#;@q#AQ%ZGAdT*`R4gbNu5(XHE{ zPFdID)sgf!sm=F(>(atci^oR^epofG*%R1z=QbS$3Ol$tgU5>{drNO?!~)IgM80hd z=m=b}wRRiXc0!u=`F!^NunS?-o_+tfVzq$jC8NF(-}Kg9P4k_eM{Y0Yt8_NG3HsBBpnI00b3o{W2WMTsXp@GOiPOi3=t}fmjn9lz@ zYM63Rv@wU9&M{|@0RL6??*8&7(fEbuI4w+8b(?}RUPry`pDMiC$YsKqsy&lo^|Aen zUPHjkm{&?FCb$wM%|s|4@omB^yD3{buH-C@)C-&*8*-XM%7rVL!<53%`xAqM4pk68 zjZKkB)41}sI|TDF_3R55G1gSY)!pg}iia>3mkz{5p_WA*h%e|m)_kHDYKWQs2icb{ zZ|sF2YzLl?fW`NA-H)N1f3no6OS^Bd>bVa72t|n<5i?|qiAsN#Rc7Uq(&rl}Im`6U z$;Wp=giLlUKeCTxFW4pic-%?;fOzvYgVD0=a6bqh&iS_4nVUM-71rnHi za6Y*b?lvrdl*#xPQ?GPB8}Iu{Po;=pI0i@Z5avo%!?@UynD>2*V|nl8lt-{-?D4zy z)vp(fF+8|r=;?n9YxF9NxU-_vEns)HuBH{w*T7V(>S-)1otTW9+zzh|_p)=p}wj^q=CN&Pfq$%c$_(T>WN*_Luz+5r!&~T&K8>6Y44AIdS2JH$7 z>lM(`RLoC1Kj{l}vmE7_ixi1Rwx*54Nzt_ejgHRd$a`20dVBCTHM&eaLJ6P=z2QZIda*q~7HZ-Sv3f~9`!@#*} zFj@3=i#X39)O$O(_EVPg&E_v5e?G%L{mA$;Ean5FZw>@>{i&KxTh1DIYzzx7J5q@S zni=h|OUF`|2hLEj@JQ&{8ROoYV6Y}9ZIXKurEk9P8?wsMc$H~<78@*+TXt`h&0Y)4 zR%~g_)0Zu0XOt?F4bo<_omyIuu68O z`&rdjGO#CnSZ0i>8m*<&nPYShG%dUuPN+ zpxRUWSG*d$WK#n^N>QI)Wkzmb3v}3RZA5c%d2-Sr%F4KGqY~M8V`5M&t&RNCxj|l zW7DF1DI$@%h&{7v&uF~L4})U|K;Bc>+?MUET`Eq7rMnQ>T|;>W_Cb!DAf=6AX3TM@ zob32$J2T6cl&BVyae=G$$#d^4;QZ!4mlmCPaCW~S!3nFJO4G)$)zM4NKBcGBJQ+< zCi1k+D5K0PB{VM=^JZN$8kC96l4?8>D>$sMH2RyIE87ylr<8)y5wZcok( zj5sYGuzI!MQIsRTvsrO-z==@hbc#-j)WKQk=5NGbt32S=>GI)BuoZX&X67o>!e*Sv zGS$twDTQ)>e(=ZfG9ST%>!SMIWbT{=+j`sI~qrKVB$^(o1DLL0g=Ici#3-xOeU=99Y6kG%|`-@|+ z7oY0FwZCtSGqxRUU_^FNsmpSHOaR+PVpb_5EWjz*QHA7tvjlp z$Vv)57A19B;)f1IPAckkezHQk!2gV=zka;yoT3?$W>A&#;aPIc%#ZIPf!NA_mUrgC zt%(;md+}*S-nD@lz3fm7#XC6`ZBO}X9*fJkdag{ zy-cvQ59#;@5yam)IlR*1$doXO3hXK7{+dPpxM6(wO`@?qIo`!A*6YbgzJ;kN8&^jX zFLW@%68Q{$O6tk#Q3@wef)IJdxR+<_g*)sL^{t!P(5eL;NL}NOqp^}{`On){Et+VU z2sGZDg#lihZ$1=%YjUk92^pRSiVg@1NS_F($({(xo^em{;x4d!usV+a%-N!}RRy-D zo)mp;v}7CZ>bpnox^!x)WI>*qS)JAW9kVp-?p;*kSY$7(bS8(y5@ZQ=6NY$&uXsR?yGFU5|INJZ> zShQmGFCr*?-!^A*(Q{z)`!`GJIR^@S`@7L_PsSpTP&(-i`I_6xkRwWd&1@c{;OL1b zOh=xYrt~tlu^fwUJe}EpTqDgjH~M5}>a^<3nm+gF_XR~EJ&_Zf_!!r39QV^)$rEOc zPZ0sdm{PhwwqRV1}gR>9^P@k1gZ6GFcFU;gQUN6 z9xNzpmAPK1syCVTBp}IKd4Q4jP6?%wzoTN_s%>P&zf)d7W|^gRYI-fOXE3!R%uzTQ zpjTzs1e3OgxLA6HX^&H;tbSxsr9w;4RNw+!t?Ow6pJSHi>=fHJhz+=G(W8}p7*9CX z&K@Q|T4Sw&r5_JTYBXbc_mQVA8*P`;_hUt5umvl8+T+V~dP3%oGLC1i42zW!%@93R zv4M!%?bj;#!G@)t=+Egr`u7C}ot3aE2g`xJq(vNFLpmQM+9lWnGAc=6;1KFSkMN-RiNZ|G*E`~&t^^+D63 z!Za0T!sEQ9|Jn_Tv|PF1{+=fY+v2Ng-3EPPW_%;2?~e1p7@SK|&c6>Yd7xT|8Lx_5 zJLEpT%NEp}>-`jZfYON2&iR}*oEZr4Bu+;#Zut}Xc?aYg2psag&1kN?RSXUHsfa%W zT--^<;7K_nA^M;T*D+V(*JcN8I*-m;7dL*s2et~HYS7q`{Lg-XJZv|_;KX}6E#XP@ zO7LtzWT7}(tN5yp``o9}X7_VQ8uFkGiejNA7-O1pL1Rr}%CK`%wlf)oh|IRyo7s`339}-(-?&f zVZei-;}rwKw(cuy7#_3L@BQJ_e7;-wk~Siynl4G9RgCXNU6MlU7~g@#4tg+xY2;w{ zB)E;{Q*i-P6BO6n#uc+d^2fFugHeFWrKkI1N?1u;Sgc#`;H3t6{Zg5Ri3Pn+TxcC+ z+?7-7BBX313mmThu~*mrVp6?KYD9a*@Vf>fZM9z(ipY7g%gFGjS3qiNYxXXyecG(G51Dy)UdF@4GgJ_?&~Klv6t-cpYk*%R81$t>wm8y9mwqH_e0634x=d zcusfC7>>a#AG0P*86i)xEo4D5wO!)lP>)gkx@S*LinOQ){HVQ(%?lql{wiLffHngw zn07%MOx6o~$yiAx*-kKqpGYDR`V;mL7Nlj9wt$I851i5A#*_E$A4}bz^K0>LZPNA> zwkG;#`?evHCxIZoR>4w_5_z*-hd3Owq9N4V39&I4DFY11!lrd#L4fh-vFNBjfgdoc-nU_=FM65D*f7J4K zas_e#*n5!Qa%|}eEYpphTmHRTpgUJAjDyS8Over4M91CDd!w^PYw#%)*H%(7>juFI za|C$CTYNsmhG=&U88mSamPWZ6dm(~*k~L1SYVPi9QBQ0okKs3FlBvTOz_gyAjOj*K_M_v9u|!>2#!dczmwNXVw@zL< zGW;=$_JhDh@WT)m+NwoecNugI?y|^DEp($NS!JrTX?5E63GNIL*MQKgc6*KOpI!+r zXSC-hUuZrRl`EPVq&a4HNh#S6*HT;QZ}RB+-Gut6W(^^ZsgQCKj~-ufmsz{_^X7v3 z&ZU#voFVXK%!0A?&?jIg&z1%#T{wYzFEd2>vXyN|H%vAj;$Nh8W$VYup-W0&Nc!-% zV(e+8j^0UoQkc30QUQo530Z82rGA;Y@Oq4+abCPvdk?PP?}h}oVmWRhHTHcU8$o+F zBIo=#z&oZ3JXd(EOS~)*|A5>DK}0V-u^=NV3@(Q_M(kYECqHw2+7E!ks`3U{cfzk_ zaIRcEn^BNEQ=mI%8j134Ft*ZFoCuM#zgb<$ik*lIH#tFcqAha4K`{xQ1#-w=@R#A>?M*Q!LH@`bE81)y6X%6WE94}9<2rWR)$;F8uW+Phv~>jwnLao zJa^srs~KlL#q}GJ%m(JNU++2HD-o;b^Bbs%nNA8G>;HMCQ+*hi;1dT^3mC|G|9`Uc r0ra2XKp;?YNG~Sn|8O2>JzXTwf0!rrl0g`bEtQRtAK9GYzwrM6ALT|e From cd928b64f38100e91b7ee927a4b37dc58078a844 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 13 Aug 2013 09:10:47 +0200 Subject: [PATCH 24/25] Fixed log conversion scripts copy operation. Each log comes now with the required conversion tools. Eats up only 10 KB flash for the good cause. --- .../logging/{log_converter.zip => conv.zip} | Bin src/modules/sdlog2/sdlog2.c | 8 ++++---- 2 files changed, 4 insertions(+), 4 deletions(-) rename ROMFS/px4fmu_common/logging/{log_converter.zip => conv.zip} (100%) diff --git a/ROMFS/px4fmu_common/logging/log_converter.zip b/ROMFS/px4fmu_common/logging/conv.zip similarity index 100% rename from ROMFS/px4fmu_common/logging/log_converter.zip rename to ROMFS/px4fmu_common/logging/conv.zip diff --git a/src/modules/sdlog2/sdlog2.c b/src/modules/sdlog2/sdlog2.c index 0da8ec568e..ba7cdd91cf 100644 --- a/src/modules/sdlog2/sdlog2.c +++ b/src/modules/sdlog2/sdlog2.c @@ -604,9 +604,9 @@ int sdlog2_thread_main(int argc, char *argv[]) errx(1, "unable to create logging folder, exiting."); } - const char *converter_in = "/etc/logging/log_converter.zip"; - char* converter_out = malloc(200); - sprintf(converter_out, "%s/log_converter.zip", folder_path); + const char *converter_in = "/etc/logging/conv.zip"; + char* converter_out = malloc(150); + sprintf(converter_out, "%s/conv.zip", folder_path); if (file_copy(converter_in, converter_out)) { errx(1, "unable to copy conversion scripts, exiting."); @@ -1265,7 +1265,7 @@ int file_copy(const char *file_old, const char *file_new) fclose(source); fclose(target); - return ret; + return OK; } void handle_command(struct vehicle_command_s *cmd) From 50e3bb28c90bb2cb93f9f0a549cf9c4838973e1c Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Wed, 14 Aug 2013 14:56:27 +0200 Subject: [PATCH 25/25] Fixed power attribute on FMU, contributed by Tridge --- nuttx-configs/px4fmu-v1/nsh/defconfig | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) diff --git a/nuttx-configs/px4fmu-v1/nsh/defconfig b/nuttx-configs/px4fmu-v1/nsh/defconfig index d338161296..9f088c37a2 100644 --- a/nuttx-configs/px4fmu-v1/nsh/defconfig +++ b/nuttx-configs/px4fmu-v1/nsh/defconfig @@ -534,9 +534,10 @@ CONFIG_USBDEV=y # # CONFIG_USBDEV_ISOCHRONOUS is not set # CONFIG_USBDEV_DUALSPEED is not set -CONFIG_USBDEV_SELFPOWERED=y -# CONFIG_USBDEV_BUSPOWERED is not set +# CONFIG_USBDEV_SELFPOWERED is not set +CONFIG_USBDEV_BUSPOWERED=y CONFIG_USBDEV_MAXPOWER=500 +# CONFIG_USBDEV_REMOTEWAKEUP is not set # CONFIG_USBDEV_DMA is not set # CONFIG_USBDEV_TRACE is not set