diff options
author | James Goppert <james.goppert@gmail.com> | 2013-06-29 15:58:58 -0400 |
---|---|---|
committer | James Goppert <james.goppert@gmail.com> | 2013-07-28 00:05:56 -0400 |
commit | 76d086e0773bda8252b9a59b737b902ddc52b59f (patch) | |
tree | 1ddda65033dcf35393c57063d1617e07f38a76cc | |
parent | 78ef6f5265d5f4e84d4e36be37fc7329587df0a3 (diff) | |
download | px4-firmware-76d086e0773bda8252b9a59b737b902ddc52b59f.tar.gz px4-firmware-76d086e0773bda8252b9a59b737b902ddc52b59f.tar.bz2 px4-firmware-76d086e0773bda8252b9a59b737b902ddc52b59f.zip |
Working with debug messages.
-rw-r--r-- | src/drivers/md25/md25.cpp | 49 |
1 files changed, 26 insertions, 23 deletions
diff --git a/src/drivers/md25/md25.cpp b/src/drivers/md25/md25.cpp index 375072bb0..4cb005721 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<debug_key_value_s> input_signal(NULL, + // debug publication + control::UOrbPublication<debug_key_value_s> 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<debug_key_value_s> output_signal(NULL, - ORB_ID(debug_key_value)); - strncpy(output_signal.key, "md25 out ", 10); - output_signal.timestamp_ms = hrt_absolute_time(); - output_signal.value = 0; // sine wave for motor 1 md25.resetEncoders(); while (true) { - float prev_revolution = md25.getRevolutions1(); - 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); |