multiple BLE connections

[Timothy]

I have a combined asset_tracker_v2 and central_main. I can connect perfectly and receive data from a single BLE advertiser. I need to be able to connect to 1-4 simultaneous advertisers. I see that there is a single default_conn that is used to manage the connection. I modified prj.conf as follows:

# Increase max connections (e.g., 4)

CONFIG_BT_MAX_CONN=4

CONFIG_BT_MAX_PAIRED=4

I also changed the following in my central_main

static struct bt_conn *default_conn;
static struct bt_conn *Gconn[CONFIG_BT_MAX_CONN];
static uint8_t conn_count = 0;

I am not sure how to replace default_conn (It is used in many places) with Gconn[conn_count] and what I need to do to connect to all of the advertisers and receive data from all of the advertisers. any help you can give would be greatly appreciated. This is in testing to go into customer evaluation and production. I have attached the central_main code. This is slightly modified from the original central_uart to be able to run with asset_tracker_v2.

Fullscreen central_uart_main.c Download

/*
 * Copyright (c) 2018 Nordic Semiconductor ASA
 *
 * SPDX-License-Identifier: LicenseRef-Nordic-5-Clause
 */

/** @file
 *  @brief Nordic UART Service Client sample
 */

#include <errno.h>
#include <zephyr/device.h>
#include <zephyr/devicetree.h>
#include <zephyr/kernel.h>
#include <zephyr/sys/byteorder.h>
#include <zephyr/sys/printk.h>

#include <zephyr/bluetooth/bluetooth.h>
#include <zephyr/bluetooth/conn.h>
#include <zephyr/bluetooth/gatt.h>
#include <zephyr/bluetooth/hci.h>
#include <zephyr/bluetooth/uuid.h>

#include <bluetooth/gatt_dm.h>
#include <bluetooth/scan.h>
#include <bluetooth/services/nus.h>
#include <bluetooth/services/nus_client.h>

#include <zephyr/settings/settings.h>

#include <zephyr/drivers/uart.h>

#include "jfet_files/jfet_common.h"
#include "mainpage.h"
#include <zephyr/drivers/watchdog.h>
#include <zephyr/logging/log.h>

/* merge */
#ifdef LTE_MERGE
// #include <zephyr/kernel.h>
// #include <stdio.h>
// #include <string.h>
// #include <stdlib.h>
// #include <app_event_manager.h>
// #include <modem/nrf_modem_lib.h>
// #include <zephyr/sys/reboot.h>
// #include <net/nrf_cloud.h>

/* Module name is used by the Application Event Manager macros in this file */
// #define MODULE main
// #include <caf/events/module_state_event.h>

// #include "modules_common.h"
// #include "events/app_module_event.h"
// #include "events/cloud_module_event.h"
// #include "events/data_module_event.h"
// #include "events/sensor_module_event.h"
// #include "events/util_module_event.h"
// #include "events/modem_module_event.h"

// #include <zephyr/logging/log.h>
// #include <zephyr/logging/log_ctrl.h>
// #include "jfet_files/jfet_common.h"
#endif

/* uncomment out if using thread */
#define USE_THREAD 1

#define LOG_MODULE_NAME central_uart
LOG_MODULE_REGISTER(LOG_MODULE_NAME, 4);

///* UART payload buffer element size. */
// #define UART_BUF_SIZE 20

#define KEY_PASSKEY_ACCEPT DK_BTN1_MSK
#define KEY_PASSKEY_REJECT DK_BTN2_MSK

#define NUS_WRITE_TIMEOUT K_MSEC(150)
#define UART_WAIT_FOR_BUF_DELAY K_MSEC(50)
#define UART_RX_TIMEOUT 50000 /* Wait for RX complete event time in microseconds. */

// static const struct device *uart = DEVICE_DT_GET(DT_CHOSEN(nordic_nus_uart));
// static struct k_work_delayable uart_work;
static struct k_work scan_work;

K_SEM_DEFINE(nus_write_sem, 0, 1);

// struct uart_data_t
//{
//	void *fifo_reserved;
//	uint8_t data[UART_BUF_SIZE];
//	uint16_t len;
// };

static K_FIFO_DEFINE(fifo_uart_tx_data);
K_FIFO_DEFINE(fifo_uart_rx_data);

static struct bt_conn *default_conn;
static struct bt_conn *Gconn[CONFIG_BT_MAX_CONN];
static uint8_t conn_count = 0;
static struct bt_nus_client nus_client;

static void ble_data_sent(struct bt_nus_client *nus, uint8_t err, const uint8_t *const data, uint16_t len) {
  ARG_UNUSED(nus);
  ARG_UNUSED(data);
  ARG_UNUSED(len);

  k_sem_give(&nus_write_sem);

  if (err) {
    LOG_WRN("ATT error code: 0x%02X", err);
  }
}

static uint8_t ble_data_received(struct bt_nus_client *nus, const uint8_t *data, uint16_t len) {
  ARG_UNUSED(nus);

  char bleData[60];
  //	int64_t time_ms = k_uptime_get();

  memset(bleData, 0, sizeof(bleData));
  strncpy(bleData, data, len);
  bleData[len] = '\0';
  // myPrintkW("ble_data_received() entered... %s\n", bleData);

  //	printk("ble_data_received() entered... len=%d\n", len);
  //	printk("ble_data_received() uptime %3.2f ... len=%d\n", time_ms / 1000.0, len);
  //	printk("ble_data_received() uptime %3.2f len=%d ", time_ms / 1000.0, len);
  //	myPrintkI("\"%s\"\r\n", bleData);
  parseBLEString(bleData);
  //	myPrintkI("BLEID: %s BLEHDCTemp: %s BLEHDCHum: %3.2f\r\n", BLEID, BLEHDCTemp, BLEHDCHum);
  ARG_UNUSED(nus);

#if 0
	int err;

	for (uint16_t pos = 0; pos != len;)
	{
		struct uart_data_t *tx = k_malloc(sizeof(*tx));

		if (!tx)
		{
			LOG_WRN("Not able to allocate UART send data buffer");
			return BT_GATT_ITER_CONTINUE;
		}

		/* Keep the last byte of TX buffer for potential LF char. */
		size_t tx_data_size = sizeof(tx->data) - 1;

		if ((len - pos) > tx_data_size)
		{
			tx->len = tx_data_size;
		}
		else
		{
			tx->len = (len - pos);
		}

		memcpy(tx->data, &data[pos], tx->len);

		pos += tx->len;

		/* Append the LF character when the CR character triggered
		 * transmission from the peer.
		 */
		if ((pos == len) && (data[len - 1] == '\r'))
		{
			tx->data[tx->len] = '\n';
			tx->len++;
		}

		err = uart_tx(uart, tx->data, tx->len, SYS_FOREVER_MS);
		if (err)
		{
			k_fifo_put(&fifo_uart_tx_data, tx);
		}
		k_free(tx);
	}
#endif

  return BT_GATT_ITER_CONTINUE;
}

#if 0
static void uart_cb(const struct device *dev, struct uart_event *evt, void *user_data) {
  ARG_UNUSED(dev);

  static size_t aborted_len;
  struct uart_data_t *buf;
  static uint8_t *aborted_buf;
  static bool disable_req;

  switch (evt->type) {
  case UART_TX_DONE:
    LOG_DBG("UART_TX_DONE");
    if ((evt->data.tx.len == 0) || (!evt->data.tx.buf)) {
      return;
    }

    if (aborted_buf) {
      buf = CONTAINER_OF(aborted_buf, struct uart_data_t, data[0]);
      aborted_buf = NULL;
      aborted_len = 0;
    } else {
      buf = CONTAINER_OF(evt->data.tx.buf, struct uart_data_t, data[0]);
    }

    k_free(buf);

    buf = k_fifo_get(&fifo_uart_tx_data, K_NO_WAIT);
    if (!buf) {
      return;
    }

    if (uart_tx(uart, buf->data, buf->len, SYS_FOREVER_MS)) {
      LOG_WRN("Failed to send data over UART");
    }

    break;

  case UART_RX_RDY:
    LOG_DBG("UART_RX_RDY");
    buf = CONTAINER_OF(evt->data.rx.buf, struct uart_data_t, data[0]);
    buf->len += evt->data.rx.len;

    if (disable_req) {
      return;
    }

    if ((evt->data.rx.buf[buf->len - 1] == '\n') || (evt->data.rx.buf[buf->len - 1] == '\r')) {
      disable_req = true;
      uart_rx_disable(uart);
    }

    break;

  case UART_RX_DISABLED:
    LOG_DBG("UART_RX_DISABLED");
    disable_req = false;

    buf = k_malloc(sizeof(*buf));
    if (buf) {
      buf->len = 0;
    } else {
      LOG_WRN("Not able to allocate UART receive buffer");
      k_work_reschedule(&uart_work, UART_WAIT_FOR_BUF_DELAY);
      return;
    }

    uart_rx_enable(uart, buf->data, sizeof(buf->data), UART_RX_TIMEOUT);

    break;

  case UART_RX_BUF_REQUEST:
    LOG_DBG("UART_RX_BUF_REQUEST");
    buf = k_malloc(sizeof(*buf));
    if (buf) {
      buf->len = 0;
      uart_rx_buf_rsp(uart, buf->data, sizeof(buf->data));
    } else {
      LOG_WRN("Not able to allocate UART receive buffer");
    }

    break;

  case UART_RX_BUF_RELEASED:
    LOG_DBG("UART_RX_BUF_RELEASED");
    printf("\r\n\nUART_RX_BUF_RELEASED\r\n");
    buf = CONTAINER_OF(evt->data.rx_buf.buf, struct uart_data_t, data[0]);

    if (buf->len > 0) {
      k_fifo_put(&fifo_uart_rx_data, buf);
    } else {
      k_free(buf);
    }

    break;

  case UART_TX_ABORTED:
    LOG_DBG("UART_TX_ABORTED");
    if (!aborted_buf) {
      aborted_buf = (uint8_t *)evt->data.tx.buf;
    }

    aborted_len += evt->data.tx.len;
    buf = CONTAINER_OF(aborted_buf, struct uart_data_t, data[0]);

    uart_tx(uart, &buf->data[aborted_len], buf->len - aborted_len, SYS_FOREVER_MS);

    break;

  default:
    break;
  }
}

static void uart_work_handler(struct k_work *item) {
  struct uart_data_t *buf;

  buf = k_malloc(sizeof(*buf));
  if (buf) {
    buf->len = 0;
  } else {
    LOG_WRN("Not able to allocate UART receive buffer");
    k_work_reschedule(&uart_work, UART_WAIT_FOR_BUF_DELAY);
    return;
  }

  uart_rx_enable(uart, buf->data, sizeof(buf->data), UART_RX_TIMEOUT);
}

static int uart_init(void) {
  int err;
  struct uart_data_t *rx;

  if (!device_is_ready(uart)) {
    LOG_ERR("UART device not ready");
    return -ENODEV;
  }

  rx = k_malloc(sizeof(*rx));
  if (rx) {
    rx->len = 0;
  } else {
    return -ENOMEM;
  }

  k_work_init_delayable(&uart_work, uart_work_handler);

  err = uart_callback_set(uart, uart_cb, NULL);
  if (err) {
    return err;
  }

  return uart_rx_enable(uart, rx->data, sizeof(rx->data), UART_RX_TIMEOUT);
}
#endif

static void discovery_complete(struct bt_gatt_dm *dm, void *context) {
  struct bt_nus_client *nus = context;
  LOG_INF("Service discovery completed");

  bt_gatt_dm_data_print(dm);

  bt_nus_handles_assign(dm, nus);
  bt_nus_subscribe_receive(nus);

  bt_gatt_dm_data_release(dm);
}

static void discovery_service_not_found(struct bt_conn *conn, void *context) { LOG_INF("Service not found"); }

static void discovery_error(struct bt_conn *conn, int err, void *context) { LOG_WRN("Error while discovering GATT database: (%d)", err); }

struct bt_gatt_dm_cb discovery_cb = {
    .completed = discovery_complete,
    .service_not_found = discovery_service_not_found,
    .error_found = discovery_error,
};

static void gatt_discover(struct bt_conn *conn) {
  int err;

  myPrintkS("gatt_discover() entered... count: %d\n", conn_count);
  if (conn != default_conn) {
    return;
  }
  myPrintkS("bt_gatt_dm_start() entered...\n");
  err = bt_gatt_dm_start(conn, BT_UUID_NUS_SERVICE, &discovery_cb, &nus_client);
  myPrintkS("bt_gatt_dm_start() returned...%d \n", err);
  if (err) {
    LOG_ERR("could not start the discovery procedure, error "
            "code: %d",
            err);
  }
}

static void exchange_func(struct bt_conn *conn, uint8_t err, struct bt_gatt_exchange_params *params) {
  if (!err) {
    LOG_INF("MTU exchange done");
  } else {
    LOG_WRN("MTU exchange failed (err %" PRIu8 ")", err);
  }
}

static void connected(struct bt_conn *conn, uint8_t conn_err) {
  char addr[BT_ADDR_LE_STR_LEN];
  int err;

  // myPrintkW("connected() entered... %s\n", addr);
  bt_addr_le_to_str(bt_conn_get_dst(conn), addr, sizeof(addr));
  //  myPrintkW("bt_addr_le_to_str() returned... %s\n", addr);
  if (conn_err) {
    LOG_INF("Failed to connect to %s, 0x%02x %s", addr, conn_err, bt_hci_err_to_str(conn_err));

    if (default_conn == conn) {
      bt_conn_unref(default_conn);
      default_conn = NULL;

      (void)k_work_submit(&scan_work);
    }

    return;
  }

  myPrintkI("BT Connected: %s count: %d\r\n", addr, conn_count);
  //  LOG_INF("Connected: %s", addr);

  static struct bt_gatt_exchange_params exchange_params;

  exchange_params.func = exchange_func;
  err = bt_gatt_exchange_mtu(conn, &exchange_params);
  if (err) {
    LOG_WRN("MTU exchange failed (err %d)", err);
  }

  err = bt_conn_set_security(conn, BT_SECURITY_L2);
  if (err) {
    LOG_WRN("Failed to set security: %d", err);

    gatt_discover(conn);
  }

  //  if (conn_count == CONFIG_BT_MAX_CONN) {
  myPrintkI("Maximum connections reached, stopping scan\r\n");
  err = bt_scan_stop();
  if (err) {
    LOG_ERR("Stop LE scan failed (err %d)", err);
  }
  //  }
}

static void disconnected(struct bt_conn *conn, uint8_t reason) {
  char addr[BT_ADDR_LE_STR_LEN];

  bt_addr_le_to_str(bt_conn_get_dst(conn), addr, sizeof(addr));

  LOG_INF("BT Disconnected: %s, reason 0x%02x %s", addr, reason, bt_hci_err_to_str(reason));

  if (default_conn != conn) {
    return;
  }

  bt_conn_unref(default_conn);
  default_conn = NULL;

  (void)k_work_submit(&scan_work);
}

static void security_changed(struct bt_conn *conn, bt_security_t level, enum bt_security_err err) {
  char addr[BT_ADDR_LE_STR_LEN];

  bt_addr_le_to_str(bt_conn_get_dst(conn), addr, sizeof(addr));

  if (!err) {
    LOG_INF("Security changed: %s level %u", addr, level);
  } else {
    LOG_WRN("Security failed: %s level %u err %d %s", addr, level, err, bt_security_err_to_str(err));
  }

  gatt_discover(conn);
}

BT_CONN_CB_DEFINE(conn_callbacks) = {.connected = connected, .disconnected = disconnected, .security_changed = security_changed};

static void scan_filter_match(struct bt_scan_device_info *device_info, struct bt_scan_filter_match *filter_match, bool connectable) {
  char addr[BT_ADDR_LE_STR_LEN];

  bt_addr_le_to_str(device_info->recv_info->addr, addr, sizeof(addr));

  LOG_INF("Filters matched. Address: %s connectable: %d", addr, connectable);
}

static void scan_connecting_error(struct bt_scan_device_info *device_info) { LOG_WRN("Connecting failed"); }

static void scan_connecting(struct bt_scan_device_info *device_info, struct bt_conn *conn) { default_conn = bt_conn_ref(conn); }
// static void scan_connecting(struct bt_scan_device_info * device_info, struct bt_conn * conn) { Gconn[conn_count++] = bt_conn_ref(conn); }

static int nus_client_init(void) {
  int err;
  struct bt_nus_client_init_param init = {.cb = {
                                              .received = ble_data_received,
                                              .sent = ble_data_sent,
                                          }};

  err = bt_nus_client_init(&nus_client, &init);
  if (err) {
    LOG_ERR("NUS Client initialization failed (err %d)", err);
    return err;
  }

  LOG_INF("NUS Client module initialized");
  return err;
}

BT_SCAN_CB_INIT(scan_cb, scan_filter_match, NULL, scan_connecting_error, scan_connecting);

static void try_add_address_filter(const struct bt_bond_info *info, void *user_data) {
  int err;
  char addr[BT_ADDR_LE_STR_LEN];
  uint8_t *filter_mode = user_data;

  bt_addr_le_to_str(&info->addr, addr, sizeof(addr));

  struct bt_conn *conn = bt_conn_lookup_addr_le(BT_ID_DEFAULT, &info->addr);

  if (conn) {
    bt_conn_unref(conn);
    return;
  }

  err = bt_scan_filter_add(BT_SCAN_FILTER_TYPE_ADDR, &info->addr);
  if (err) {
    LOG_ERR("Address filter cannot be added (err %d): %s", err, addr);
    return;
  }

  LOG_INF("Address filter added: %s", addr);
  *filter_mode |= BT_SCAN_ADDR_FILTER;
}

static int scan_start(void) {
  int err;
  uint8_t filter_mode = 0;

  err = bt_scan_stop();
  if (err) {
    LOG_ERR("Failed to stop scanning (err %d)", err);
    return err;
  }

  bt_scan_filter_remove_all();

  err = bt_scan_filter_add(BT_SCAN_FILTER_TYPE_UUID, BT_UUID_NUS_SERVICE);
  if (err) {
    LOG_ERR("UUID filter cannot be added (err %d", err);
    return err;
  }
  filter_mode |= BT_SCAN_UUID_FILTER;

  bt_foreach_bond(BT_ID_DEFAULT, try_add_address_filter, &filter_mode);

  err = bt_scan_filter_enable(filter_mode, false);
  if (err) {
    LOG_ERR("Filters cannot be turned on (err %d)", err);
    return err;
  }

  err = bt_scan_start(BT_SCAN_TYPE_SCAN_ACTIVE);
  if (err) {
    LOG_ERR("Scanning failed to start (err %d)", err);
    return err;
  }

  myPrintkI("Scan started\r\n");
  LOG_INF("Scan started");
  return 0;
}

static void scan_work_handler(struct k_work *item) {
  ARG_UNUSED(item);

  (void)scan_start();
}

static void scan_init(void) {
  struct bt_scan_init_param scan_init = {
      .connect_if_match = true,
  };

  bt_scan_init(&scan_init);
  bt_scan_cb_register(&scan_cb);

  k_work_init(&scan_work, scan_work_handler);
  LOG_INF("Scan module initialized");
}

static void auth_cancel(struct bt_conn *conn) {
  char addr[BT_ADDR_LE_STR_LEN];

  bt_addr_le_to_str(bt_conn_get_dst(conn), addr, sizeof(addr));

  LOG_INF("Pairing cancelled: %s", addr);
}

static void pairing_complete(struct bt_conn *conn, bool bonded) {
  char addr[BT_ADDR_LE_STR_LEN];

  bt_addr_le_to_str(bt_conn_get_dst(conn), addr, sizeof(addr));

  LOG_INF("Pairing completed: %s, bonded: %d", addr, bonded);
}

static void pairing_failed(struct bt_conn *conn, enum bt_security_err reason) {
  char addr[BT_ADDR_LE_STR_LEN];

  bt_addr_le_to_str(bt_conn_get_dst(conn), addr, sizeof(addr));

  LOG_WRN("Pairing failed conn: %s, reason %d %s", addr, reason, bt_security_err_to_str(reason));
}

static struct bt_conn_auth_cb conn_auth_callbacks = {
    .cancel = auth_cancel,
};

static struct bt_conn_auth_info_cb conn_auth_info_callbacks = {.pairing_complete = pairing_complete, .pairing_failed = pairing_failed};

#if 1
struct k_timer wdt_timer; //!< timer to reset WDT

/*!
 * \fn void wdt_handler(struct k_work *work)
 *
 * \brief work function WDT timer handler
 *
 *
 * \param work
 *
 */
void wdt_handler(struct k_work *work) {
  if (stopWDTFeed == 0) /* check if WDT feed disabled*/
  {
    //		myPrintkS("Feeding watchdog... %d\n",stopWDTFeed);
    wdt_feed(wdt, wdt_channel_id);
  }
}

K_WORK_DEFINE(wdt_work, wdt_handler);

/*!
 * \fn void wdt_timer_handler(struct k_timer *dummy)
 *
 * \brief timer CLI command handler
 *
 * timer handler just submits timer work function to the work queue
 *
 * \param dummy pointer to a timer sturcture
 *
 */
void wdt_timer_handler(struct k_timer *dummy) { k_work_submit(&wdt_work); }
#endif

#ifdef USE_THREAD
#define MY_STACK_SIZE 1024
#define MY_PRIORITY 7
const k_tid_t my_uart_thread_id;

void my_uart_thread_logic(void *arg1, void *arg2, void *arg3) {
  int err = 0;

  struct uart_data_t nus_data = {
      .len = 0,
  };
  myPrintkI("starting BLE thread\r\n");
  /* Your logic from main()'s loop goes here */
  while (1) {
    // e.g., k_fifo_get(&fifo_uart_rx_data, K_FOREVER);
    // ... process data ...

    /* IMPORTANT: If your loop doesn't have a blocking call like
       k_fifo_get(..., K_FOREVER), you MUST add a sleep to prevent
       starving other threads of the same priority. */
    //	for (;;)
    {
      /* Wait indefinitely for data to be sent over Bluetooth */
      struct uart_data_t *buf = k_fifo_get(&fifo_uart_rx_data, K_FOREVER);
      printf("\r\n\nUART seen %s\r\n\n", buf->data);

      int plen = MIN(sizeof(nus_data.data) - nus_data.len, buf->len);
      int loc = 0;

      while (plen > 0) {
        memcpy(&nus_data.data[nus_data.len], &buf->data[loc], plen);
        nus_data.len += plen;
        loc += plen;
        if (nus_data.len >= sizeof(nus_data.data) || (nus_data.data[nus_data.len - 1] == '\n') ||
            (nus_data.data[nus_data.len - 1] == '\r')) {
          if (nus_data.data[0] == ':') {
            memset(cliCommand, 0, strlen(cliCommand));
            memcpy(cliCommand, &nus_data.data[1], nus_data.len - 1);
            cliCommand[strlen(cliCommand) - 1] = '\0';
            printf("\r\nCLI Command: %s\r\n", cliCommand);
            executeCmd(cliCommand);
          } else {
            myPrintkI("\r\nBLE Send: %d %s\r\n\n", nus_data.len, nus_data.data);
            err = bt_nus_client_send(&nus_client, nus_data.data, nus_data.len);
            if (err) {
              LOG_WRN("Failed to send data over BLE connection"
                      "(err %d)",
                      err);
            }
            //						err = k_sem_take(&nus_write_sem, NUS_WRITE_TIMEOUT);
            //						if (err)
            //						{
            //							LOG_WRN("NUS send timeout");
            //						}
          }

          nus_data.len = 0;
        }

        plen = MIN(sizeof(nus_data.data), buf->len - loc);
      }

      //			k_free(buf);
    }
    k_msleep(10);
  }
}
#endif

#ifdef USE_THREAD
K_THREAD_DEFINE(my_uart_thread_id, MY_STACK_SIZE, my_uart_thread_logic, NULL, NULL, NULL, MY_PRIORITY, 0, 0);
#endif

int central_uart_main(void) {
  int err;
  k_msgq_init(&my_msgq1, my_msgq1_buffer, sizeof(struct data_item_type), 1);
  k_msgq_init(&my_msgq2, my_msgq2_buffer, sizeof(struct data_item_type), 1);
  k_msgq_init(&my_msgq3, my_msgq3_buffer, sizeof(struct data_battery_type), 1);

#if 1
  struct wdt_timeout_cfg wdt_config = {
      /* Reset SoC when watchdog timer expires. */
      .flags = WDT_FLAG_RESET_SOC,

      /* Expire watchdog after max window */
      .window.min = WDT_MIN_WINDOW,
      .window.max = WDT_MAX_WINDOW,
  };
#endif

  err = bt_conn_auth_cb_register(&conn_auth_callbacks);
  if (err) {
    //		LOG_ERR("Failed to register authorization callbacks.");
    myPrintkE("Failed to register authorization callbacks.\r\n");
    return 0;
  }
  myPrintkI("Passed register authorization callbacks.\r\n");

  err = bt_conn_auth_info_cb_register(&conn_auth_info_callbacks);
  if (err) {
    myPrintkE("Failed to register authorization info callbacks.\r\n");
    return 0;
  }
  myPrintkI("Passed register authorization info callbacks.\r\n");

  err = bt_enable(NULL);
  if (err) {
    myPrintkE("Bluetooth init failed (err %d)\r\n", err);
    //		LOG_ERR("Bluetooth init failed (err %d)", err);
    return 0;
  }
  myPrintkI("Bluetooth initialized\r\n");
  //	LOG_INF("Bluetooth initialized");

  if (IS_ENABLED(CONFIG_SETTINGS)) {
    settings_load();
  }

  //	err = uart_init();
  //	if (err != 0)
  //	{
  //		printk("\r\nuart_init failed (err %d)\r\n", err);
  //		return 0;
  //	}
  //		printk("\r\nuart_init passed (err %d)\r\n", err);

  err = nus_client_init();
  if (err != 0) {
    myPrintkE("nus_client_init failed (err %d)\r\n", err);
    return 0;
  }
  myPrintkI("nus_client_init passed (err %d)\r\n", err);

  scan_init();
  err = scan_start();
  if (err) {
    myPrintkE("scan_start failed (err %d)\r\n", err);
    return 0;
  }
  myPrintkI("scan_start passed (err %d)\r\n", err);

  //	printk("Starting Bluetooth Central UART (central_uart_tjm) sample\n");
  myPrintkI("Starting Bluetooth Central UART (central_uart_tjm)\r\n");

  /* set up watchdog timer */
#if 1
  if (!device_is_ready(wdt)) {
    myPrintkE("%s: Watchdog device not ready.\n", wdt->name);
    return 0;
  }
  myPrintkI("%s: Watchdog device ready.\n", wdt->name);

  wdt_channel_id = wdt_install_timeout(wdt, &wdt_config);

  switch (wdt_channel_id) {
  case -EBUSY:
    myPrintkE("Watchdog timeout can not be installed while watchdog has already been setup.\n");
    return 0;
  case -ENOTSUP:
    myPrintkE("Watchdog Callback support rejected, continuing anyway\n");
    return 0;
  case -EINVAL:
    myPrintkE("Watchdog install error: invalid configuration\n");
    return 0;
  default:
    if (wdt_channel_id < 0) {
      myPrintkE("Watchdog install error: %d\n", wdt_channel_id);
      return 0;
    }
    myPrintkI("Watchdog install passed\n");
  }

#if 0
	if (wdt_channel_id < 0)
	{
		myPrintkE("Watchdog install error\n");
		return 0;
	}
	myPrintkI("Watchdog install passed\n");

	if (wdt_channel_id == -ENOTSUP)
	{
		/* IWDG driver for STM32 doesn't support callback */
		myPrintkE("Watchdog Callback support rejected, continuing anyway\n");
		wdt_config.callback = NULL;
		wdt_channel_id = wdt_install_timeout(wdt, &wdt_config);
	}
	myPrintkI("Watchdog Callback support passed\n");
#endif

  err = wdt_setup(wdt, WDT_OPT);
  if (err < 0) {
    myPrintkE("Watchdog setup error\n");
    return 0;
  }
  myPrintkI("Watchdog setup passed. Start WDT feed timer\n");

#if 1
  k_timer_init(&wdt_timer, wdt_timer_handler, NULL);
  k_timer_start(&wdt_timer, K_MSEC(WDG_FEED_INTERVAL), K_MSEC(WDG_FEED_INTERVAL));
#else
  /* Feeding watchdog. */
  myPrintkS("Feeding watchdog %d times %d ms\n", WDT_FEED_TRIES, WDG_FEED_INTERVAL);
  for (int i = 0; i < WDT_FEED_TRIES; ++i) {
    myPrintkS("Feeding watchdog...\n");
    wdt_feed(wdt, wdt_channel_id);
    k_sleep(K_MSEC(WDG_FEED_INTERVAL));
  }

  /* Waiting for the SoC reset. */
  myPrintkS("Waiting for reset...\n");
  while (1) {
    k_yield();
  }
  return 0;
#endif

#endif /* end WDT timer */

#ifdef LTE_MERGE
  /* merge from dl_agri_new_nrf9160 */

  if (app_event_manager_init()) {
    /* Without the Application Event Manager, the application will not work
     * as intended. A reboot is required in an attempt to recover.
     */
    //		LOG_ERR("Application Event Manager could not be initialized, rebooting...");
    //		strcpy(flashParameters.sysRebootReason, "Event manager init failed");
    //		setFlashxParameters(runFlag ? false : true);
    //		k_sleep(K_SECONDS(5));
    //		sys_reboot(SYS_REBOOT_COLD);
  } else {
    //		module_set_state(MODULE_STATE_READY);
    //		SEND_EVENT(app, APP_EVT_START);

#if defined(CONFIG_NRF_MODEM_LIB)
//		modem_init();
#endif
  }

#endif /* end dl_agri)new_nrf9160 merge */

#ifndef USE_THREAD
  struct uart_data_t nus_data = {
      .len = 0,
  };

  for (;;) {
    /* Wait indefinitely for data to be sent over Bluetooth */
    struct uart_data_t *buf = k_fifo_get(&fifo_uart_rx_data, K_FOREVER);

    int plen = MIN(sizeof(nus_data.data) - nus_data.len, buf->len);
    int loc = 0;

    while (plen > 0) {
      memcpy(&nus_data.data[nus_data.len], &buf->data[loc], plen);
      nus_data.len += plen;
      loc += plen;
      if (nus_data.len >= sizeof(nus_data.data) || (nus_data.data[nus_data.len - 1] == '\n') || (nus_data.data[nus_data.len - 1] == '\r')) {
        if (nus_data.data[0] == ':') {
          memset(cliCommand, 0, strlen(cliCommand));
          memcpy(cliCommand, &nus_data.data[1], nus_data.len - 1);
          cliCommand[strlen(cliCommand) - 1] = '\0';
          //					printf("\r\nCLI Command: %s\r\n", cliCommand);
          executeCmd(cliCommand);
        } else {
          myPrintkI("\r\nBLE Send: %d %s\r\n\n", nus_data.len, nus_data.data);
          err = bt_nus_client_send(&nus_client, nus_data.data, nus_data.len);
        }
        if (err) {
          LOG_WRN("Failed to send data over BLE connection"
                  "(err %d)",
                  err);
        }

        err = k_sem_take(&nus_write_sem, NUS_WRITE_TIMEOUT);
        if (err) {
          LOG_WRN("NUS send timeout");
        }

        nus_data.len = 0;
      }

      plen = MIN(sizeof(nus_data.data), buf->len - loc);
    }

    k_free(buf);
  }
#endif
  return 0;
}

void parseBLEString(char *bleData) {
  myPrintkS("parseBLEString BLE data seen %s\r\n", bleData);
  uint8_t i;
  int len = strlen(bleData);
  struct data_item_type tempBLEData;
  struct data_item_type temp2BLEData;
  struct data_battery_type tempBatteryData;

  /* parse all parameters on the command line into cmdLineparameter array */
  numBLEParameters = 0; /* no parameters seen yet */

  BLEParameter[numBLEParameters++] = &bleData[0];
  for (i = 0; i < len; i++) {
    //            printf("%d 0x%02X %c\r\n",i,bleData[i],bleData[i]);
    if ((bleData[i] == ' ') || (bleData[i] == '\t')) {
      bleData[i] = '\0';
      BLEParameter[numBLEParameters++] = &bleData[i + 1];
    }
  }
  bleData[i] = '\0';

  /* wait for HSC temperature/humidity data */
  if (strcmp(BLEParameter[2], "t1") == 0) {
    strcpy(tempBLEData.BLEId, BLEParameter[1]);
    tempBLEData.BLETemp = atof(BLEParameter[3]);
    tempBLEData.BLEHum = atof(BLEParameter[5]);
    memcpy(my_msgq1_buffer, &tempBLEData, sizeof(struct data_item_type));
    myPrintkS("BLEId: %s BLEHDC2080Temp: %3.2f BLEHDC2080Hum: %3.2f\r\n", tempBLEData.BLEId, tempBLEData.BLETemp, tempBLEData.BLEHum);
    /* send data to consumers */
    if (k_msgq_put(&my_msgq1, my_msgq1_buffer, K_NO_WAIT) != 0) {
      myPrintkW("Message queue 1 is full, purging old data\r\n");
      /* message queue is full: purge old data & try again */
      k_msgq_purge(&my_msgq1);
      k_msgq_put(&my_msgq1, my_msgq1_buffer, K_NO_WAIT);
    }
  }

  /* wait for BME688 temperature/humidity data */
  if (strcmp(BLEParameter[2], "t2") == 0) {
    strcpy(temp2BLEData.BLEId, BLEParameter[1]);
    temp2BLEData.BLETemp = atof(BLEParameter[3]);
    temp2BLEData.BLEHum = atof(BLEParameter[5]);
    memcpy(my_msgq2_buffer, &temp2BLEData, sizeof(struct data_item_type));
    myPrintkS("BLEId: %s BLEBME688Temp2: %3.2f BLEBME688Hum2: %3.2f\r\n", temp2BLEData.BLEId, temp2BLEData.BLETemp, temp2BLEData.BLEHum);
    /* send data to consumers */
    if (k_msgq_put(&my_msgq2, my_msgq2_buffer, K_NO_WAIT) != 0) {
      myPrintkW("Message queue 2 is full, purging old data\r\n");
      /* message queue is full: purge old data & try again */
      k_msgq_purge(&my_msgq2);
      k_msgq_put(&my_msgq2, my_msgq2_buffer, K_NO_WAIT);
    }
  }

  /* wait for Battery data */
  if (strcmp(BLEParameter[2], "bat") == 0) {
    strcpy(tempBatteryData.BLEId, BLEParameter[1]);
    tempBatteryData.BLEBattery = atoi(BLEParameter[3]);
    memcpy(my_msgq3_buffer, &tempBatteryData, sizeof(struct data_battery_type));
    myPrintkS("BLEId: %s Battery: %d \r\n", tempBatteryData.BLEId, tempBatteryData.BLEBattery);
    /* send data to consumers */
    if (k_msgq_put(&my_msgq3, my_msgq3_buffer, K_NO_WAIT) != 0) {
      myPrintkW("Message queue 3 is full, purging old data\r\n");
      /* message queue is full: purge old data & try again */
      k_msgq_purge(&my_msgq3);
      k_msgq_put(&my_msgq3, my_msgq3_buffer, K_NO_WAIT);
    }
  }
}

/*
 * Black \033[0;30m.
 * Red \033[0;31m.
 * Green \033[0;32m.
 * Yellow \033[0;33m.
 * Blue \033[0;34m.
 * Purple \033[0;35m.
 * Cyan \033[0;36m.
 * White \033[0;37m.
 */

/* yellow (Warning)*/
#if 0
void myPrintkW(char *str) {
  printk("%s", "\33[1;33m");
  printk("%s", str);
  // printk("%s", myStr);
  printk("%s", "\33[0m");
  return;
}
#endif

int myPrintkW(char *restrict fmt, ...) {
  int n = 0;
  va_list ap;

  printk("%s", "\33[1;33m");
  va_start(ap, fmt);
  //  n = vprintf(fmt, ap);
  vprintk(fmt, ap);
  va_end(ap);
  printk("%s", "\33[0m");
  // printf("n=%d\n", n);
  return n;
}

/* Green (Information)*/
#if 0
void myPrintkI(char *str) {
  printk("%s", "\33[1;32m");
  printk("%s", str);
  // printk("%s", myStr);
  printk("%s", "\33[0m");
  return;
}
#endif

int myPrintkI(char *restrict fmt, ...) {
  int n = 0;
  va_list ap;

  printk("%s", "\33[1;32m");
  va_start(ap, fmt);
  //  n = vprintf(fmt, ap);
  vprintk(fmt, ap);
  va_end(ap);
  printk("%s", "\33[0m");
  // printf("n=%d\n", n);
  return n;
}

/* CYAN (Status)*/
#if 0
void myPrintkS(char *str) {
  printk("%s", "\33[1;36m");
  printk("%s", str);
  // printk("%s", myStr);
  printk("%s", "\33[0m");
}
#endif

int myPrintkS(char *restrict fmt, ...) {
  int n = 0;
  va_list ap;

  printk("%s", "\33[1;36m");
  va_start(ap, fmt);
  //  n = vprintf(fmt, ap);
  vprintk(fmt, ap);
  va_end(ap);
  printk("%s", "\33[0m");
  // printf("n=%d\n", n);
  return n;
}

/* Red (Error)*/
#if 0
void myPrintkE(char *str) {
  printk("%s", "\33[1;31m");
  printk("%s", str);
  // printk("%s", myStr);
  printk("%s", "\33[0m");
  return;
}
#endif

int myPrintkE(char *restrict fmt, ...) {
  int n = 0;
  va_list ap;

  printk("%s", "\33[1;31m");
  va_start(ap, fmt);
  //  n = vprintf(fmt, ap);
  vprintk(fmt, ap);
  va_end(ap);
  printk("%s", "\33[0m");
  // printf("n=%d\n", n);
  return n;
}

[Sigurd]

Hi!

This is for an older NCS version, but it show-cases how to a multi NUS central. Hope that this can be of help.

Enter the Multi-NUS: A Simple Wireless UART Network

https://github.com/NordicMatt/multi-NUS/tree/master

[Timothy]

Sigurd

thank you for the quick response. We have a lot of time invested in the simple central_uart example. We have merged central_uart with asset_tracker_v2 and are planning on testing at customer site immediately. The customer demo will show one BLE advertiser connecting to our application (asset_tracker_v2 + central_uart).  They require N advertisers connecting simultaneously to our application. The example you gave is not the same as central_uart. I do not see how to modify my central_uart example that I attached to work with multiple advertisers.

can you please take a look at the attached central_uart_main.c and help me get that to work with multiple advertisers? It will be a long process to abandon central_main and move to a new approach. I really appreciate any help you can give.

[Timothy]

Sigrid

we have invested a lot of time and effort to get the single BLE connection working with the modified central_uart. This is working with the asset_tracker_v2 that we have used for years and have several hundred installed in the field. I do not want to start over with a different approach. I have made some changes and I have some things working. I will attach my latest central_uart_main.

1. I can always connect to both BLE advertisers but MOST of the time if fails to discover one of the two advertisers. The case shown below where it does connect and discover both advertisers and receives data from both as shown in green. the device iDs are 3EF2E0E9 and 5956DB21. I have replaced default_conn and conn with arrays. the maximum connections needed is 4. I have attached the latest file. I think I am close because it does work sometimes but fails with the -120 error  "socket already connected" most of the time.

any help you can give would be greatly appreciated.

scan_connecting called count: 0 count_disc: 0
BT Connected: CF:2D:2C:F8:5D:E3 (random) count: 1
Maximum connections not reached, continuing scan
[[00:00:01.233,734] <inf> central_uart: MTU exchange done
[00:00:01.433,349] <err> bt_smp: pairing failed (peer reason 0x3)
[00:00:01.433,929] <wrn> central_uart: Security failed: CF:2D:2C:F8:5D:E3 (random) level 1 err 4
gatt_discover() entered... count: 1 count_disc: 0
bt_gatt_dm_start() called...
Gconn[conn_count_disc]: 0x20020d90
start the discovery procedure returned...0
[00:00:01.434,509] <wrn> central_uart: Pairing failed conn: CF:2D:2C:F8:5D:E3 (random), reason 4
Service discovery completed
parseBLEString BLE data seen 8 3EF2E0E9 r2 82643128
parseBLEString BLE data seen 9 3EF2E0E9 bat 32
BLEId: 3EF2E0E9 Battery: 32
parseBLEString BLE data seen 10 3EF2E0E9 t1 25.95 h1 48.14
BLEId: 3EF2E0E9 BLEHDC2080Temp: 25.95 BLEHDC2080Hum: 48.14
[00:00:32.154,907] <inf> central_uart: Filters matched. Address: DD:BD:A9:D6:F1:51 (random) connectable: 0
scan_connecting called count: 1 count_disc: 1
BT Connected: DD:BD:A9:D6:F1:51 (random) count: 2
Maximum connections not reached, continuing scan
[00:00:32.391,876] <inf> central_uart: MTU exchange done
[00:00:32.491,790] <err> bt_smp: pairing failed (peer reason 0x3)
[00:00:32.492,370] <wrn> central_uart: Security failed: DD:BD:A9:D6:F1:51 (random) level 1 err 4
gatt_discover() entered... count: 2 count_disc: 1
bt_gatt_dm_start() called...
Gconn[conn_count_disc]: 0x20020e58
start the discovery procedure returned...0
[00:00:32.492,980] <wrn> central_uart: Pairing failed conn: DD:BD:A9:D6:F1:51 (random), reason 4
Service discovery completed
parseBLEString BLE data seen 11 3EF2E0E9 t2 65.40 h 0.00
BLEId: 3EF2E0E9 BLEBME688Temp2: 65.40 BLEBME688Hum2: 0.00
parseBLEString BLE data seen 2 5956DB21 p2 99005
parseBLEString BLE data seen 12 3EF2E0E9 p2 -17350
parseBLEString BLE data seen 3 5956DB21 r2 62000

2. the successful case happens maybe 10% of the time. the normal case is shown below. the difference s that I get the -120 error. I am connected to both advertisers as shown but I only discovered one (random selection) and receiving data from only one.

Scan started
[00:00:00.995,758] <inf> central_uart: Scan started
scan_start passed (err 0)
Starting Bluetooth Central UART (central_uart_tjm)
watchdog@18000: Watchdog device ready.
Watchdog install passed
Watchdog setup passed. Start WDT feed timer
[00:00:00.996,520] <inf> app_event_manager: APP_EVT_START
[00:00:01.007,629] <inf> central_uart: Filters matched. Address: DD:BD:A9:D6:F1:51 (random) connectable: 0
scan_connecting called count: 0 count_disc: 0
BT Connected: DD:BD:A9:D6:F1:51 (random) count: 1
Maximum connections not reached, continuing scan
[00:00:01.192,352] <inf> central_uart: Filters matched. Address: CF:2D:2C:F8:5D:E3 (random) connectable: 0
scan_connecting called count: 1 count_disc: 0
[00:00:01.214,569] <inf> central_uart: MTU exchange done
[00:00:01.261,779] <inf> app_event_manager: DATA_EVT_CONFIG_INIT
BT Connected: CF:2D:2C:F8:5D:E3 (random) count: 2
Maximum connections not reached, continuing scan
[00:00:01.421,813] <inf> central_uart: MTU exchange done
[00:00:01.464,233] <err> bt_smp: pairing failed (peer reason 0x3)
[00:00:01.464,813] <wrn> central_uart: Security failed: DD:BD:A9:D6:F1:51 (random) level 1 err 4
gatt_discover() entered... count: 2 count_disc: 0
bt_gatt_dm_start() called...
Gconn[conn_count_disc]: 0x20020d90
start the discovery procedure returned...0
[00:00:01.465,393] <wrn> central_uart: Pairing failed conn: DD:BD:A9:D6:F1:51 (random), reason 4
[00:00:01.521,728] <err> bt_smp: pairing failed (peer reason 0x3)
[00:00:01.522,308] <wrn> central_uart: Security failed: CF:2D:2C:F8:5D:E3 (random) level 1 err 4
gatt_discover() entered... count: 2 count_disc: 1
bt_gatt_dm_start() called...
Gconn[conn_count_disc]: 0x20020e58
start the discovery procedure returned...-120
could not start the discovery procedure, error code: -120
[00:00:01.584,411] <inf> app_event_manager: MODEM_EVT_LTE_CONNECTING
[00:00:01.622,955] <wrn> central_uart: Pairing failed conn: CF:2D:2C:F8:5D:E3 (random), reason 4
Service discovery completed
[00:00:02.939,941] <wrn> modem_module: The modem has detected a reset loop. LTE network attach is now restricted for the next 30 minutes. Power-cycle the device to circumvent this restriction.
[00:00:02.940,338] <inf> app_event_manager: MODEM_EVT_LTE_CELL_UPDATE
parseBLEString BLE data seen 170 5956DB21 t1 25.08 h1 47.68
BLEId: 5956DB21 BLEHDC2080Temp: 25.08 BLEHDC2080Hum: 47.68
parseBLEString BLE data seen 171 5956DB21 t2 24.48 h 37.02
BLEId: 5956DB21 BLEBME688Temp2: 24.48 BLEBME688Hum2: 37.02
parseBLEString BLE data seen 172 5956DB21 p2 99013

Fullscreen 0878.central_uart_main.c Download

/*
 * Copyright (c) 2018 Nordic Semiconductor ASA
 *
 * SPDX-License-Identifier: LicenseRef-Nordic-5-Clause
 */

/** @file
 *  @brief Nordic UART Service Client sample
 */

#include <errno.h>
#include <zephyr/device.h>
#include <zephyr/devicetree.h>
#include <zephyr/kernel.h>
#include <zephyr/sys/byteorder.h>
#include <zephyr/sys/printk.h>

#include <zephyr/bluetooth/bluetooth.h>
#include <zephyr/bluetooth/conn.h>
#include <zephyr/bluetooth/gatt.h>
#include <zephyr/bluetooth/hci.h>
#include <zephyr/bluetooth/uuid.h>

#include <bluetooth/gatt_dm.h>
#include <bluetooth/scan.h>
#include <bluetooth/services/nus.h>
#include <bluetooth/services/nus_client.h>

#include <zephyr/settings/settings.h>

#include <zephyr/drivers/uart.h>

#include "jfet_files/jfet_common.h"
#include "mainpage.h"
#include <zephyr/drivers/watchdog.h>
#include <zephyr/logging/log.h>

/* merge */
#ifdef LTE_MERGE
// #include <zephyr/kernel.h>
// #include <stdio.h>
// #include <string.h>
// #include <stdlib.h>
// #include <app_event_manager.h>
// #include <modem/nrf_modem_lib.h>
// #include <zephyr/sys/reboot.h>
// #include <net/nrf_cloud.h>

/* Module name is used by the Application Event Manager macros in this file */
// #define MODULE main
// #include <caf/events/module_state_event.h>

// #include "modules_common.h"
// #include "events/app_module_event.h"
// #include "events/cloud_module_event.h"
// #include "events/data_module_event.h"
// #include "events/sensor_module_event.h"
// #include "events/util_module_event.h"
// #include "events/modem_module_event.h"

// #include <zephyr/logging/log.h>
// #include <zephyr/logging/log_ctrl.h>
// #include "jfet_files/jfet_common.h"
#endif

/* uncomment out if using thread */
#define USE_THREAD 1

#define LOG_MODULE_NAME central_uart
LOG_MODULE_REGISTER(LOG_MODULE_NAME, 4);

///* UART payload buffer element size. */
// #define UART_BUF_SIZE 20

#define KEY_PASSKEY_ACCEPT DK_BTN1_MSK
#define KEY_PASSKEY_REJECT DK_BTN2_MSK

#define NUS_WRITE_TIMEOUT K_MSEC(150)
#define UART_WAIT_FOR_BUF_DELAY K_MSEC(50)
#define UART_RX_TIMEOUT 50000 /* Wait for RX complete event time in microseconds. */

// static const struct device *uart = DEVICE_DT_GET(DT_CHOSEN(nordic_nus_uart));
// static struct k_work_delayable uart_work;
static struct k_work scan_work;

K_SEM_DEFINE(nus_write_sem, 0, 1);

// struct uart_data_t
//{
//	void *fifo_reserved;
//	uint8_t data[UART_BUF_SIZE];
//	uint16_t len;
// };

static K_FIFO_DEFINE(fifo_uart_tx_data);
K_FIFO_DEFINE(fifo_uart_rx_data);

static struct bt_conn *default_conn;
static struct bt_conn *Gconn[CONFIG_BT_MAX_CONN] = {0};
static uint8_t conn_count = 0;
static uint8_t conn_count_disc = 0;
static struct bt_nus_client nus_client;
static struct bt_nus_client Gnus_client[CONFIG_BT_MAX_CONN] = {0};

static void ble_data_sent(struct bt_nus_client *nus, uint8_t err, const uint8_t *const data, uint16_t len) {
  ARG_UNUSED(nus);
  ARG_UNUSED(data);
  ARG_UNUSED(len);

  k_sem_give(&nus_write_sem);

  if (err) {
    LOG_WRN("ATT error code: 0x%02X", err);
  }
}

static uint8_t ble_data_received(struct bt_nus_client *nus, const uint8_t *data, uint16_t len) {
  ARG_UNUSED(nus);

  char bleData[60];
  //	int64_t time_ms = k_uptime_get();

  memset(bleData, 0, sizeof(bleData));
  strncpy(bleData, data, len);
  bleData[len] = '\0';
  // myPrintkW("ble_data_received() entered... %s\n", bleData);

  //	printk("ble_data_received() entered... len=%d\n", len);
  //	printk("ble_data_received() uptime %3.2f ... len=%d\n", time_ms / 1000.0, len);
  //	printk("ble_data_received() uptime %3.2f len=%d ", time_ms / 1000.0, len);
  //	myPrintkI("\"%s\"\r\n", bleData);
  parseBLEString(bleData);
  //	myPrintkI("BLEID: %s BLEHDCTemp: %s BLEHDCHum: %3.2f\r\n", BLEID, BLEHDCTemp, BLEHDCHum);
  ARG_UNUSED(nus);

#if 0
	int err;

	for (uint16_t pos = 0; pos != len;)
	{
		struct uart_data_t *tx = k_malloc(sizeof(*tx));

		if (!tx)
		{
			LOG_WRN("Not able to allocate UART send data buffer");
			return BT_GATT_ITER_CONTINUE;
		}

		/* Keep the last byte of TX buffer for potential LF char. */
		size_t tx_data_size = sizeof(tx->data) - 1;

		if ((len - pos) > tx_data_size)
		{
			tx->len = tx_data_size;
		}
		else
		{
			tx->len = (len - pos);
		}

		memcpy(tx->data, &data[pos], tx->len);

		pos += tx->len;

		/* Append the LF character when the CR character triggered
		 * transmission from the peer.
		 */
		if ((pos == len) && (data[len - 1] == '\r'))
		{
			tx->data[tx->len] = '\n';
			tx->len++;
		}

		err = uart_tx(uart, tx->data, tx->len, SYS_FOREVER_MS);
		if (err)
		{
			k_fifo_put(&fifo_uart_tx_data, tx);
		}
		k_free(tx);
	}
#endif

  return BT_GATT_ITER_CONTINUE;
}

#if 0
static void uart_cb(const struct device *dev, struct uart_event *evt, void *user_data) {
  ARG_UNUSED(dev);

  static size_t aborted_len;
  struct uart_data_t *buf;
  static uint8_t *aborted_buf;
  static bool disable_req;

  switch (evt->type) {
  case UART_TX_DONE:
    LOG_DBG("UART_TX_DONE");
    if ((evt->data.tx.len == 0) || (!evt->data.tx.buf)) {
      return;
    }

    if (aborted_buf) {
      buf = CONTAINER_OF(aborted_buf, struct uart_data_t, data[0]);
      aborted_buf = NULL;
      aborted_len = 0;
    } else {
      buf = CONTAINER_OF(evt->data.tx.buf, struct uart_data_t, data[0]);
    }

    k_free(buf);

    buf = k_fifo_get(&fifo_uart_tx_data, K_NO_WAIT);
    if (!buf) {
      return;
    }

    if (uart_tx(uart, buf->data, buf->len, SYS_FOREVER_MS)) {
      LOG_WRN("Failed to send data over UART");
    }

    break;

  case UART_RX_RDY:
    LOG_DBG("UART_RX_RDY");
    buf = CONTAINER_OF(evt->data.rx.buf, struct uart_data_t, data[0]);
    buf->len += evt->data.rx.len;

    if (disable_req) {
      return;
    }

    if ((evt->data.rx.buf[buf->len - 1] == '\n') || (evt->data.rx.buf[buf->len - 1] == '\r')) {
      disable_req = true;
      uart_rx_disable(uart);
    }

    break;

  case UART_RX_DISABLED:
    LOG_DBG("UART_RX_DISABLED");
    disable_req = false;

    buf = k_malloc(sizeof(*buf));
    if (buf) {
      buf->len = 0;
    } else {
      LOG_WRN("Not able to allocate UART receive buffer");
      k_work_reschedule(&uart_work, UART_WAIT_FOR_BUF_DELAY);
      return;
    }

    uart_rx_enable(uart, buf->data, sizeof(buf->data), UART_RX_TIMEOUT);

    break;

  case UART_RX_BUF_REQUEST:
    LOG_DBG("UART_RX_BUF_REQUEST");
    buf = k_malloc(sizeof(*buf));
    if (buf) {
      buf->len = 0;
      uart_rx_buf_rsp(uart, buf->data, sizeof(buf->data));
    } else {
      LOG_WRN("Not able to allocate UART receive buffer");
    }

    break;

  case UART_RX_BUF_RELEASED:
    LOG_DBG("UART_RX_BUF_RELEASED");
    printf("\r\n\nUART_RX_BUF_RELEASED\r\n");
    buf = CONTAINER_OF(evt->data.rx_buf.buf, struct uart_data_t, data[0]);

    if (buf->len > 0) {
      k_fifo_put(&fifo_uart_rx_data, buf);
    } else {
      k_free(buf);
    }

    break;

  case UART_TX_ABORTED:
    LOG_DBG("UART_TX_ABORTED");
    if (!aborted_buf) {
      aborted_buf = (uint8_t *)evt->data.tx.buf;
    }

    aborted_len += evt->data.tx.len;
    buf = CONTAINER_OF(aborted_buf, struct uart_data_t, data[0]);

    uart_tx(uart, &buf->data[aborted_len], buf->len - aborted_len, SYS_FOREVER_MS);

    break;

  default:
    break;
  }
}

static void uart_work_handler(struct k_work *item) {
  struct uart_data_t *buf;

  buf = k_malloc(sizeof(*buf));
  if (buf) {
    buf->len = 0;
  } else {
    LOG_WRN("Not able to allocate UART receive buffer");
    k_work_reschedule(&uart_work, UART_WAIT_FOR_BUF_DELAY);
    return;
  }

  uart_rx_enable(uart, buf->data, sizeof(buf->data), UART_RX_TIMEOUT);
}

static int uart_init(void) {
  int err;
  struct uart_data_t *rx;

  if (!device_is_ready(uart)) {
    LOG_ERR("UART device not ready");
    return -ENODEV;
  }

  rx = k_malloc(sizeof(*rx));
  if (rx) {
    rx->len = 0;
  } else {
    return -ENOMEM;
  }

  k_work_init_delayable(&uart_work, uart_work_handler);

  err = uart_callback_set(uart, uart_cb, NULL);
  if (err) {
    return err;
  }

  return uart_rx_enable(uart, rx->data, sizeof(rx->data), UART_RX_TIMEOUT);
}
#endif

static void discovery_complete(struct bt_gatt_dm *dm, void *context) {
  struct bt_nus_client *nus = context;
  myPrintkI("Service discovery completed\r\n");

  bt_gatt_dm_data_print(dm);

  bt_nus_handles_assign(dm, nus);
  bt_nus_subscribe_receive(nus);

  bt_gatt_dm_data_release(dm);
}

static void discovery_service_not_found(struct bt_conn *conn, void *context) { LOG_INF("Service not found"); }

static void discovery_error(struct bt_conn *conn, int err, void *context) { LOG_WRN("Error while discovering GATT database: (%d)", err); }

struct bt_gatt_dm_cb discovery_cb[CONFIG_BT_MAX_CONN] = {0};
// struct bt_gatt_dm_cb discovery_cb = {
//     .completed = discovery_complete,
//     .service_not_found = discovery_service_not_found,
//     .error_found = discovery_error,
// };

static void gatt_discover(struct bt_conn *conn) {
  int err;
  myPrintkS("gatt_discover() entered... count: %d  count_disc: %d\n", conn_count, conn_count_disc);
  //  if (conn != default_conn) {
  //    return;
  //  }
  myPrintkS("bt_gatt_dm_start() called...\n");
  myPrintkS("Gconn[conn_count_disc]: %p\n", Gconn[conn_count_disc]);
  //  err = bt_gatt_dm_start(conn, BT_UUID_NUS_SERVICE, &discovery_cb[conn_count_disc++], &nus_client);
  // err = bt_gatt_dm_start(Gconn[conn_count_disc], BT_UUID_NUS_SERVICE, &discovery_cb[conn_count_disc], &nus_client);
  err = bt_gatt_dm_start(Gconn[conn_count_disc], BT_UUID_NUS_SERVICE, &discovery_cb[conn_count_disc], &Gnus_client[conn_count_disc]);
  myPrintkS("start the discovery procedure returned...%d \n", err);
  if (err) {
    myPrintkE("could not start the discovery procedure, error code: %d \r\n", err);
    k_sleep(K_MSEC(100));
  }
  conn_count_disc++;
}

static void exchange_func(struct bt_conn *conn, uint8_t err, struct bt_gatt_exchange_params *params) {
  if (!err) {
    LOG_INF("MTU exchange done");
  } else {
    LOG_WRN("MTU exchange failed (err %" PRIu8 ")", err);
  }
}

static void connected(struct bt_conn *conn, uint8_t conn_err) {
#if 0
  char addr[BT_ADDR_LE_STR_LEN];
  int err;

  if (conn_err) {
    // handle error
    myPrintkE("Failed to connect to %s, 0x%02x %s\r\n", addr, conn_err, bt_hci_err_to_str(conn_err));
    return;
  }
  if (conn_count < CONFIG_BT_MAX_CONN) {
    Gconn[conn_count] = bt_conn_ref(conn);
    conn_count++;
  }
  // Resume scanning if not at max
  if (conn_count < CONFIG_BT_MAX_CONN) {
    // restart scan
  }
#else
  char addr[BT_ADDR_LE_STR_LEN];
  int err;

  if (conn_count < CONFIG_BT_MAX_CONN) {
    Gconn[conn_count] = bt_conn_ref(conn);
    conn_count++;
  }

  bt_addr_le_to_str(bt_conn_get_dst(conn), addr, sizeof(addr));
  if (conn_err) {
    myPrintkE("Failed to connect to %s, 0x%02x %s\r\n", addr, conn_err, bt_hci_err_to_str(conn_err));

    if (default_conn == conn) {
      bt_conn_unref(default_conn);
      default_conn = NULL;

      (void)k_work_submit(&scan_work);
    }

    return;
  }

  myPrintkI("BT Connected: %s count: %d\r\n", addr, conn_count);
  //  LOG_INF("Connected: %s", addr);

  static struct bt_gatt_exchange_params exchange_params;

  exchange_params.func = exchange_func;
  err = bt_gatt_exchange_mtu(conn, &exchange_params);
  if (err) {
    LOG_WRN("MTU exchange failed (err %d)", err);
  }

  err = bt_conn_set_security(conn, BT_SECURITY_L2);
  if (err) {
    LOG_WRN("Failed to set security: %d", err);

    gatt_discover(conn);
  }

  if (conn_count == CONFIG_BT_MAX_CONN) {
    myPrintkI("Maximum connections reached, stopping scan\r\n");
    err = bt_scan_stop();
    if (err) {
      LOG_ERR("Stop LE scan failed (err %d)", err);
    }
  } else {
    myPrintkI("Maximum connections not reached, continuing scan\r\n");

    err = bt_scan_start(BT_SCAN_TYPE_SCAN_ACTIVE);
    if (err) {
      LOG_ERR("Scanning failed to start (err %d)", err);
      return;
    }
  }
#endif
}

static void disconnected(struct bt_conn *conn, uint8_t reason) {
#if 0
  for (int i = 0; i < CONFIG_BT_MAX_CONN; i++) {
    if (Gconn[i] == conn) {
      bt_conn_unref(Gconn[i]);
      Gconn[i] = NULL;
      conn_count--;
      break;
    }
  }
  // Restart scanning
#else
  char addr[BT_ADDR_LE_STR_LEN];

  bt_addr_le_to_str(bt_conn_get_dst(conn), addr, sizeof(addr));

  LOG_INF("BT Disconnected: %s, reason 0x%02x %s", addr, reason, bt_hci_err_to_str(reason));

  if (default_conn != conn) {
    return;
  }

  bt_conn_unref(default_conn);
  default_conn = NULL;

  (void)k_work_submit(&scan_work);
#endif
}

static void security_changed(struct bt_conn *conn, bt_security_t level, enum bt_security_err err) {
  char addr[BT_ADDR_LE_STR_LEN];

  bt_addr_le_to_str(bt_conn_get_dst(conn), addr, sizeof(addr));

  if (!err) {
    LOG_INF("Security changed: %s level %u", addr, level);
  } else {
    LOG_WRN("Security failed: %s level %u err %d %s", addr, level, err, bt_security_err_to_str(err));
  }

  gatt_discover(conn);
}

BT_CONN_CB_DEFINE(conn_callbacks) = {.connected = connected, .disconnected = disconnected, .security_changed = security_changed};

static void scan_filter_match(struct bt_scan_device_info *device_info, struct bt_scan_filter_match *filter_match, bool connectable) {
  char addr[BT_ADDR_LE_STR_LEN];

  bt_addr_le_to_str(device_info->recv_info->addr, addr, sizeof(addr));

  LOG_INF("Filters matched. Address: %s connectable: %d", addr, connectable);
}

static void scan_connecting_error(struct bt_scan_device_info *device_info) { LOG_WRN("Connecting failed"); }

// static void scan_connecting(struct bt_scan_device_info *device_info, struct bt_conn *conn) { default_conn = bt_conn_ref(conn); }
static void scan_connecting(struct bt_scan_device_info *device_info, struct bt_conn *conn) {
  myPrintkI("scan_connecting called count: %d count_disc: %d\r\n", conn_count, conn_count_disc);
  Gconn[conn_count] = bt_conn_ref(conn);
}

static int nus_client_init(void) {
  int err;
  struct bt_nus_client_init_param init = {.cb = {
                                              .received = ble_data_received,
                                              .sent = ble_data_sent,
                                          }};
#if 1
  for (int i = 0; i < CONFIG_BT_MAX_CONN; i++) {

    err = bt_nus_client_init(&Gnus_client[i], &init);
    if (err) {
      myPrintkE("NUS Client initialization failed (err %d)\r\n", err);
      return err;
    }
    myPrintkI("NUS Client module initialized %d\r\n", i);
  }
#else

  err = bt_nus_client_init(&nus_client, &init);
  if (err) {
    myPrintkE("NUS Client initialization failed (err %d)\r\n", err);
    return err;
  }
  myPrintkI("NUS Client module initialized\r\n");
#endif

  return err;
}

BT_SCAN_CB_INIT(scan_cb, scan_filter_match, NULL, scan_connecting_error, scan_connecting);

static void try_add_address_filter(const struct bt_bond_info *info, void *user_data) {
  int err;
  char addr[BT_ADDR_LE_STR_LEN];
  uint8_t *filter_mode = user_data;

  bt_addr_le_to_str(&info->addr, addr, sizeof(addr));

  struct bt_conn *conn = bt_conn_lookup_addr_le(BT_ID_DEFAULT, &info->addr);

  if (conn) {
    bt_conn_unref(conn);
    return;
  }

  err = bt_scan_filter_add(BT_SCAN_FILTER_TYPE_ADDR, &info->addr);
  if (err) {
    LOG_ERR("Address filter cannot be added (err %d): %s", err, addr);
    return;
  }

  LOG_INF("Address filter added: %s", addr);
  *filter_mode |= BT_SCAN_ADDR_FILTER;
}

static int scan_start(void) {
  int err;
  uint8_t filter_mode = 0;

  err = bt_scan_stop();
  if (err) {
    LOG_ERR("Failed to stop scanning (err %d)", err);
    return err;
  }

  bt_scan_filter_remove_all();

  err = bt_scan_filter_add(BT_SCAN_FILTER_TYPE_UUID, BT_UUID_NUS_SERVICE);
  if (err) {
    LOG_ERR("UUID filter cannot be added (err %d", err);
    return err;
  }
  filter_mode |= BT_SCAN_UUID_FILTER;

  bt_foreach_bond(BT_ID_DEFAULT, try_add_address_filter, &filter_mode);

  err = bt_scan_filter_enable(filter_mode, false);
  if (err) {
    LOG_ERR("Filters cannot be turned on (err %d)", err);
    return err;
  }

  err = bt_scan_start(BT_SCAN_TYPE_SCAN_ACTIVE);
  if (err) {
    LOG_ERR("Scanning failed to start (err %d)", err);
    return err;
  }

  myPrintkI("Scan started\r\n");
  LOG_INF("Scan started");
  return 0;
}

static void scan_work_handler(struct k_work *item) {
  ARG_UNUSED(item);

  (void)scan_start();
}

static void scan_init(void) {
  struct bt_scan_init_param scan_init = {
      .connect_if_match = true,
  };

  bt_scan_init(&scan_init);
  bt_scan_cb_register(&scan_cb);

  k_work_init(&scan_work, scan_work_handler);
  LOG_INF("Scan module initialized");
}

static void auth_cancel(struct bt_conn *conn) {
  char addr[BT_ADDR_LE_STR_LEN];

  bt_addr_le_to_str(bt_conn_get_dst(conn), addr, sizeof(addr));

  LOG_INF("Pairing cancelled: %s", addr);
}

static void pairing_complete(struct bt_conn *conn, bool bonded) {
  char addr[BT_ADDR_LE_STR_LEN];

  bt_addr_le_to_str(bt_conn_get_dst(conn), addr, sizeof(addr));

  LOG_INF("Pairing completed: %s, bonded: %d", addr, bonded);
}

static void pairing_failed(struct bt_conn *conn, enum bt_security_err reason) {
  char addr[BT_ADDR_LE_STR_LEN];

  bt_addr_le_to_str(bt_conn_get_dst(conn), addr, sizeof(addr));

  LOG_WRN("Pairing failed conn: %s, reason %d %s", addr, reason, bt_security_err_to_str(reason));
}

static struct bt_conn_auth_cb conn_auth_callbacks = {
    .cancel = auth_cancel,
};

static struct bt_conn_auth_info_cb conn_auth_info_callbacks = {.pairing_complete = pairing_complete, .pairing_failed = pairing_failed};

#if 1
struct k_timer wdt_timer; //!< timer to reset WDT

/*!
 * \fn void wdt_handler(struct k_work *work)
 *
 * \brief work function WDT timer handler
 *
 *
 * \param work
 *
 */
void wdt_handler(struct k_work *work) {
  if (stopWDTFeed == 0) /* check if WDT feed disabled*/
  {
    //		myPrintkS("Feeding watchdog... %d\n",stopWDTFeed);
    wdt_feed(wdt, wdt_channel_id);
  }
}

K_WORK_DEFINE(wdt_work, wdt_handler);

/*!
 * \fn void wdt_timer_handler(struct k_timer *dummy)
 *
 * \brief timer CLI command handler
 *
 * timer handler just submits timer work function to the work queue
 *
 * \param dummy pointer to a timer sturcture
 *
 */
void wdt_timer_handler(struct k_timer *dummy) { k_work_submit(&wdt_work); }
#endif

#ifdef USE_THREAD
#define MY_STACK_SIZE 1024
#define MY_PRIORITY 7
const k_tid_t my_uart_thread_id;

void my_uart_thread_logic(void *arg1, void *arg2, void *arg3) {
  int err = 0;

  struct uart_data_t nus_data = {
      .len = 0,
  };
  myPrintkI("starting BLE thread\r\n");
  /* Your logic from main()'s loop goes here */
  while (1) {
    // e.g., k_fifo_get(&fifo_uart_rx_data, K_FOREVER);
    // ... process data ...

    /* IMPORTANT: If your loop doesn't have a blocking call like
       k_fifo_get(..., K_FOREVER), you MUST add a sleep to prevent
       starving other threads of the same priority. */
    //	for (;;)
    {
      /* Wait indefinitely for data to be sent over Bluetooth */
      struct uart_data_t *buf = k_fifo_get(&fifo_uart_rx_data, K_FOREVER);
      printf("\r\n\nUART seen %s\r\n\n", buf->data);

      int plen = MIN(sizeof(nus_data.data) - nus_data.len, buf->len);
      int loc = 0;

      while (plen > 0) {
        memcpy(&nus_data.data[nus_data.len], &buf->data[loc], plen);
        nus_data.len += plen;
        loc += plen;
        if (nus_data.len >= sizeof(nus_data.data) || (nus_data.data[nus_data.len - 1] == '\n') ||
            (nus_data.data[nus_data.len - 1] == '\r')) {
          if (nus_data.data[0] == ':') {
            memset(cliCommand, 0, strlen(cliCommand));
            memcpy(cliCommand, &nus_data.data[1], nus_data.len - 1);
            cliCommand[strlen(cliCommand) - 1] = '\0';
            printf("\r\nCLI Command: %s\r\n", cliCommand);
            executeCmd(cliCommand);
          } else {
            myPrintkI("\r\nBLE Send: %d %s\r\n\n", nus_data.len, nus_data.data);
            err = bt_nus_client_send(&nus_client, nus_data.data, nus_data.len);
            if (err) {
              myPrintkE("Failed to send data over BLE connection(err %d)\r\n", err);
            }
            //						err = k_sem_take(&nus_write_sem, NUS_WRITE_TIMEOUT);
            //						if (err)
            //						{
            //							LOG_WRN("NUS send timeout");
            //						}
          }

          nus_data.len = 0;
        }

        plen = MIN(sizeof(nus_data.data), buf->len - loc);
      }

      //			k_free(buf);
    }
    k_msleep(10);
  }
}
#endif

#ifdef USE_THREAD
K_THREAD_DEFINE(my_uart_thread_id, MY_STACK_SIZE, my_uart_thread_logic, NULL, NULL, NULL, MY_PRIORITY, 0, 0);
#endif

int central_uart_main(void) {
  int err;
  k_msgq_init(&my_msgq1, my_msgq1_buffer, sizeof(struct data_item_type), 1);
  k_msgq_init(&my_msgq2, my_msgq2_buffer, sizeof(struct data_item_type), 1);
  k_msgq_init(&my_msgq3, my_msgq3_buffer, sizeof(struct data_battery_type), 1);

  /* initialize the discovery call backs */
  for (int i = 0; i < CONFIG_BT_MAX_CONN; i++) {
    discovery_cb[i].completed = discovery_complete;
    discovery_cb[i].service_not_found = discovery_service_not_found;
    discovery_cb[i].error_found = discovery_error;
  }

#if 1
  struct wdt_timeout_cfg wdt_config = {
      /* Reset SoC when watchdog timer expires. */
      .flags = WDT_FLAG_RESET_SOC,

      /* Expire watchdog after max window */
      .window.min = WDT_MIN_WINDOW,
      .window.max = WDT_MAX_WINDOW,
  };
#endif

  err = bt_conn_auth_cb_register(&conn_auth_callbacks);
  if (err) {
    //		LOG_ERR("Failed to register authorization callbacks.");
    myPrintkE("Failed to register authorization callbacks.\r\n");
    return 0;
  }
  myPrintkI("Passed register authorization callbacks.\r\n");

  err = bt_conn_auth_info_cb_register(&conn_auth_info_callbacks);
  if (err) {
    myPrintkE("Failed to register authorization info callbacks.\r\n");
    return 0;
  }
  myPrintkI("Passed register authorization info callbacks.\r\n");

  err = bt_enable(NULL);
  if (err) {
    myPrintkE("Bluetooth init failed (err %d)\r\n", err);
    //		LOG_ERR("Bluetooth init failed (err %d)", err);
    return 0;
  }
  myPrintkI("Bluetooth initialized\r\n");
  //	LOG_INF("Bluetooth initialized");

  if (IS_ENABLED(CONFIG_SETTINGS)) {
    settings_load();
  }

  //	err = uart_init();
  //	if (err != 0)
  //	{
  //		printk("\r\nuart_init failed (err %d)\r\n", err);
  //		return 0;
  //	}
  //		printk("\r\nuart_init passed (err %d)\r\n", err);

  err = nus_client_init();
  if (err != 0) {
    myPrintkE("nus_client_init failed (err %d)\r\n", err);
    return 0;
  }
  myPrintkI("nus_client_init passed (err %d)\r\n", err);

  scan_init();
  err = scan_start();
  if (err) {
    myPrintkE("scan_start failed (err %d)\r\n", err);
    return 0;
  }
  myPrintkI("scan_start passed (err %d)\r\n", err);

  //	printk("Starting Bluetooth Central UART (central_uart_tjm) sample\n");
  myPrintkI("Starting Bluetooth Central UART (central_uart_tjm)\r\n");

  /* set up watchdog timer */
#if 1
  if (!device_is_ready(wdt)) {
    myPrintkE("%s: Watchdog device not ready.\n", wdt->name);
    return 0;
  }
  myPrintkI("%s: Watchdog device ready.\n", wdt->name);

  wdt_channel_id = wdt_install_timeout(wdt, &wdt_config);

  switch (wdt_channel_id) {
  case -EBUSY:
    myPrintkE("Watchdog timeout can not be installed while watchdog has already been setup.\n");
    return 0;
  case -ENOTSUP:
    myPrintkE("Watchdog Callback support rejected, continuing anyway\n");
    return 0;
  case -EINVAL:
    myPrintkE("Watchdog install error: invalid configuration\n");
    return 0;
  default:
    if (wdt_channel_id < 0) {
      myPrintkE("Watchdog install error: %d\n", wdt_channel_id);
      return 0;
    }
    myPrintkI("Watchdog install passed\n");
  }

#if 0
	if (wdt_channel_id < 0)
	{
		myPrintkE("Watchdog install error\n");
		return 0;
	}
	myPrintkI("Watchdog install passed\n");

	if (wdt_channel_id == -ENOTSUP)
	{
		/* IWDG driver for STM32 doesn't support callback */
		myPrintkE("Watchdog Callback support rejected, continuing anyway\n");
		wdt_config.callback = NULL;
		wdt_channel_id = wdt_install_timeout(wdt, &wdt_config);
	}
	myPrintkI("Watchdog Callback support passed\n");
#endif

  err = wdt_setup(wdt, WDT_OPT);
  if (err < 0) {
    myPrintkE("Watchdog setup error\n");
    return 0;
  }
  myPrintkI("Watchdog setup passed. Start WDT feed timer\n");

#if 1
  k_timer_init(&wdt_timer, wdt_timer_handler, NULL);
  k_timer_start(&wdt_timer, K_MSEC(WDG_FEED_INTERVAL), K_MSEC(WDG_FEED_INTERVAL));
#else
  /* Feeding watchdog. */
  myPrintkS("Feeding watchdog %d times %d ms\n", WDT_FEED_TRIES, WDG_FEED_INTERVAL);
  for (int i = 0; i < WDT_FEED_TRIES; ++i) {
    myPrintkS("Feeding watchdog...\n");
    wdt_feed(wdt, wdt_channel_id);
    k_sleep(K_MSEC(WDG_FEED_INTERVAL));
  }

  /* Waiting for the SoC reset. */
  myPrintkS("Waiting for reset...\n");
  while (1) {
    k_yield();
  }
  return 0;
#endif

#endif /* end WDT timer */

#ifdef LTE_MERGE
  /* merge from dl_agri_new_nrf9160 */

  if (app_event_manager_init()) {
    /* Without the Application Event Manager, the application will not work
     * as intended. A reboot is required in an attempt to recover.
     */
    //		LOG_ERR("Application Event Manager could not be initialized, rebooting...");
    //		strcpy(flashParameters.sysRebootReason, "Event manager init failed");
    //		setFlashxParameters(runFlag ? false : true);
    //		k_sleep(K_SECONDS(5));
    //		sys_reboot(SYS_REBOOT_COLD);
  } else {
    //		module_set_state(MODULE_STATE_READY);
    //		SEND_EVENT(app, APP_EVT_START);

#if defined(CONFIG_NRF_MODEM_LIB)
//		modem_init();
#endif
  }

#endif /* end dl_agri)new_nrf9160 merge */

#ifndef USE_THREAD
  struct uart_data_t nus_data = {
      .len = 0,
  };

  for (;;) {
    /* Wait indefinitely for data to be sent over Bluetooth */
    struct uart_data_t *buf = k_fifo_get(&fifo_uart_rx_data, K_FOREVER);

    int plen = MIN(sizeof(nus_data.data) - nus_data.len, buf->len);
    int loc = 0;

    while (plen > 0) {
      memcpy(&nus_data.data[nus_data.len], &buf->data[loc], plen);
      nus_data.len += plen;
      loc += plen;
      if (nus_data.len >= sizeof(nus_data.data) || (nus_data.data[nus_data.len - 1] == '\n') || (nus_data.data[nus_data.len - 1] == '\r')) {
        if (nus_data.data[0] == ':') {
          memset(cliCommand, 0, strlen(cliCommand));
          memcpy(cliCommand, &nus_data.data[1], nus_data.len - 1);
          cliCommand[strlen(cliCommand) - 1] = '\0';
          //					printf("\r\nCLI Command: %s\r\n", cliCommand);
          executeCmd(cliCommand);
        } else {
          myPrintkI("\r\nBLE Send: %d %s\r\n\n", nus_data.len, nus_data.data);
          err = bt_nus_client_send(&nus_client, nus_data.data, nus_data.len);
        }
        if (err) {
          LOG_WRN("Failed to send data over BLE connection"
                  "(err %d)",
                  err);
        }

        err = k_sem_take(&nus_write_sem, NUS_WRITE_TIMEOUT);
        if (err) {
          LOG_WRN("NUS send timeout");
        }

        nus_data.len = 0;
      }

      plen = MIN(sizeof(nus_data.data), buf->len - loc);
    }

    k_free(buf);
  }
#endif
  return 0;
}

void parseBLEString(char *bleData) {
  myPrintkS("parseBLEString BLE data seen %s\r\n", bleData);
  uint8_t i;
  int len = strlen(bleData);
  struct data_item_type tempBLEData;
  struct data_item_type temp2BLEData;
  struct data_battery_type tempBatteryData;

  /* parse all parameters on the command line into cmdLineparameter array */
  numBLEParameters = 0; /* no parameters seen yet */

  BLEParameter[numBLEParameters++] = &bleData[0];
  for (i = 0; i < len; i++) {
    //            printf("%d 0x%02X %c\r\n",i,bleData[i],bleData[i]);
    if ((bleData[i] == ' ') || (bleData[i] == '\t')) {
      bleData[i] = '\0';
      BLEParameter[numBLEParameters++] = &bleData[i + 1];
    }
  }
  bleData[i] = '\0';

  /* wait for HSC temperature/humidity data */
  if (strcmp(BLEParameter[2], "t1") == 0) {
    strcpy(tempBLEData.BLEId, BLEParameter[1]);
    tempBLEData.BLETemp = atof(BLEParameter[3]);
    tempBLEData.BLEHum = atof(BLEParameter[5]);
    memcpy(my_msgq1_buffer, &tempBLEData, sizeof(struct data_item_type));
    myPrintkS("BLEId: %s BLEHDC2080Temp: %3.2f BLEHDC2080Hum: %3.2f\r\n", tempBLEData.BLEId, tempBLEData.BLETemp, tempBLEData.BLEHum);
    /* send data to consumers */
    if (k_msgq_put(&my_msgq1, my_msgq1_buffer, K_NO_WAIT) != 0) {
      myPrintkW("Message queue 1 is full, purging old data\r\n");
      /* message queue is full: purge old data & try again */
      k_msgq_purge(&my_msgq1);
      k_msgq_put(&my_msgq1, my_msgq1_buffer, K_NO_WAIT);
    }
  }

  /* wait for BME688 temperature/humidity data */
  if (strcmp(BLEParameter[2], "t2") == 0) {
    strcpy(temp2BLEData.BLEId, BLEParameter[1]);
    temp2BLEData.BLETemp = atof(BLEParameter[3]);
    temp2BLEData.BLEHum = atof(BLEParameter[5]);
    memcpy(my_msgq2_buffer, &temp2BLEData, sizeof(struct data_item_type));
    myPrintkS("BLEId: %s BLEBME688Temp2: %3.2f BLEBME688Hum2: %3.2f\r\n", temp2BLEData.BLEId, temp2BLEData.BLETemp, temp2BLEData.BLEHum);
    /* send data to consumers */
    if (k_msgq_put(&my_msgq2, my_msgq2_buffer, K_NO_WAIT) != 0) {
      myPrintkW("Message queue 2 is full, purging old data\r\n");
      /* message queue is full: purge old data & try again */
      k_msgq_purge(&my_msgq2);
      k_msgq_put(&my_msgq2, my_msgq2_buffer, K_NO_WAIT);
    }
  }

  /* wait for Battery data */
  if (strcmp(BLEParameter[2], "bat") == 0) {
    strcpy(tempBatteryData.BLEId, BLEParameter[1]);
    tempBatteryData.BLEBattery = atoi(BLEParameter[3]);
    memcpy(my_msgq3_buffer, &tempBatteryData, sizeof(struct data_battery_type));
    myPrintkS("BLEId: %s Battery: %d \r\n", tempBatteryData.BLEId, tempBatteryData.BLEBattery);
    /* send data to consumers */
    if (k_msgq_put(&my_msgq3, my_msgq3_buffer, K_NO_WAIT) != 0) {
      myPrintkW("Message queue 3 is full, purging old data\r\n");
      /* message queue is full: purge old data & try again */
      k_msgq_purge(&my_msgq3);
      k_msgq_put(&my_msgq3, my_msgq3_buffer, K_NO_WAIT);
    }
  }
}

/*
 * Black \033[0;30m.
 * Red \033[0;31m.
 * Green \033[0;32m.
 * Yellow \033[0;33m.
 * Blue \033[0;34m.
 * Purple \033[0;35m.
 * Cyan \033[0;36m.
 * White \033[0;37m.
 */

/* yellow (Warning)*/
#if 0
void myPrintkW(char *str) {
  printk("%s", "\33[1;33m");
  printk("%s", str);
  // printk("%s", myStr);
  printk("%s", "\33[0m");
  return;
}
#endif

int myPrintkW(char *restrict fmt, ...) {
  int n = 0;
  va_list ap;

  printk("%s", "\33[1;33m");
  va_start(ap, fmt);
  //  n = vprintf(fmt, ap);
  vprintk(fmt, ap);
  va_end(ap);
  printk("%s", "\33[0m");
  // printf("n=%d\n", n);
  return n;
}

/* Green (Information)*/
#if 0
void myPrintkI(char *str) {
  printk("%s", "\33[1;32m");
  printk("%s", str);
  // printk("%s", myStr);
  printk("%s", "\33[0m");
  return;
}
#endif

int myPrintkI(char *restrict fmt, ...) {
  int n = 0;
  va_list ap;

  printk("%s", "\33[1;32m");
  va_start(ap, fmt);
  //  n = vprintf(fmt, ap);
  vprintk(fmt, ap);
  va_end(ap);
  printk("%s", "\33[0m");
  // printf("n=%d\n", n);
  return n;
}

/* CYAN (Status)*/
#if 0
void myPrintkS(char *str) {
  printk("%s", "\33[1;36m");
  printk("%s", str);
  // printk("%s", myStr);
  printk("%s", "\33[0m");
}
#endif

int myPrintkS(char *restrict fmt, ...) {
  int n = 0;
  va_list ap;

  printk("%s", "\33[1;36m");
  va_start(ap, fmt);
  //  n = vprintf(fmt, ap);
  vprintk(fmt, ap);
  va_end(ap);
  printk("%s", "\33[0m");
  // printf("n=%d\n", n);
  return n;
}

/* Red (Error)*/
#if 0
void myPrintkE(char *str) {
  printk("%s", "\33[1;31m");
  printk("%s", str);
  // printk("%s", myStr);
  printk("%s", "\33[0m");
  return;
}
#endif

int myPrintkE(char *restrict fmt, ...) {
  int n = 0;
  va_list ap;

  printk("%s", "\33[1;31m");
  va_start(ap, fmt);
  //  n = vprintf(fmt, ap);
  vprintk(fmt, ap);
  va_end(ap);
  printk("%s", "\33[0m");
  // printf("n=%d\n", n);
  return n;
}