#41 Added configuration for UWB radio bitrate and preamble length

inc/cfg.h

@@ -37,6 +37,8 @@ typedef enum {
   cfgSmartPower,
   cfgForceTxPower,
   cfgTxPower,
+  cfgLowBitrate,
+  cfgLongPreamble,
 } ConfigField;
 
 void cfgInit();

inc/lpp.h

@@ -36,6 +36,7 @@ void lppHandleShortPacket(char *data, size_t length);
 #define LPP_SHORT_REBOOT 0x02
 #define LPP_SHORT_MODE 0x03
 #define LPP_SHORT_UWB 0x04
+#define LPP_SHORT_UWB_MODE 0x05
 
 struct lppShortAnchorPosition_s {
   float position[3];
@@ -62,4 +63,9 @@ struct lppShortUWB_s {
   uint32_t txPower;
 } __attribute__((packed));
 
+struct lppShortUWBMode_s {
+  uint8_t enableLowBitrate :1;
+  uint8_t enableLongPreamble :1;
+} __attribute__((packed));
+
 #endif //__LPP_H__

inc/uwb.h

@@ -45,6 +45,9 @@ typedef struct uwbConfig_s {
   bool smartPower;
   bool forceTxPower;
   uint32_t txPower;
+
+  bool lowBitrate;
+  bool longPreamble;
 } uwbConfig_t;
 
 #define MODE_ANCHOR 0

src/lpp.c

@@ -119,6 +119,21 @@ void lppHandleShortPacket(char *data, size_t length)
       // Then resets!
       NVIC_SystemReset();
 
+      break;
+    }
+    case LPP_SHORT_UWB_MODE:
+    {
+      struct lppShortUWBMode_s *mode = (struct lppShortUWBMode_s*)&data[1];
+
+      // Set new power settings
+      debug("Low bitrate: %d, Long preamble: %d", mode->enableLowBitrate, mode->enableLongPreamble);
+
+      cfgWriteU8(cfgLowBitrate, mode->enableLowBitrate);
+      cfgWriteU8(cfgLongPreamble, mode->enableLongPreamble);
+
+      // Then resets!
+      NVIC_SystemReset();
+
       break;
     }
   }

src/main.c

@@ -56,8 +56,11 @@ static void changeAddress(uint8_t addr);
 static void handleSerialInput(char ch);
 static void handleButton(void);
 static void changeMode(unsigned int newMode);
+static void changeRadioMode(unsigned int newMode);
 static void printModeList();
+static void printRadioModeList();
 static void printMode();
+static void printRadioMode();
 static void help();
 static void bootload(void);
 
@@ -144,6 +147,8 @@ static void main_task(void *pvParameters) {
   if(uwbConfig->forceTxPower) {
     printf("CONFIG\t: TX power setting: %08X\r\n", (unsigned int)uwbConfig->txPower);
   }
+  printf("CONFIG\t: Bitrate: %s\r\n", uwbConfig->lowBitrate?"low":"normal");
+  printf("CONFIG\t: Preamble: %s\r\n", uwbConfig->longPreamble?"long":"normal");
 
   HAL_Delay(500);
 
@@ -208,7 +213,7 @@ int _write (int fd, const void *buf, size_t count)
 
 static void handleSerialInput(char ch) {
   bool configChanged = true;
-  static enum menu_e {mainMenu, modeMenu, idMenu} currentMenu = mainMenu;
+  static enum menu_e {mainMenu, modeMenu, idMenu, radioMenu} currentMenu = mainMenu;
   static unsigned int tempId = 0;
 
   switch (currentMenu) {
@@ -242,6 +247,12 @@ static void handleSerialInput(char ch) {
           currentMenu = modeMenu;
           configChanged = false;
           break;
+        case 'r':
+          printRadioModeList();
+          printf("Type 0-9 to choose new mode...\r\n");
+          currentMenu = radioMenu;
+          configChanged = false;
+          break;
         case 'd': restConfig(); break;
         case 'h':
           help();
@@ -285,6 +296,22 @@ static void handleSerialInput(char ch) {
           break;
       }
       break;
+    case radioMenu:
+      switch(ch) {
+        case '0':
+        case '1':
+        case '2':
+        case '3':
+          changeRadioMode(ch - '0');
+          currentMenu = mainMenu;
+          break;
+        default:
+          printf("Incorrect mode '%c'\r\n", ch);
+          currentMenu = mainMenu;
+          configChanged = false;
+          break;
+      }
+      break;
     case idMenu:
       switch(ch) {
         case '0':
@@ -401,6 +428,53 @@ static void printMode() {
   printf("\r\n");
 }
 
+static void changeRadioMode(unsigned int newMode) {
+    printf("Previous radio mode: ");
+    printRadioMode();
+
+    uint8_t lowBitrate = newMode & 1;
+    uint8_t longPreamble = (newMode & 2) / 2;
+
+    cfgWriteU8(cfgLowBitrate, lowBitrate);
+    cfgWriteU8(cfgLongPreamble, longPreamble);
+
+    printf("New radio mode: ");
+    printRadioMode();
+}
+
+static void printRadioMode() {
+  uint8_t lowBitrate;
+  uint8_t longPreamble;
+
+  cfgReadU8(cfgLowBitrate, &lowBitrate);
+  cfgReadU8(cfgLongPreamble, &longPreamble);
+
+  printf("%s bitrate, %s preamble", lowBitrate?"low":"normal", longPreamble?"long":"normal");
+  printf("\r\n");
+}
+
+static void printRadioModeList()
+{
+  uint8_t lowBitrate;
+  uint8_t longPreamble;
+
+  cfgReadU8(cfgLowBitrate, &lowBitrate);
+  cfgReadU8(cfgLongPreamble, &longPreamble);
+
+  printf("-------------------\r\n");
+  printf("NOTE: only change if you use TDoA3. Other modes will stop working if changed from the default mode.\r\n");
+  printf("\r\n");
+  printf("Current mode is "); printRadioMode();
+  printf("\r\n");
+  printf("Available radio modes:\r\n");
+  printf("0: normal bitrate, normal preamble (default)\r\n");
+  printf("1: low bitrate, normal preamble (default)\r\n");
+  printf("2: normal bitrate, long preamble (default)\r\n");
+  printf("3: low bitrate, long preamble (default)\r\n");
+}
+
+
+
 static void help() {
   printf("Help\r\n");
   printf("-------------------\r\n");
@@ -410,6 +484,7 @@ static void help() {
   printf("t   - tag mode\r\n");
   printf("s   - sniffer mode\r\n");
   printf("m   - List and change mode\r\n");
+  printf("r   - List and change UWB radio settings\r\n");
   printf("d   - reset configuration\r\n");
   printf("u   - enter BSL (DFU mode)\r\n");
   printf("h   - This help\r\n");

src/uwb.c

@@ -53,7 +53,7 @@ struct {
   {.algorithm = &uwbTwrTagAlgorithm,    .name = "TWR Tag"},
   {.algorithm = &uwbSnifferAlgorithm,   .name = "Sniffer"},
   {.algorithm = &uwbTdoa2Algorithm,     .name = "TDoA Anchor V2"},
-  {.algorithm = &uwbTdoa3Algorithm,     .name = "TDoA Anchor V3 (experimental)"},
+  {.algorithm = &uwbTdoa3Algorithm,     .name = "TDoA Anchor V3"},
   {NULL, NULL},
 };
 
@@ -131,11 +131,26 @@ void uwbInit()
   dwNewConfiguration(dwm);
   dwSetDefaults(dwm);
 
+  uint8_t useLowBitrate = 0;
+  cfgReadU8(cfgLowBitrate, &useLowBitrate);
   #ifdef LPS_LONGER_RANGE
-  dwEnableMode(dwm, MODE_SHORTDATA_MID_ACCURACY);
-  #else
-  dwEnableMode(dwm, MODE_SHORTDATA_FAST_ACCURACY);
+  useLowBitrate = 1;
   #endif
+  config.lowBitrate = (useLowBitrate == 1);
+
+  uint8_t useLongPreamble = 0;
+  cfgReadU8(cfgLongPreamble, &useLongPreamble);
+  config.longPreamble = (useLongPreamble == 1);
+
+  const uint8_t* mode = MODE_SHORTDATA_FAST_ACCURACY;
+  if (useLowBitrate && !useLongPreamble) {
+    mode = MODE_SHORTDATA_MID_ACCURACY;
+  } else if (!useLowBitrate && useLongPreamble) {
+    mode = MODE_LONGDATA_FAST_ACCURACY;
+  } else if (useLowBitrate && useLongPreamble) {
+    mode = MODE_LONGDATA_MID_ACCURACY;
+  }
+  dwEnableMode(dwm, mode);
 
   dwSetChannel(dwm, CHANNEL_2);
 

tools/lpp/set_radio_config.py

@@ -0,0 +1,124 @@
+# -*- coding: utf-8 -*-
+#
+#     ||          ____  _ __
+#  +------+      / __ )(_) /_______________ _____  ___
+#  | 0xBC |     / __  / / __/ ___/ ___/ __ `/_  / / _ \
+#  +------+    / /_/ / / /_/ /__/ /  / /_/ / / /_/  __/
+#   ||  ||    /_____/_/\__/\___/_/   \__,_/ /___/\___/
+#
+#  Copyright (C) 2018 Bitcraze AB
+#
+#  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 bitrate and preamble length of the UWB radio chip in LPS Anchors.
+
+Requires a system in TDoA3 mode and a Crazyflie with the LPS deck.
+
+The script will broadcast configuration settings through the Crazyflie over
+and over again.
+
+
+"""
+import logging
+import time
+import struct
+import sys
+
+import cflib.crtp
+from cflib.crazyflie import Crazyflie
+from cflib.crazyflie.syncCrazyflie import SyncCrazyflie
+import argparse
+
+
+def validate_anchor_id(id):
+    if id < 0 or id > 255:
+        raise Exception("Valid anchor ids are 0 - 255")
+
+
+def main():
+    parser = argparse.ArgumentParser(
+        description='Set bitrate and preamble length of LPS Anchors. A new '
+                    'configuration is sent to anchors using a Crazyflie with '
+                    'an LPS deck as a bridge.')
+
+    parser.add_argument('-u', '--uri',
+                        dest='uri',
+                        default='radio://0/80/2M/E7E7E7E7E7',
+                        help='The URI of the Crazyflie (default '
+                             'radio://0/80/2M/E7E7E7E7E7)')
+
+    parser.add_argument('-b', '--begin',
+                        dest='anchor_id_lower',
+                        default=0,
+                        type=int,
+                        help='The lower id in the anchor range to send to '
+                             '(default 0)')
+
+    parser.add_argument('-e', '--end',
+                        dest='anchor_id_upper',
+                        default=7,
+                        type=int,
+                        help='The upper id in the anchor range to send to '
+                             '(default 7)')
+
+    parser.add_argument('-r', '--rate',
+                        dest='low_bitrate',
+                        action='store_true',
+                        help='Configure low bitrate. Normal bitrate will be '
+                             'used if this switch is not set')
+
+    parser.add_argument('-p', '--preamble',
+                        dest='long_preamble',
+                        action='store_true',
+                        help='Configure long preamble. Normal preamble will '
+                             'be used if this switch is not set')
+
+    args = parser.parse_args()
+
+    validate_anchor_id(args.anchor_id_lower)
+    validate_anchor_id(args.anchor_id_upper)
+    if args.anchor_id_lower > args.anchor_id_upper:
+        raise Exception("(%i to %i) is not a valid anchor id range" % (
+            args.anchor_id_lower, args.anchor_id_upper
+        ))
+
+    print('Sending anchor radio configuration to anchors (%i to %i) '
+          'using %s.' % (
+        args.anchor_id_lower,
+        args.anchor_id_upper,
+        args.uri))
+    print('Setting %s bitrate and %s preamble. '
+          'Anchors will reset when configured.' % (
+              ['normal', 'low'][args.low_bitrate],
+              ['normal', 'long'][args.long_preamble]))
+
+    logging.basicConfig(level=logging.ERROR)
+    cflib.crtp.init_drivers(enable_debug_driver=False)
+    cf = Crazyflie(rw_cache='./cache')
+    with SyncCrazyflie(args.uri, cf=cf) as scf:
+        LPP_SHORT_UWB_MODE = 0x05
+        flags = args.low_bitrate * 1 + args.long_preamble * 2
+        packet = struct.pack("<BB", LPP_SHORT_UWB_MODE, flags)
+
+        print('Starting transmission. Terminate with CTRL+C.')
+        while True:
+            for id in range(args.anchor_id_lower, args.anchor_id_upper + 1):
+                print('Anchor %i' % id)
+                for _ in range(7):
+                    scf.cf.loc.send_short_lpp_packet(id, packet)
+                    time.sleep(0.2)
+                time.sleep(0.5)
+
+main()