fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-03 16:58:54 +08:00
Code Issues Packages Projects Releases Wiki Activity

Working on debug output.

Browse Source
This commit is contained in:
James Goppert
2013-07-28 00:05:56 -04:00
parent 77c084a4cf
commit 4cfcea1767
1 changed files with 14 additions and 11 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/drivers/md25/md25.cpp
+14 -11
View File
@@ -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);
}