#27 Added support for service packets in TDoA 3. Also added python sc…

…ript to set all anchor positions in one go.
bitcraze · May 14, 2018 · 0b24094 · 0b24094
1 parent 9a0b9c3
commit 0b24094
Show file tree

Hide file tree

Showing 3 changed files with 114 additions and 21 deletions.
diff --git a/src/uwb_tdoa_anchor3.c b/src/uwb_tdoa_anchor3.c
@@ -341,27 +341,14 @@ static bool extractFromPacket(const rangePacket3_t* rangePacket, uint32_t* remot
   return false;
 }
 
-static void handleRxPacket(dwDevice_t *dev)
-{
-  static packet_t rxPacket;
-  dwTime_t rxTime = { .full = 0 };
-
-  dwGetRawReceiveTimestamp(dev, &rxTime);
-  dwCorrectTimestamp(dev, &rxTime);
-
-  int dataLength = dwGetDataLength(dev);
-  rxPacket.payload[0] = 0;
-  dwGetData(dev, (uint8_t*)&rxPacket, dataLength);
-
-  if (dataLength == 0 || rxPacket.payload[0] != PACKET_TYPE_TDOA3) {
-    return;
-  }
 
-  const uint8_t remoteAnchorId = rxPacket.sourceAddress[0];
+static void handleRangePacket(const uint32_t rxTime, const packet_t* rxPacket)
+{
+  const uint8_t remoteAnchorId = rxPacket->sourceAddress[0];
   ctx.anchorRxCount[remoteAnchorId]++;
   anchorContext_t* anchorCtx = getContext(remoteAnchorId);
   if (anchorCtx) {
-    const rangePacket3_t* rangePacket = (rangePacket3_t *)rxPacket.payload;
+    const rangePacket3_t* rangePacket = (rangePacket3_t *)rxPacket->payload;
 
     uint32_t remoteTx = rangePacket->header.txTimeStamp;
     uint8_t remoteTxSeqNr = rangePacket->header.seq;
@@ -371,22 +358,48 @@ static void handleRxPacket(dwDevice_t *dev)
     bool dataFound = extractFromPacket(rangePacket, &remoteRx, &remoteRxSeqNr);
 
     if (dataFound) {
-      uint16_t distance = calculateDistance(anchorCtx, remoteTxSeqNr, remoteRxSeqNr, remoteTx, remoteRx, rxTime.low32);
+      uint16_t distance = calculateDistance(anchorCtx, remoteTxSeqNr, remoteRxSeqNr, remoteTx, remoteRx, rxTime);
 
       if (distance > 0) {
         anchorCtx->distance = distance;
       }
 
-      anchorCtx->rxTimeStamp = rxTime.low32;
+      anchorCtx->rxTimeStamp = rxTime;
       anchorCtx->seqNr = remoteTxSeqNr;
       anchorCtx->txTimeStamp = remoteTx;
     }
   }
 }
 
-// TODO krri Handle service packets
+static void handleRxPacket(dwDevice_t *dev)
+{
+  static packet_t rxPacket;
+  dwTime_t rxTime = { .full = 0 };
+
+  dwGetRawReceiveTimestamp(dev, &rxTime);
+  dwCorrectTimestamp(dev, &rxTime);
+
+  int dataLength = dwGetDataLength(dev);
+  rxPacket.payload[0] = 0;
+  dwGetData(dev, (uint8_t*)&rxPacket, dataLength);
+
+  if (dataLength == 0) {
+    return;
+  }
+
+  switch(rxPacket.payload[0]) {
+  case PACKET_TYPE_TDOA3:
+    handleRangePacket(rxTime.low32, &rxPacket);
+    break;
+  case SHORT_LPP:
+    lppHandleShortPacket(&rxPacket.payload[1], dataLength - MAC802154_HEADER_LENGTH - 1);
+    break;
+  default:
+    // Do nothing
+    break;
+  }
+}
 
-// Setup the radio to receive a packet in the next timeslot
 static void setupRx(dwDevice_t *dev)
 {
   dwNewReceive(dev);

diff --git a/tools/lpp/positions_example.yaml b/tools/lpp/positions_example.yaml
@@ -0,0 +1,9 @@
+---
+0: {x: -2.03, y: -2.14, z: 0.17}
+1: {x: -1.51, y: 1.8, z: 0.15}
+2: {x: 1.78, y: 1.76, z: 2.41}
+3: {x: 1.30, y: -1.94, z: 0.27}
+4: {x: -1.50, y: -1.46, z: 2.50}
+5: {x: -1.56, y: 1.81, z: 2.38}
+6: {x: 1.82, y: 1.80, z: 0.15}
+7: {x: 1.29, y: -1.83, z: 2.53}
diff --git a/tools/lpp/set_positions.py b/tools/lpp/set_positions.py
@@ -0,0 +1,71 @@
+# -*- coding: utf-8 -*-
+#
+#     ||          ____  _ __
+#  +------+      / __ )(_) /_______________ _____  ___
+#  | 0xBC |     / __  / / __/ ___/ ___/ __ `/_  / / _ \
+#  +------+    / /_/ / / /_/ /__/ /  / /_/ / / /_/  __/
+#   ||  ||    /_____/_/\__/\___/_/   \__,_/ /___/\___/
+#
+#  Copyright (C) 2018 Bitcraze AB
+#
+#  Crazyflie Nano Quadcopter Client
+#
+#  This program is free software; you can redistribute it and/or
+#  modify it under the terms of the GNU General Public License
+#  as published by the Free Software Foundation; either version 2
+#  of the License, or (at your option) any later version.
+#
+#  This program is distributed in the hope that it will be useful,
+#  but WITHOUT ANY WARRANTY; without even the implied warranty of
+#  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+#  GNU General Public License for more details.
+#  You should have received a copy of the GNU General Public License
+#  along with this program; if not, write to the Free Software
+#  Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston,
+#  MA  02110-1301, USA.
+"""
+Sets the position data in anchors.
+
+Requires a system in TDoA mode and a Crazyflie with the LPS deck.
+
+Use by piping yaml document with positions into the script
+cat ./tools/lpp/positions_example.yaml | python3 tools/lpp/set_positions.py
+"""
+import logging
+import time
+import struct
+import sys
+import yaml
+
+import cflib.crtp
+from cflib.crazyflie import Crazyflie
+from cflib.crazyflie.syncCrazyflie import SyncCrazyflie
+
+uri = 'radio://0/80/2M/E7E7E7E7E7'
+
+# Only output errors from the logging framework
+logging.basicConfig(level=logging.ERROR)
+
+# Initialize the low-level drivers (don't list the debug drivers)
+cflib.crtp.init_drivers(enable_debug_driver=False)
+
+cf = Crazyflie(rw_cache='./cache')
+with SyncCrazyflie(uri, cf=cf) as scf:
+    print("Setting positions")
+
+    for packet in yaml.load_all(sys.stdin, Loader=yaml.CLoader):
+        if not packet:
+            continue
+
+        for _ in range(5):
+            for id, position in packet.items():
+                x = position['x']
+                y = position['y']
+                z = position['z']
+
+                print("Anchor {}, ({}, {}, {})".format(id, x, y, z))
+                LPP_SHORT_ANCHOR_POSITION = 0x01
+                position_pack = struct.pack("<Bfff", LPP_SHORT_ANCHOR_POSITION, x, y, z)
+                scf.cf.loc.send_short_lpp_packet(id, position_pack)
+
+            time.sleep(0.5)