diff --git a/src/plugins/RangeTestPlugin.cpp b/src/plugins/RangeTestPlugin.cpp index 8f03589c..c719ad04 100644 --- a/src/plugins/RangeTestPlugin.cpp +++ b/src/plugins/RangeTestPlugin.cpp @@ -5,13 +5,13 @@ #include "Router.h" #include "configuration.h" #include +#include #include /* As a sender, I can send packets every n-seonds. These packets include an incramented PacketID. As a receiver, I can receive packets from multiple senders. These packets can be saved to the spiffs. - */ RangeTestPlugin *rangeTestPlugin; @@ -19,9 +19,11 @@ RangeTestPluginRadio *rangeTestPluginRadio; RangeTestPlugin::RangeTestPlugin() : concurrency::OSThread("RangeTestPlugin") {} -uint16_t packetSequence = 0; +uint32_t packetSequence = 0; -// char serialStringChar[Constants_DATA_PAYLOAD_LEN]; +#define SEC_PER_DAY 86400 +#define SEC_PER_HOUR 3600 +#define SEC_PER_MIN 60 int32_t RangeTestPlugin::runOnce() { @@ -34,8 +36,12 @@ int32_t RangeTestPlugin::runOnce() // radioConfig.preferences.range_test_plugin_enabled = 1; // radioConfig.preferences.range_test_plugin_sender = 0; + // radioConfig.preferences.range_test_plugin_save = 1; + + // Fixed position is useful when testing indoors. // radioConfig.preferences.fixed_position = 1; + uint32_t senderHeartbeat = radioConfig.preferences.range_test_plugin_sender * 1000; if (radioConfig.preferences.range_test_plugin_enabled) { @@ -137,6 +143,9 @@ bool RangeTestPluginRadio::handleReceived(const MeshPacket &mp) NodeInfo *n = nodeDB.getNode(mp.from); + if (radioConfig.preferences.range_test_plugin_save) { + appendFile(mp); + } DEBUG_MSG("-----------------------------------------\n"); DEBUG_MSG("p.payload.bytes \"%s\"\n", p.payload.bytes); @@ -171,3 +180,125 @@ bool RangeTestPluginRadio::handleReceived(const MeshPacket &mp) return true; // Let others look at this message also if they want } + +/// Ported from my old java code, returns distance in meters along the globe +/// surface (by magic?) +float RangeTestPluginRadio::latLongToMeter(double lat_a, double lng_a, double lat_b, double lng_b) +{ + double pk = (180 / 3.14169); + double a1 = lat_a / pk; + double a2 = lng_a / pk; + double b1 = lat_b / pk; + double b2 = lng_b / pk; + double cos_b1 = cos(b1); + double cos_a1 = cos(a1); + double t1 = cos_a1 * cos(a2) * cos_b1 * cos(b2); + double t2 = cos_a1 * sin(a2) * cos_b1 * sin(b2); + double t3 = sin(a1) * sin(b1); + double tt = acos(t1 + t2 + t3); + if (isnan(tt)) + tt = 0.0; // Must have been the same point? + + return (float)(6366000 * tt); +} + +bool RangeTestPluginRadio::appendFile(const MeshPacket &mp) +{ + auto &p = mp.decoded.data; + + NodeInfo *n = nodeDB.getNode(mp.from); + /* + DEBUG_MSG("-----------------------------------------\n"); + DEBUG_MSG("p.payload.bytes \"%s\"\n", p.payload.bytes); + DEBUG_MSG("p.payload.size %d\n", p.payload.size); + DEBUG_MSG("---- Received Packet:\n"); + DEBUG_MSG("mp.from %d\n", mp.from); + DEBUG_MSG("mp.rx_snr %f\n", mp.rx_snr); + DEBUG_MSG("mp.hop_limit %d\n", mp.hop_limit); + // DEBUG_MSG("mp.decoded.position.latitude_i %d\n", mp.decoded.position.latitude_i); // Depricated + // DEBUG_MSG("mp.decoded.position.longitude_i %d\n", mp.decoded.position.longitude_i); // Depricated + DEBUG_MSG("---- Node Information of Received Packet (mp.from):\n"); + DEBUG_MSG("n->user.long_name %s\n", n->user.long_name); + DEBUG_MSG("n->user.short_name %s\n", n->user.short_name); + DEBUG_MSG("n->user.macaddr %X\n", n->user.macaddr); + DEBUG_MSG("n->has_position %d\n", n->has_position); + DEBUG_MSG("n->position.latitude_i %d\n", n->position.latitude_i); + DEBUG_MSG("n->position.longitude_i %d\n", n->position.longitude_i); + DEBUG_MSG("n->position.battery_level %d\n", n->position.battery_level); + DEBUG_MSG("---- Current device location information:\n"); + DEBUG_MSG("gpsStatus->getLatitude() %d\n", gpsStatus->getLatitude()); + DEBUG_MSG("gpsStatus->getLongitude() %d\n", gpsStatus->getLongitude()); + DEBUG_MSG("gpsStatus->getHasLock() %d\n", gpsStatus->getHasLock()); + DEBUG_MSG("gpsStatus->getDOP() %d\n", gpsStatus->getDOP()); + DEBUG_MSG("-----------------------------------------\n"); + */ + if (!SPIFFS.begin(true)) { + DEBUG_MSG("An Error has occurred while mounting SPIFFS\n"); + return 0; + } + + if (SPIFFS.totalBytes() - SPIFFS.usedBytes() < 51200) { + DEBUG_MSG("SPIFFS doesn't have enough free space. Abourting write.\n"); + return 0; + } + + //--------- Write to file + File fileToWrite = SPIFFS.open("/static/rangetest.csv", FILE_WRITE); + + if (!fileToWrite) { + DEBUG_MSG("There was an error opening the file for writing\n"); + return 0; + } + + if (fileToWrite.println("time,sender mac,rx snr,sender lat,sender long,rx lat,rx long,distance,payload")) { + DEBUG_MSG("File was written\n"); + } else { + DEBUG_MSG("File write failed\n"); + } + + fileToWrite.close(); + + //--------- Apend content to file + File fileToAppend = SPIFFS.open("/static/rangetest.csv", FILE_APPEND); + + if (!fileToAppend) { + DEBUG_MSG("There was an error opening the file for appending\n"); + return 0; + } + + struct timeval tv; + if (!gettimeofday(&tv, NULL)) { + long hms = tv.tv_sec % SEC_PER_DAY; + // hms += tz.tz_dsttime * SEC_PER_HOUR; + // hms -= tz.tz_minuteswest * SEC_PER_MIN; + // mod `hms` to ensure in positive range of [0...SEC_PER_DAY) + hms = (hms + SEC_PER_DAY) % SEC_PER_DAY; + + // Tear apart hms into h:m:s + int hour = hms / SEC_PER_HOUR; + int min = (hms % SEC_PER_HOUR) / SEC_PER_MIN; + int sec = (hms % SEC_PER_HOUR) % SEC_PER_MIN; // or hms % SEC_PER_MIN + + fileToAppend.printf("%02d:%02d:%02d ", hour, min, sec); // Time + } else { + fileToAppend.printf("??:??:?? "); // Time + } + + fileToAppend.printf("$X,", n->user.macaddr); // Mac Address + fileToAppend.printf("%f,", mp.rx_snr); // RX SNR + fileToAppend.printf("%d,", n->position.latitude_i); // Sender Lat + fileToAppend.printf("%d,", n->position.longitude_i); // Sender Long + fileToAppend.printf("%d,", gpsStatus->getLatitude()); // RX Lat + fileToAppend.printf("%d,", gpsStatus->getLongitude()); // RX Long + + float distance = latLongToMeter(n->position.latitude_i * 1e-7, n->position.longitude_i * 1e-7, + gpsStatus->getLatitude() * 1e-7, gpsStatus->getLongitude() * 1e-7); + fileToAppend.printf("%f,", distance); // Distance in meters + + // TODO: If quotes are found in the payload, it has to be escaped. + fileToAppend.printf("\"%s\"\n", p.payload.bytes); + + fileToAppend.close(); + + return 1; +} diff --git a/src/plugins/RangeTestPlugin.h b/src/plugins/RangeTestPlugin.h index f66744db..8a10ca39 100644 --- a/src/plugins/RangeTestPlugin.h +++ b/src/plugins/RangeTestPlugin.h @@ -28,7 +28,6 @@ class RangeTestPluginRadio : public SinglePortPlugin uint32_t lastRxID; public: - RangeTestPluginRadio() : SinglePortPlugin("RangeTestPluginRadio", PortNum_TEXT_MESSAGE_APP) {} /** @@ -36,6 +35,16 @@ class RangeTestPluginRadio : public SinglePortPlugin */ void sendPayload(NodeNum dest = NODENUM_BROADCAST, bool wantReplies = false); + /** + * Append range test data to the file on the spiffs + */ + bool appendFile(const MeshPacket &mp); + + /** + * Kevin's magical calculation of two points to meters. + */ + float latLongToMeter(double lat_a, double lng_a, double lat_b, double lng_b); + protected: virtual MeshPacket *allocReply();