From 4cfcea176785975590d1d2952eb0b0270a6949b3 Mon Sep 17 00:00:00 2001 From: James Goppert Date: Sat, 29 Jun 2013 16:53:24 -0400 Subject: Working on debug output. --- src/drivers/md25/md25.cpp | 25 ++++++++++++++----------- 1 file changed, 14 insertions(+), 11 deletions(-) (limited to 'src') diff --git a/src/drivers/md25/md25.cpp b/src/drivers/md25/md25.cpp index f265ec451..9dac5e5ea 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); } -- cgit v1.2.3