Merge pull request #684 from geeksville/dev

Dev

proto

@@ -1 +1 @@
-Subproject commit a76ceb1509b2ec3d844af0378e998e7d4737492c
+Subproject commit b1aed06442025624841b2288fac273d9bc41c438

src/mesh/MeshPacketQueue.cpp

@@ -0,0 +1,91 @@
+#include "MeshPacketQueue.h"
+
+#include <algorithm>
+
+/// @return the priority of the specified packet
+inline uint32_t getPriority(MeshPacket *p)
+{
+    auto pri = p->priority;
+    return pri;
+}
+
+/// @return "true" if "p1" is ordered before "p2"
+bool CompareMeshPacket::operator()(MeshPacket *p1, MeshPacket *p2)
+{
+    assert(p1 && p2);
+    auto p1p = getPriority(p1), p2p = getPriority(p2);
+
+    // If priorities differ, use that
+    // for equal priorities, order by id (older packets have higher priority - this will briefly be wrong when IDs roll over but
+    // no big deal)
+    return (p1p != p2p) ? (p1p < p2p)         // prefer bigger priorities
+                        : (p1->id >= p2->id); // prefer smaller packet ids
+}
+
+MeshPacketQueue::MeshPacketQueue(size_t _maxLen) : maxLen(_maxLen) {}
+
+/** Some clients might not properly set priority, therefore we fix it here.
+ */
+void fixPriority(MeshPacket *p)
+{
+    // We might receive acks from other nodes (and since generated remotely, they won't have priority assigned.  Check for that
+    // and fix it
+    if (p->priority == MeshPacket_Priority_UNSET) {
+        // if acks give high priority
+        // if a reliable message give a bit higher default priority
+        p->priority = p->decoded.which_ackVariant ? MeshPacket_Priority_ACK :                
+                          (p->want_ack ? MeshPacket_Priority_RELIABLE : MeshPacket_Priority_DEFAULT);
+    }
+}
+
+/** enqueue a packet, return false if full */
+bool MeshPacketQueue::enqueue(MeshPacket *p)
+{
+
+    fixPriority(p);
+
+    // fixme if there is something lower priority in the queue that can be deleted to make space, delete that instead
+    if (size() >= maxLen)
+        return false;
+    else {
+        push(p);
+        return true;
+    }
+}
+
+MeshPacket *MeshPacketQueue::dequeue()
+{
+    if (empty())
+        return NULL;
+    else {
+        auto p = top();
+        pop(); // remove the first item
+        return p;
+    }
+}
+
+// this is kinda yucky, but I'm not sure if all arduino c++ compilers support closuers.  And we only have one
+// thread that can run at a time - so safe
+static NodeNum findFrom;
+static PacketId findId;
+
+static bool isMyPacket(MeshPacket *p)
+{
+    return p->id == findId && p->from == findFrom;
+}
+
+/** Attempt to find and remove a packet from this queue.  Returns true the packet which was removed from the queue */
+MeshPacket *MeshPacketQueue::remove(NodeNum from, PacketId id)
+{
+    findFrom = from;
+    findId = id;
+    auto it = std::find_if(this->c.begin(), this->c.end(), isMyPacket);
+    if (it != this->c.end()) {
+        auto p = *it;
+        this->c.erase(it);
+        std::make_heap(this->c.begin(), this->c.end(), this->comp);
+        return p;
+    } else {
+        return NULL;
+    }
+}

src/mesh/MeshPacketQueue.h

@@ -0,0 +1,33 @@
+#pragma once
+
+#include "MeshTypes.h"
+
+#include <assert.h>
+#include <queue>
+
+// this is an strucure which implements the
+// operator overloading
+struct CompareMeshPacket {
+    bool operator()(MeshPacket *p1, MeshPacket *p2);
+};
+
+/**
+ * A priority queue of packets.
+ *
+ */
+class MeshPacketQueue : public std::priority_queue<MeshPacket *, std::vector<MeshPacket *>, CompareMeshPacket>
+{
+    size_t maxLen;
+  public:
+    MeshPacketQueue(size_t _maxLen);
+
+    /** enqueue a packet, return false if full */
+    bool enqueue(MeshPacket *p);
+
+    // bool isEmpty();
+
+    MeshPacket *dequeue();
+
+    /** Attempt to find and remove a packet from this queue.  Returns true the packet which was removed from the queue */
+    MeshPacket *remove(NodeNum from, PacketId id);
+};

src/mesh/MeshService.cpp

@@ -169,6 +169,11 @@ void MeshService::handleToRadio(MeshPacket &p)
     }
 }
 
+/** Attempt to cancel a previously sent packet from this _local_ node.  Returns true if a packet was found we could cancel */
+bool MeshService::cancelSending(PacketId id) {
+    return router->cancelSending(nodeDB.getNodeNum(), id);
+}
+
 void MeshService::sendToMesh(MeshPacket *p)
 {
     nodeDB.updateFrom(*p); // update our local DB for this packet (because phone might have sent position packets etc...)

src/mesh/MeshService.h

@@ -79,6 +79,9 @@ class MeshService
     /// cache
     void sendToMesh(MeshPacket *p);
 
+    /** Attempt to cancel a previously sent packet from this _local_ node.  Returns true if a packet was found we could cancel */
+    bool cancelSending(PacketId id);
+
     /// Pull the latest power and time info into my nodeinfo
     NodeInfo *refreshMyNodeInfo();
 

src/mesh/NodeDB.cpp

@@ -600,7 +600,7 @@ void recordCriticalError(CriticalErrorCode code, uint32_t address)
     // Print error to screen and serial port
     String lcd = String("Critical error ") + code + "!\n";
     screen->print(lcd.c_str());
-    DEBUG_MSG("NOTE! Recording critical error %d, address=%x\n", code, address);
+    DEBUG_MSG("NOTE! Recording critical error %d, address=%lx\n", code, address);
     
     // Record error to DB
     myNodeInfo.error_code = code;

src/mesh/RadioInterface.cpp

@@ -161,6 +161,9 @@ void printPacket(const char *prefix, const MeshPacket *p)
     if (p->rx_snr != 0.0) {
         DEBUG_MSG(" rxSNR=%g", p->rx_snr);
     }
+    if(p->priority != 0)
+        DEBUG_MSG(" priority=%d", p->priority);
+        
     DEBUG_MSG(")\n");
 }
 

src/mesh/RadioInterface.h

@@ -105,6 +105,9 @@ class RadioInterface
      */
     virtual ErrorCode send(MeshPacket *p) = 0;
 
+    /** Attempt to cancel a previously sent packet.  Returns true if a packet was found we could cancel */
+    virtual bool cancelSending(NodeNum from, PacketId id) { return false; }
+
     // methods from radiohead
 
     /// Initialise the Driver transport hardware and software.

src/mesh/RadioLibInterface.cpp

@@ -100,7 +100,7 @@ ErrorCode RadioLibInterface::send(MeshPacket *p)
     uint32_t xmitMsec = getPacketTime(p);
 
     DEBUG_MSG("txGood=%d,rxGood=%d,rxBad=%d\n", txGood, rxGood, rxBad);
-    ErrorCode res = txQueue.enqueue(p, 0) ? ERRNO_OK : ERRNO_UNKNOWN;
+    ErrorCode res = txQueue.enqueue(p) ? ERRNO_OK : ERRNO_UNKNOWN;
 
     if (res != ERRNO_OK) { // we weren't able to queue it, so we must drop it to prevent leaks
         packetPool.release(p);
@@ -125,13 +125,25 @@ ErrorCode RadioLibInterface::send(MeshPacket *p)
 
 bool RadioLibInterface::canSleep()
 {
-    bool res = txQueue.isEmpty();
+    bool res = txQueue.empty();
     if (!res) // only print debug messages if we are vetoing sleep
         DEBUG_MSG("radio wait to sleep, txEmpty=%d\n", res);
 
     return res;
 }
 
+/** Attempt to cancel a previously sent packet.  Returns true if a packet was found we could cancel */
+bool RadioLibInterface::cancelSending(NodeNum from, PacketId id) {
+    auto p = txQueue.remove(from, id);
+    if(p)
+        packetPool.release(p); // free the packet we just removed
+
+    bool result = (p != NULL);
+    DEBUG_MSG("cancelSending id=0x%x, removed=%d", id, result);
+    return result;
+}
+
+
 /** radio helper thread callback.
 
 We never immediately transmit after any operation (either rx or tx).  Instead we should start receiving and
@@ -165,12 +177,12 @@ void RadioLibInterface::onNotify(uint32_t notification)
 
         // If we are not currently in receive mode, then restart the timer and try again later (this can happen if the main thread
         // has placed the unit into standby)  FIXME, how will this work if the chipset is in sleep mode?
-        if (!txQueue.isEmpty()) {
+        if (!txQueue.empty()) {
             if (!canSendImmediately()) {
                 startTransmitTimer(); // try again in a little while
             } else {
                 // Send any outgoing packets we have ready
-                MeshPacket *txp = txQueue.dequeuePtr(0);
+                MeshPacket *txp = txQueue.dequeue();
                 assert(txp);
                 startSend(txp);
             }
@@ -186,7 +198,7 @@ void RadioLibInterface::onNotify(uint32_t notification)
 void RadioLibInterface::startTransmitTimer(bool withDelay)
 {
     // If we have work to do and the timer wasn't already scheduled, schedule it now
-    if (!txQueue.isEmpty()) {
+    if (!txQueue.empty()) {
         uint32_t delay = !withDelay ? 1 : getTxDelayMsec();
         // DEBUG_MSG("xmit timer %d\n", delay);
         notifyLater(delay, TRANSMIT_DELAY_COMPLETED, false); // This will implicitly enable

src/mesh/RadioLibInterface.h

@@ -2,6 +2,7 @@
 
 #include "../concurrency/OSThread.h"
 #include "RadioInterface.h"
+#include "MeshPacketQueue.h"
 
 #ifdef CubeCell_BoardPlus
 #define RADIOLIB_SOFTWARE_SERIAL_UNSUPPORTED
@@ -74,7 +75,7 @@ class RadioLibInterface : public RadioInterface, protected concurrency::Notified
      */
     uint32_t rxBad = 0, rxGood = 0, txGood = 0;
 
-    PointerQueue<MeshPacket> txQueue = PointerQueue<MeshPacket>(MAX_TX_QUEUE);
+    MeshPacketQueue txQueue = MeshPacketQueue(MAX_TX_QUEUE);
 
   protected:
 
@@ -136,6 +137,9 @@ class RadioLibInterface : public RadioInterface, protected concurrency::Notified
      */
     virtual bool isActivelyReceiving() = 0;
 
+    /** Attempt to cancel a previously sent packet.  Returns true if a packet was found we could cancel */
+    virtual bool cancelSending(NodeNum from, PacketId id);
+
   private:
     /** if we have something waiting to send, start a short random timer so we can come check for collision before actually doing
      * the transmit

src/mesh/Router.cpp

@@ -120,10 +120,13 @@ void Router::sendAckNak(ErrorReason err, NodeNum to, PacketId idFrom)
         p->decoded.which_payloadVariant = SubPacket_error_reason_tag;
         p->decoded.error_reason = err;
     }
+    p->priority = MeshPacket_Priority_ACK;
 
     sendLocal(p); // we sometimes send directly to the local node
 }
 
+
+
 ErrorCode Router::sendLocal(MeshPacket *p)
 {
     // No need to deliver externally if the destination is the local node
@@ -199,6 +202,13 @@ ErrorCode Router::send(MeshPacket *p)
     } */
 }
 
+/** Attempt to cancel a previously sent packet.  Returns true if a packet was found we could cancel */
+bool Router::cancelSending(NodeNum from, PacketId id) {
+    return iface ? iface->cancelSending(from, id) : false;
+}
+
+
+
 /**
  * Every (non duplicate) packet this node receives will be passed through this method.  This allows subclasses to
  * update routing tables etc... based on what we overhear (even for messages not destined to our node)

src/mesh/Router.h

@@ -55,7 +55,12 @@ class Router : protected concurrency::OSThread
      */
     ErrorCode sendLocal(MeshPacket *p);
 
-    /// Allocate and return a meshpacket which defaults as send to broadcast from the current node.
+    /** Attempt to cancel a previously sent packet.  Returns true if a packet was found we could cancel */
+    bool cancelSending(NodeNum from, PacketId id);    
+
+    /** Allocate and return a meshpacket which defaults as send to broadcast from the current node.
+     * The returned packet is guaranteed to have a unique packet ID already assigned
+     */
     MeshPacket *allocForSending();
 
     /**

src/mesh/generated/deviceonly.pb.h

@@ -80,7 +80,7 @@ extern const pb_msgdesc_t DeviceState_msg;
 #define DeviceState_fields &DeviceState_msg
 
 /* Maximum encoded size of messages (where known) */
-#define DeviceState_size                         6262
+#define DeviceState_size                         6266
 
 #ifdef __cplusplus
 } /* extern "C" */

src/mesh/generated/mesh.pb.c

@@ -58,3 +58,4 @@ PB_BIND(ToRadio, ToRadio, 2)
 
 
 
+

src/mesh/generated/mesh.pb.h

@@ -83,6 +83,16 @@ typedef enum _CriticalErrorCode {
     CriticalErrorCode_TransmitFailed = 8
 } CriticalErrorCode;
 
+typedef enum _MeshPacket_Priority {
+    MeshPacket_Priority_UNSET = 0,
+    MeshPacket_Priority_MIN = 1,
+    MeshPacket_Priority_BACKGROUND = 10,
+    MeshPacket_Priority_DEFAULT = 64,
+    MeshPacket_Priority_RELIABLE = 70,
+    MeshPacket_Priority_ACK = 120,
+    MeshPacket_Priority_MAX = 127
+} MeshPacket_Priority;
+
 typedef enum _ChannelSettings_ModemConfig {
     ChannelSettings_ModemConfig_Bw125Cr45Sf128 = 0,
     ChannelSettings_ModemConfig_Bw500Cr45Sf128 = 1,
@@ -265,6 +275,7 @@ typedef struct _MeshPacket {
     uint32_t rx_time;
     uint32_t hop_limit;
     bool want_ack;
+    MeshPacket_Priority priority;
 } MeshPacket;
 
 typedef struct _FromRadio {
@@ -323,6 +334,10 @@ typedef struct _ToRadio {
 #define _CriticalErrorCode_MAX CriticalErrorCode_TransmitFailed
 #define _CriticalErrorCode_ARRAYSIZE ((CriticalErrorCode)(CriticalErrorCode_TransmitFailed+1))
 
+#define _MeshPacket_Priority_MIN MeshPacket_Priority_UNSET
+#define _MeshPacket_Priority_MAX MeshPacket_Priority_MAX
+#define _MeshPacket_Priority_ARRAYSIZE ((MeshPacket_Priority)(MeshPacket_Priority_MAX+1))
+
 #define _ChannelSettings_ModemConfig_MIN ChannelSettings_ModemConfig_Bw125Cr45Sf128
 #define _ChannelSettings_ModemConfig_MAX ChannelSettings_ModemConfig_Bw125Cr48Sf4096
 #define _ChannelSettings_ModemConfig_ARRAYSIZE ((ChannelSettings_ModemConfig)(ChannelSettings_ModemConfig_Bw125Cr48Sf4096+1))
@@ -342,7 +357,7 @@ extern "C" {
 #define User_init_default                        {"", "", "", {0}}
 #define RouteDiscovery_init_default              {0, {0, 0, 0, 0, 0, 0, 0, 0}}
 #define SubPacket_init_default                   {0, {Position_init_default}, 0, 0, 0, 0, {0}, 0}
-#define MeshPacket_init_default                  {0, 0, 0, {SubPacket_init_default}, 0, 0, 0, 0, 0, 0}
+#define MeshPacket_init_default                  {0, 0, 0, {SubPacket_init_default}, 0, 0, 0, 0, 0, 0, _MeshPacket_Priority_MIN}
 #define ChannelSettings_init_default             {0, _ChannelSettings_ModemConfig_MIN, {0, {0}}, "", 0, 0, 0, 0, 0, 0, 0}
 #define RadioConfig_init_default                 {false, RadioConfig_UserPreferences_init_default, false, ChannelSettings_init_default}
 #define RadioConfig_UserPreferences_init_default {0, 0, 0, 0, 0, 0, 0, 0, 0, 0, "", "", 0, _RegionCode_MIN, _ChargeCurrent_MIN, _LocationSharing_MIN, _GpsOperation_MIN, 0, 0, 0, 0, 0, 0, 0, 0, {0, 0, 0}, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0}
@@ -356,7 +371,7 @@ extern "C" {
 #define User_init_zero                           {"", "", "", {0}}
 #define RouteDiscovery_init_zero                 {0, {0, 0, 0, 0, 0, 0, 0, 0}}
 #define SubPacket_init_zero                      {0, {Position_init_zero}, 0, 0, 0, 0, {0}, 0}
-#define MeshPacket_init_zero                     {0, 0, 0, {SubPacket_init_zero}, 0, 0, 0, 0, 0, 0}
+#define MeshPacket_init_zero                     {0, 0, 0, {SubPacket_init_zero}, 0, 0, 0, 0, 0, 0, _MeshPacket_Priority_MIN}
 #define ChannelSettings_init_zero                {0, _ChannelSettings_ModemConfig_MIN, {0, {0}}, "", 0, 0, 0, 0, 0, 0, 0}
 #define RadioConfig_init_zero                    {false, RadioConfig_UserPreferences_init_zero, false, ChannelSettings_init_zero}
 #define RadioConfig_UserPreferences_init_zero    {0, 0, 0, 0, 0, 0, 0, 0, 0, 0, "", "", 0, _RegionCode_MIN, _ChargeCurrent_MIN, _LocationSharing_MIN, _GpsOperation_MIN, 0, 0, 0, 0, 0, 0, 0, 0, {0, 0, 0}, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0}
@@ -479,6 +494,7 @@ extern "C" {
 #define MeshPacket_rx_time_tag                   9
 #define MeshPacket_hop_limit_tag                 10
 #define MeshPacket_want_ack_tag                  11
+#define MeshPacket_priority_tag                  12
 #define FromRadio_num_tag                        1
 #define FromRadio_packet_tag                     2
 #define FromRadio_my_info_tag                    3
@@ -554,7 +570,8 @@ X(a, STATIC,   SINGULAR, UINT32,   id,                6) \
 X(a, STATIC,   SINGULAR, FLOAT,    rx_snr,            7) \
 X(a, STATIC,   SINGULAR, FIXED32,  rx_time,           9) \
 X(a, STATIC,   SINGULAR, UINT32,   hop_limit,        10) \
-X(a, STATIC,   SINGULAR, BOOL,     want_ack,         11)
+X(a, STATIC,   SINGULAR, BOOL,     want_ack,         11) \
+X(a, STATIC,   SINGULAR, UENUM,    priority,         12)
 #define MeshPacket_CALLBACK NULL
 #define MeshPacket_DEFAULT NULL
 #define MeshPacket_payloadVariant_decoded_MSGTYPE SubPacket
@@ -734,7 +751,7 @@ extern const pb_msgdesc_t ToRadio_msg;
 #define User_size                                72
 #define RouteDiscovery_size                      88
 #define SubPacket_size                           275
-#define MeshPacket_size                          320
+#define MeshPacket_size                          322
 #define ChannelSettings_size                     95
 #define RadioConfig_size                         405
 #define RadioConfig_UserPreferences_size         305

src/plugins/NodeInfoPlugin.cpp

@@ -28,9 +28,15 @@ bool NodeInfoPlugin::handleReceivedProtobuf(const MeshPacket &mp, const User &p)
 
 void NodeInfoPlugin::sendOurNodeInfo(NodeNum dest, bool wantReplies)
 {
+    // cancel any not yet sent (now stale) position packets
+    if(prevPacketId) // if we wrap around to zero, we'll simply fail to cancel in that rare case (no big deal)
+        service.cancelSending(prevPacketId);
+
     MeshPacket *p = allocReply();
     p->to = dest;
     p->decoded.want_response = wantReplies;
+    p->priority = MeshPacket_Priority_BACKGROUND;
+    prevPacketId = p->id;
 
     service.sendToMesh(p);
 }

src/plugins/NodeInfoPlugin.h

@@ -6,6 +6,9 @@
  */
 class NodeInfoPlugin : public ProtobufPlugin<User>
 {
+    /// The id of the last packet we sent, to allow us to cancel it if we make something fresher
+    PacketId prevPacketId = 0;
+      
   public:
     /** Constructor
      * name is for debugging output

src/plugins/PositionPlugin.cpp

@@ -37,9 +37,15 @@ MeshPacket *PositionPlugin::allocReply()
 
 void PositionPlugin::sendOurPosition(NodeNum dest, bool wantReplies)
 {
+    // cancel any not yet sent (now stale) position packets
+    if(prevPacketId) // if we wrap around to zero, we'll simply fail to cancel in that rare case (no big deal)
+        service.cancelSending(prevPacketId);
+
     MeshPacket *p = allocReply();
     p->to = dest;
     p->decoded.want_response = wantReplies;
+    p->priority = MeshPacket_Priority_BACKGROUND;
+    prevPacketId = p->id;
 
     service.sendToMesh(p);
 }

src/plugins/PositionPlugin.h

@@ -6,6 +6,9 @@
  */
 class PositionPlugin : public ProtobufPlugin<Position>
 {
+    /// The id of the last packet we sent, to allow us to cancel it if we make something fresher
+    PacketId prevPacketId = 0;
+
   public:
     /** Constructor
      * name is for debugging output

version.properties

@@ -1,4 +1,4 @@
 [VERSION]  
 major = 1  
 minor = 1
-build = 42
+build = 46