diff --git a/Makefile b/Makefile index da7d3a4..4102d3f 100644 --- a/Makefile +++ b/Makefile @@ -32,7 +32,7 @@ simulator: build_simulator gazebo --verbose ${CURDIR}/gazebo/flix.world log: - PORT=$(PORT) tools/grab_log.py + tools/log.py plot: plotjuggler -d $(shell ls -t tools/log/*.csv | head -n1) diff --git a/flix/cli.ino b/flix/cli.ino index 90ccbec..47c0ce8 100644 --- a/flix/cli.ino +++ b/flix/cli.ino @@ -38,7 +38,7 @@ const char* motd = "stab/acro/auto - set mode\n" "rc - show RC data\n" "mot - show motor output\n" -"log - dump in-RAM log\n" +"log [dump] - print log header [and data]\n" "cr - calibrate RC\n" "ca - calibrate accel\n" "mfr, mfl, mrr, mrl - test motor (remove props)\n" @@ -135,7 +135,8 @@ void doCommand(String str, bool echo = false) { print("front-right %g front-left %g rear-right %g rear-left %g\n", motors[MOTOR_FRONT_RIGHT], motors[MOTOR_FRONT_LEFT], motors[MOTOR_REAR_RIGHT], motors[MOTOR_REAR_LEFT]); } else if (command == "log") { - dumpLog(); + printLogHeader(); + if (arg0 == "dump") printLogData(); } else if (command == "cr") { calibrateRC(); } else if (command == "ca") { diff --git a/flix/log.ino b/flix/log.ino index 0309ee4..6ccd6c6 100644 --- a/flix/log.ino +++ b/flix/log.ino @@ -61,12 +61,13 @@ void logData() { } } -void dumpLog() { - // Print header +void printLogHeader() { for (int i = 0; i < logColumns; i++) { print("%s%s", logEntries[i].name, i < logColumns - 1 ? "," : "\n"); } - // Print data +} + +void printLogData() { for (int i = 0; i < LOG_SIZE; i++) { if (logBuffer[i][0] == 0) continue; // skip empty records for (int j = 0; j < logColumns; j++) { diff --git a/flix/mavlink.ino b/flix/mavlink.ino index 7d90517..cf69de3 100644 --- a/flix/mavlink.ino +++ b/flix/mavlink.ino @@ -210,6 +210,20 @@ void handleMavlink(const void *_msg) { armed = motors[0] > 0 || motors[1] > 0 || motors[2] > 0 || motors[3] > 0; } + if (msg.msgid == MAVLINK_MSG_ID_LOG_REQUEST_DATA) { + mavlink_log_request_data_t m; + mavlink_msg_log_request_data_decode(&msg, &m); + if (m.target_system && m.target_system != SYSTEM_ID) return; + + // Send all log records + for (int i = 0; i < sizeof(logBuffer) / sizeof(logBuffer[0]); i++) { + mavlink_message_t msg; + mavlink_msg_log_data_pack(SYSTEM_ID, MAV_COMP_ID_AUTOPILOT1, &msg, 0, i, + sizeof(logBuffer[0]), (uint8_t *)logBuffer[i]); + sendMessage(&msg); + } + } + // Handle commands if (msg.msgid == MAVLINK_MSG_ID_COMMAND_LONG) { mavlink_command_long_t m; diff --git a/gazebo/flix.h b/gazebo/flix.h index cc0af04..05062d2 100644 --- a/gazebo/flix.h +++ b/gazebo/flix.h @@ -45,7 +45,8 @@ void normalizeRC(); void calibrateRC(); void calibrateRCChannel(float *channel, uint16_t zero[16], uint16_t max[16], const char *str); void printRCCalibration(); -void dumpLog(); +void printLogHeader(); +void printLogData(); void processMavlink(); void sendMavlink(); void sendMessage(const void *msg); diff --git a/tools/log.py b/tools/log.py index 547f48a..a1e3f1a 100755 --- a/tools/log.py +++ b/tools/log.py @@ -3,21 +3,49 @@ # Download flight log remotely and save to file import os +import time import datetime +import struct +from pymavlink.dialects.v20.common import MAVLink_log_data_message from pyflix import Flix DIR = os.path.dirname(os.path.realpath(__file__)) - flix = Flix() print('Downloading log...') -lines = flix.cli('log').splitlines() -# sort by timestamp -header = lines.pop(0) -lines.sort(key=lambda line: float(line.split(',')[0])) +header = flix.cli('log') +print('Received header:\n- ' + '\n- '.join(header.split(','))) + +records = [] + +def on_record(msg: MAVLink_log_data_message): + global stop + stop = time.time() + 1 # extend timeout + records.append([]) + i = 0 + data = bytes(msg.data) + while i + 4 <= msg.count: + records[-1].append(struct.unpack('