Diff: main.cpp

Revision:

0:e1e70da93044

Child:

2:972a5704f152

diff -r 000000000000 -r e1e70da93044 main.cpp
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Wed Apr 24 10:11:06 2019 -0700
@@ -0,0 +1,1699 @@
+#include "mbed.h"
+#include "radio.h"
+#include "demo.h"
+
+bool svc_lis2dh12;
+//#define MENU_DEBUG
+
+RawSerial pc(USBTX, USBRX);
+
+menuState_t menuState;
+
+typedef enum {
+    MSG_TYPE_PACKET = 0,
+    MSG_TYPE_PER,
+    MSG_TYPE_PINGPONG
+} msgType_e;
+
+enum {
+    CMD_TEMP,
+    CMD_PRES,
+    CMD_ACCEL,
+    CMD_PHOTOS
+};
+
+#define MAX_MENU_ROWS       16
+// [row][column]
+const menu_t* menu_table[MAX_MENU_ROWS][MAX_MENU_COLUMNS];
+int8_t StopMenuCols[MAX_MENU_ROWS];
+
+//#define SCROLLING_ROWS      20
+#define SCROLLING_ROWS      30
+
+struct _cp_ {
+    uint8_t row;
+    uint8_t tableCol;
+} curpos;
+
+uint8_t entry_buf_idx;
+char entry_buf[64];
+
+msgType_e msg_type;
+
+const uint8_t PingMsg[] = "PING";
+const uint8_t PongMsg[] = "PONG";
+const uint8_t PerMsg[]  = "PER";
+
+volatile struct _flags_ {
+    uint8_t ping_master     : 1; // 0
+    uint8_t send_ping       : 1; // 1
+    uint8_t send_pong       : 1; // 2
+    uint8_t pingpongEnable  : 1; // 3
+    uint8_t do_next_tx      : 1; // 4
+} flags;
+
+uint8_t botRow;
+int regAddr = -1;
+
+/*
+ * available pins PA_0, PA_1, PA_2, PA_3, PA_4, PA_5, PA_6, PA_7, PB_0, PB_1, PC_0, PC_1, PC_2
+#define PHOTOS_PIN      
+*/
+#ifdef PHOTOS_PIN
+AnalogIn photos(PHOTOS_PIN);
+#endif /* PHOTOS_PIN */
+
+#define UART_RX_BUF_LEN     8
+uint8_t uart_rx_buf[UART_RX_BUF_LEN];
+volatile uint8_t uart_rx_buf_in;
+volatile uint8_t uart_rx_buf_out;
+bool accel_tx_en;
+
+#ifdef MENU_DEBUG
+char wait_uart_rx()
+{
+    char ret;
+    unsigned foo = uart_rx_buf_in;
+    while (uart_rx_buf_in == foo) {
+        log_printf("waiting %u %u\r\n", uart_rx_buf_in, foo);
+    }
+
+    ret = uart_rx_buf[uart_rx_buf_out++];
+    if (uart_rx_buf_out == UART_RX_BUF_LEN)
+        uart_rx_buf_out = 0;
+
+    return ret;
+}
+#endif /* MENU_DEBUG */
+
+unsigned lfsr;
+#define LFSR_INIT       0x1ff
+
+uint8_t get_pn9_byte()
+{
+    uint8_t ret = 0;
+    int xor_out;
+
+    xor_out = ((lfsr >> 5) & 0xf) ^ (lfsr & 0xf);   // four bits at a time
+    lfsr = (lfsr >> 4) | (xor_out << 5);    // four bits at a time
+
+    ret |= (lfsr >> 5) & 0x0f;
+
+    xor_out = ((lfsr >> 5) & 0xf) ^ (lfsr & 0xf);   // four bits at a time
+    lfsr = (lfsr >> 4) | (xor_out << 5);    // four bits at a time
+
+    ret |= ((lfsr >> 1) & 0xf0);
+
+    return ret;
+}
+
+void hw_reset_push()
+{
+    Radio::hw_reset();
+
+    Radio::readChip();
+    menuState.mode = MENUMODE_REINIT_MENU;
+}
+
+const button_item_t hw_reset_item = { _ITEM_BUTTON, "hw_reset", hw_reset_push };
+
+void clearIrqs_push()
+{
+    Radio::clearIrqFlags();
+}
+
+const button_item_t clearirqs_item = { _ITEM_BUTTON, "clearIrqs", clearIrqs_push };
+
+const dropdown_item_t pktType_item = { _ITEM_DROPDOWN, Radio::pktType_strs, Radio::pktType_strs, Radio::pktType_read, Radio::pktType_write};
+
+
+unsigned msgType_read(bool fw)
+{
+    return msg_type;
+}
+
+menuMode_e msgType_write(unsigned widx)
+{
+    msg_type = (msgType_e)widx;
+
+    if (msg_type == MSG_TYPE_PER) {
+        flags.pingpongEnable = 0;
+        if (Radio::get_payload_length() < 7)
+            Radio::set_payload_length(7);
+    } else if (msg_type == MSG_TYPE_PINGPONG) {
+        if (Radio::get_payload_length() != 12)
+            Radio::set_payload_length(12);
+    } else if (msg_type == MSG_TYPE_PACKET) {
+        flags.pingpongEnable = 0;
+    }
+
+    return MENUMODE_REINIT_MENU;
+}
+
+const char* const msgType_strs[] = {
+    "PACKET ",
+    "PER    ",
+    "PINGPONG",
+    NULL
+};
+
+const dropdown_item_t msgType_item = { _ITEM_DROPDOWN, msgType_strs, msgType_strs, msgType_read, msgType_write};
+
+#define LAST_CHIP_MENU_ROW          (MAX_MENU_ROWS-5)
+
+const value_item_t tx_payload_length_item = { _ITEM_VALUE, 5, Radio::tx_payload_length_print, Radio::tx_payload_length_write};
+
+void pn9_push()
+{
+    uint8_t i, len = Radio::get_payload_length();
+    for (i = 0; i < len; i++)
+        Radio::radio.tx_buf[i] = get_pn9_byte();
+}
+
+const button_item_t tx_payload_pn9_item  = { _ITEM_BUTTON, "PN9", pn9_push };
+
+void regAddr_print()
+{
+    if (regAddr != -1)
+        pc.printf("%x", regAddr);
+}
+
+bool regAddr_write(const char* txt)
+{
+    sscanf(txt, "%x", &regAddr);
+    return false;
+}
+
+const value_item_t regAddr_item = { _ITEM_VALUE, 5, regAddr_print, regAddr_write};
+
+void regValue_print()
+{
+    if (regAddr != -1) {
+        pc.printf("%x", Radio::read_register(regAddr));
+    }
+}
+
+bool regValue_write(const char* txt)
+{
+    unsigned val;
+    sscanf(txt, "%x", &val);
+    if (regAddr != -1) {
+        Radio::write_register(regAddr, val);
+    }
+    return false;
+}
+
+const value_item_t regValue_item = { _ITEM_VALUE, 5, regValue_print, regValue_write};
+
+void tx_payload_print()
+{
+    uint8_t i, len = Radio::get_payload_length();
+    for (i = 0; i < len; i++)
+        pc.printf("%02x ", Radio::radio.tx_buf[i]);
+}
+
+bool tx_payload_write(const char* txt)
+{
+    unsigned o, i = 0, pktidx = 0;
+    while (i < entry_buf_idx) {
+        sscanf(entry_buf+i, "%02x", &o);
+        pc.printf("%02x ", o);
+        i += 2;
+        Radio::radio.tx_buf[pktidx++] = o;
+        while (entry_buf[i] == ' ' && i < entry_buf_idx)
+            i++;
+    }
+    log_printf("set payload len %u\r\n", pktidx);
+    Radio::set_payload_length(pktidx);
+    return true;
+}
+
+const value_item_t tx_payload_item = { _ITEM_VALUE, 80, tx_payload_print, tx_payload_write};
+
+void txpkt_push()
+{
+    Radio::txPkt();
+}
+
+const button_item_t tx_pkt_item  = { _ITEM_BUTTON, "TXPKT", txpkt_push };
+
+void rxpkt_push()
+{
+    Radio::Rx();
+    menuState.mode = MENUMODE_REDRAW;
+}
+const button_item_t rx_pkt_item  = { _ITEM_BUTTON, "RXPKT", rxpkt_push };
+
+const dropdown_item_t opmode_item = { _ITEM_DROPDOWN, Radio::opmode_status_strs, Radio::opmode_select_strs, Radio::opmode_read, Radio::opmode_write };
+
+void tx_carrier_push()
+{
+    Radio::tx_carrier();
+}
+const button_item_t tx_carrier_item = { _ITEM_BUTTON, "TX_CARRIER", tx_carrier_push };
+
+void tx_preamble_push()
+{
+    Radio::tx_preamble();
+}
+const button_item_t tx_preamble_item = { _ITEM_BUTTON, "TX_PREAMBLE", tx_preamble_push };
+
+bool accel_en_read()
+{
+    uint8_t status;
+    accel_is_enabled(&status);
+    return status;
+}
+
+bool accel_tx_en_read()
+{
+    uint8_t status;
+    accel_is_enabled(&status);
+    return status && accel_tx_en;
+}
+
+
+bool accel_en_push()
+{
+    uint8_t status;
+    accel_is_enabled(&status);
+
+    accel_tx_en = false;
+    if (status)
+        accel_enable(false);
+    else
+        accel_enable(true);
+
+    accel_is_enabled(&status);
+    return status;
+}
+
+bool accel_tx_en_push()
+{
+    uint8_t status;
+    accel_is_enabled(&status);
+
+    accel_tx_en = true;
+    if (status)
+        accel_enable(false);
+    else
+        accel_enable(true);
+
+    accel_is_enabled(&status);
+    return status;
+}
+
+const toggle_item_t accel_enable_item = { _ITEM_TOGGLE, "enable", NULL, accel_en_read, accel_en_push};
+const toggle_item_t accel_tx_enable_item = { _ITEM_TOGGLE, "tx_enable", NULL, accel_tx_en_read, accel_tx_en_push};
+
+const menu_t lis12dh12_menu[] = {
+    { {LAST_CHIP_MENU_ROW-1,  1}, "LIS12DH12 ", &accel_enable_item, FLAG_MSGTYPE_ALL },
+    { {LAST_CHIP_MENU_ROW-1,  24}, NULL, &accel_tx_enable_item, FLAG_MSGTYPE_ALL },
+};
+
+
+#ifdef PHOTOS_PIN
+void photos_push()
+{
+    uint16_t val = photos.read_u16();
+    log_printf("photos %u\r\n", val);
+}
+
+void tx_photos_push()
+{
+    uint16_t val = photos.read_u16();
+    log_printf("Tx photos %u\r\n", val);
+    Radio::radio.tx_buf[0] = CMD_PHOTOS;
+    Radio::set_payload_length(sizeof(uint16_t)+1);
+    memcpy(&Radio::radio.tx_buf[1], &val, sizeof(uint16_t));
+    Radio::txPkt();
+}
+
+const button_item_t photos_item = { _ITEM_BUTTON, "photos", photos_push };
+const button_item_t tx_photos_item = { _ITEM_BUTTON, "tx_photos", tx_photos_push };
+
+const menu_t photos_menu[] = {
+    { {LAST_CHIP_MENU_ROW-2,  1}, "PHOTOS ", &photos_item, FLAG_MSGTYPE_ALL },
+    { {LAST_CHIP_MENU_ROW-2,  24}, NULL, &tx_photos_item, FLAG_MSGTYPE_ALL },
+};
+#endif /* PHOTOS_PIN */
+
+void temperature_push()
+{
+    displayFloatToInt_t val;
+    demo_sample_temp(&val);
+}
+
+void pressure_push()
+{
+    displayFloatToInt_t val;
+    demo_sample_pressure(&val);
+}
+
+void tx_temp_push()
+{
+    displayFloatToInt_t val;
+    demo_sample_temp(&val);
+
+    Radio::set_payload_length(sizeof(displayFloatToInt_t)+1);
+
+    Radio::radio.tx_buf[0] = CMD_TEMP;
+    memcpy(&Radio::radio.tx_buf[1], &val, sizeof(displayFloatToInt_t));
+
+    Radio::txPkt();
+}
+
+void tx_pressure_push()
+{
+    displayFloatToInt_t val;
+    demo_sample_pressure(&val);
+
+    Radio::set_payload_length(sizeof(displayFloatToInt_t)+1);
+
+    Radio::radio.tx_buf[0] = CMD_PRES;
+    memcpy(&Radio::radio.tx_buf[1], &val, sizeof(displayFloatToInt_t));
+
+    Radio::txPkt();
+}
+
+
+const button_item_t temperature_item = { _ITEM_BUTTON, "temperature", temperature_push };
+const button_item_t pressure_item = { _ITEM_BUTTON, "pressure", pressure_push };
+const button_item_t tx_temp_item = { _ITEM_BUTTON, "tx_temp", tx_temp_push };
+const button_item_t tx_pressure_item = { _ITEM_BUTTON, "tx_pressure", tx_pressure_push };
+
+
+const menu_t lps22hh_menu[] = {
+    { {LAST_CHIP_MENU_ROW,  1}, "LPS22HH ", &temperature_item, FLAG_MSGTYPE_ALL },
+    { {LAST_CHIP_MENU_ROW, 22}, NULL, &pressure_item, FLAG_MSGTYPE_ALL },
+    { {LAST_CHIP_MENU_ROW, 31}, NULL, &tx_temp_item, FLAG_MSGTYPE_ALL },
+    { {LAST_CHIP_MENU_ROW, 40}, NULL, &tx_pressure_item, FLAG_MSGTYPE_ALL },
+};
+
+const menu_t msg_pkt_menu[] = {
+    { {LAST_CHIP_MENU_ROW+1,  1}, "tx payload length:", &tx_payload_length_item, FLAG_MSGTYPE_PKT },
+    { {LAST_CHIP_MENU_ROW+1, 25},                 NULL,    &tx_payload_pn9_item, FLAG_MSGTYPE_PKT, &tx_payload_item },
+    { {LAST_CHIP_MENU_ROW+1, 40},           "regAddr:",           &regAddr_item, FLAG_MSGTYPE_PKT, &regValue_item },
+    { {LAST_CHIP_MENU_ROW+1, 53},          "regValue:",          &regValue_item, FLAG_MSGTYPE_PKT, },
+    { {LAST_CHIP_MENU_ROW+2,  1},                 NULL,        &tx_payload_item, FLAG_MSGTYPE_PKT },
+    { {LAST_CHIP_MENU_ROW+3,  1},                 NULL,            &tx_pkt_item, FLAG_MSGTYPE_PKT, &opmode_item },
+    { {LAST_CHIP_MENU_ROW+3, 10},                 NULL,            &rx_pkt_item, FLAG_MSGTYPE_PKT },
+    { {LAST_CHIP_MENU_ROW+3, 20},                 NULL,        &tx_carrier_item, FLAG_MSGTYPE_PKT, &opmode_item },
+    { {LAST_CHIP_MENU_ROW+3, 45},                 NULL,       &tx_preamble_item, FLAG_MSGTYPE_PKT, &opmode_item },
+
+    { {0, 0}, NULL, NULL }
+};
+
+unsigned tx_ipd_ms;
+Timeout mbedTimeout;
+unsigned MaxNumPacket;
+unsigned CntPacketTx;
+unsigned PacketRxSequencePrev;
+unsigned CntPacketRxKO;
+unsigned CntPacketRxOK;
+unsigned RxTimeOutCount;
+unsigned receivedCntPacket;
+
+void do_next_tx ()
+{
+    Radio::radio.tx_buf[0] = CntPacketTx >> 24;
+    Radio::radio.tx_buf[1] = CntPacketTx >> 16;
+    Radio::radio.tx_buf[2] = CntPacketTx >> 8;
+    Radio::radio.tx_buf[3] = CntPacketTx;
+
+    Radio::radio.tx_buf[4] = PerMsg[0];
+    Radio::radio.tx_buf[5] = PerMsg[1];
+    Radio::radio.tx_buf[6] = PerMsg[2];
+
+    Radio::txPkt();
+}
+
+void next_tx_callback()
+{
+    flags.do_next_tx = 1;
+}
+
+
+void pertx_push()
+{
+    CntPacketTx = 1;    // PacketRxSequencePrev initialized to 0 on receiver
+
+    log_printf("do perTx\r\n");
+
+    next_tx_callback();
+}
+
+
+const button_item_t pertx_item = { _ITEM_BUTTON, "PERTX", pertx_push };
+
+void tx_ipd_print()
+{
+    pc.printf("%u", tx_ipd_ms);
+}
+
+bool tx_ipd_write(const char* valStr)
+{
+    sscanf(valStr, "%u", &tx_ipd_ms);
+    return false;
+}
+
+value_item_t per_ipd_item = { _ITEM_VALUE, 4, tx_ipd_print, tx_ipd_write };
+
+void numpkts_print()
+{
+    pc.printf("%u", MaxNumPacket);
+}
+
+bool numpkts_write(const char* valStr)
+{
+    sscanf(valStr, "%u", &MaxNumPacket);
+    return false;
+}
+
+value_item_t per_numpkts_item = { _ITEM_VALUE, 6, numpkts_print, numpkts_write };
+
+void perrx_push()
+{
+    PacketRxSequencePrev = 0;
+    CntPacketRxKO = 0;
+    CntPacketRxOK = 0;
+    RxTimeOutCount = 0;
+
+    Radio::Rx();
+}
+
+const button_item_t perrx_item = { _ITEM_BUTTON, "PERRX", perrx_push };
+
+void per_reset_push()
+{
+    PacketRxSequencePrev = 0;
+    CntPacketRxKO = 0;
+    CntPacketRxOK = 0;
+    RxTimeOutCount = 0;
+}
+
+const button_item_t per_clear_item = { _ITEM_BUTTON, "resetCount", per_reset_push };
+
+const menu_t msg_per_menu[] = {
+    { {LAST_CHIP_MENU_ROW+3,  1},          NULL,       &pertx_item, FLAG_MSGTYPE_PER },
+    { {LAST_CHIP_MENU_ROW+3,  12}, "TX IPD ms:",     &per_ipd_item, FLAG_MSGTYPE_PER },
+    { {LAST_CHIP_MENU_ROW+3,  26},   "numPkts:", &per_numpkts_item, FLAG_MSGTYPE_PER },
+    { {LAST_CHIP_MENU_ROW+3,  40},         NULL,       &perrx_item, FLAG_MSGTYPE_PER, &opmode_item },
+    { {LAST_CHIP_MENU_ROW+3,  50},         NULL,   &per_clear_item, FLAG_MSGTYPE_PER, &opmode_item },
+    { {0, 0}, NULL, NULL }
+};
+
+void SendPong()
+{
+    unsigned failCnt = CntPacketRxKO + RxTimeOutCount;
+
+    /* ping slave tx */
+    log_printf("ACK PKT%u\r\n", receivedCntPacket);
+
+    Radio::radio.tx_buf[ 0] = receivedCntPacket >> 24;
+    Radio::radio.tx_buf[ 1] = receivedCntPacket >> 16;
+    Radio::radio.tx_buf[ 2] = receivedCntPacket >> 8;
+    Radio::radio.tx_buf[ 3] = receivedCntPacket;
+    Radio::radio.tx_buf[ 4] = failCnt >> 24;
+    Radio::radio.tx_buf[ 5] = failCnt >> 16;
+    Radio::radio.tx_buf[ 6] = failCnt >> 8;
+    Radio::radio.tx_buf[ 7] = failCnt;
+    Radio::radio.tx_buf[ 8] = PongMsg[0];
+    Radio::radio.tx_buf[ 9] = PongMsg[1];
+    Radio::radio.tx_buf[10] = PongMsg[2];
+    Radio::radio.tx_buf[11] = PongMsg[3];
+
+    Radio::txPkt();
+}
+
+void SendPing()
+{
+    /* ping master tx */
+
+    log_printf("MASTER > PING PKT%u\r\n", CntPacketTx);
+    Radio::radio.tx_buf[0] = CntPacketTx >> 24;
+    Radio::radio.tx_buf[1] = CntPacketTx >> 16;
+    Radio::radio.tx_buf[2] = CntPacketTx >> 8;
+    Radio::radio.tx_buf[3] = CntPacketTx;
+    Radio::radio.tx_buf[4] = PingMsg[0];
+    Radio::radio.tx_buf[5] = PingMsg[1];
+    Radio::radio.tx_buf[6] = PingMsg[2];
+    Radio::radio.tx_buf[7] = PingMsg[3];
+
+    Radio::txPkt();
+}
+
+void
+pingpong_start_push()
+{
+    CntPacketTx = 1;
+    CntPacketRxKO = 0;
+    CntPacketRxOK = 0;
+    RxTimeOutCount = 0;
+    receivedCntPacket = 0;
+
+    flags.send_ping = 1;
+
+    flags.ping_master = 1;
+
+    flags.pingpongEnable = 1;
+    log_printf("ping start\r\n");
+}
+
+const button_item_t pingpong_start_item = { _ITEM_BUTTON, "START", pingpong_start_push };
+
+void
+pingpong_stop_push ()
+{
+    flags.pingpongEnable = 0;
+}
+
+const button_item_t pingpong_stop_item = { _ITEM_BUTTON, "STOP", pingpong_stop_push };
+
+const menu_t msg_pingpong_menu[] = {
+    { {LAST_CHIP_MENU_ROW+3,  1}, NULL, &pingpong_start_item, FLAG_MSGTYPE_PING },
+    { {LAST_CHIP_MENU_ROW+3, 15}, NULL,  &pingpong_stop_item, FLAG_MSGTYPE_PING },
+    { {0, 0}, NULL, NULL }
+};
+
+const menu_t* get_msg_menu()
+{
+    switch (msg_type) {
+        case MSG_TYPE_PACKET:
+            return msg_pkt_menu;
+        case MSG_TYPE_PER:
+            return msg_per_menu;
+        case MSG_TYPE_PINGPONG:
+            return msg_pingpong_menu;
+    }
+    return NULL;
+}
+
+void frf_print()
+{
+    float MHz;
+#ifdef SX127x_H
+    MHz = Radio::radio.get_frf_MHz();
+#else
+    MHz = Radio::radio.getMHz();
+#endif
+    pc.printf("%.3fMHz", MHz);
+}
+
+bool frf_write(const char* valStr)
+{
+    float MHz;
+    sscanf(valStr, "%f", &MHz);
+#ifdef SX127x_H
+    Radio::radio.set_frf_MHz(MHz);
+#else
+    Radio::radio.setMHz(MHz);
+#endif
+    return false;
+}
+
+const value_item_t frf_item = { _ITEM_VALUE, 14, frf_print, frf_write };
+
+const value_item_t Radio::tx_dbm_item = { _ITEM_VALUE, 7, Radio::tx_dbm_print, Radio::tx_dbm_write };
+
+const dropdown_item_t tx_ramp_item = { _ITEM_DROPDOWN, Radio::tx_ramp_strs, Radio::tx_ramp_strs, Radio::tx_ramp_read, Radio::tx_ramp_write };
+
+const button_item_t chipNum_item = { _ITEM_BUTTON, Radio::chipNum_str, NULL};
+
+void botrow_print()
+{
+    pc.printf("%u", botRow);
+}
+
+bool botrow_write(const char* str)
+{
+    unsigned n;
+    sscanf(str, "%u", &n);
+    botRow = n;
+
+    pc.printf("\e[%u;%ur", MAX_MENU_ROWS, botRow); // set scrolling region
+
+    return false;
+}
+
+const value_item_t bottomRow_item = { _ITEM_VALUE, 4, botrow_print, botrow_write };
+
+const menu_t common_menu[] = {
+    {  {1, 1},          NULL,      &hw_reset_item, FLAG_MSGTYPE_ALL },
+    { {1, 15}, "packetType:",       &pktType_item, FLAG_MSGTYPE_ALL },
+    { {1, 37},          NULL,       &msgType_item, FLAG_MSGTYPE_ALL },
+    { {1, 50},          NULL,       &chipNum_item, FLAG_MSGTYPE_ALL },
+    { {1, 60},  "bottomRow:",     &bottomRow_item, FLAG_MSGTYPE_ALL },
+    { {2, 1},           NULL,           &frf_item, FLAG_MSGTYPE_ALL },
+    { {2, 15},     "TX dBm:", &Radio::tx_dbm_item, FLAG_MSGTYPE_ALL },
+    { {2, 30},    "ramp us:",       &tx_ramp_item, FLAG_MSGTYPE_ALL },
+    { {2, 45},          NULL,     &clearirqs_item, FLAG_MSGTYPE_ALL },
+    { {2, 55},     "opmode:",        &opmode_item, FLAG_MSGTYPE_ALL },
+
+
+    { {0, 0}, NULL, NULL }
+};
+
+bool
+is_menu_item_changable(uint8_t table_row, uint8_t table_col)
+{
+    const menu_t* m = menu_table[table_row][table_col];
+    const dropdown_item_t* di = (const dropdown_item_t*)m->itemPtr;
+
+    switch (msg_type) {
+        case MSG_TYPE_PACKET:
+            if (m->flags & FLAG_MSGTYPE_PKT)
+                break;
+            else
+                return false;
+        case MSG_TYPE_PER:
+            if (m->flags & FLAG_MSGTYPE_PER)
+                break;
+            else
+                return false;
+        case MSG_TYPE_PINGPONG:
+            if (m->flags & FLAG_MSGTYPE_PING)
+                break;
+            else
+                return false;
+    }
+
+    if (di->itemType == _ITEM_DROPDOWN) {
+        if (di->selectable_strs == NULL || di->selectable_strs[0] == NULL) {
+            log_printf("NULLstrs%u,%u\r\n", table_row, table_col);
+            return false;
+        }
+    } else if (di->itemType == _ITEM_VALUE) {
+        const value_item_t* vi = (const value_item_t*)m->itemPtr;
+        if (vi->write == NULL)
+            return false;
+    } else if (di->itemType == _ITEM_BUTTON) {
+        const button_item_t* bi = (const button_item_t*)m->itemPtr;
+        if (bi->push == NULL)
+            return false;
+    } else if (di->itemType == _ITEM_TOGGLE) {
+        const toggle_item_t * ti = (const toggle_item_t *)m->itemPtr;
+        if (ti->push == NULL)
+            return false;
+    }
+
+    return true;
+} // ..is_menu_item_changable()
+
+void read_menu_item(const menu_t* m, bool selected)
+{
+    uint8_t valCol;
+    const dropdown_item_t* di = (const dropdown_item_t*)m->itemPtr;
+
+    switch (msg_type) {
+        case MSG_TYPE_PACKET:
+            if (m->flags & FLAG_MSGTYPE_PKT)
+                break;
+            else
+                return;
+        case MSG_TYPE_PER:
+            if (m->flags & FLAG_MSGTYPE_PER)
+                break;
+            else
+                return;
+        case MSG_TYPE_PINGPONG:
+            if (m->flags & FLAG_MSGTYPE_PING)
+                break;
+            else
+                return;
+    }
+
+    pc.printf("\e[%u;%uf", m->pos.row, m->pos.col);  // set (force) cursor to row;column
+    valCol = m->pos.col;
+    if (m->label) {
+        pc.printf(m->label);
+        valCol += strlen(m->label);
+    }
+    if (di->itemType == _ITEM_DROPDOWN) {
+        if (di->read && di->printed_strs) {
+            uint8_t ridx = di->read(false);
+            if (selected)
+                pc.printf("\e[7m");
+            pc.printf(di->printed_strs[ridx]);
+            if (selected)
+                pc.printf("\e[0m");
+        } else if (di->printed_strs) {
+            pc.printf(di->printed_strs[0]);
+        }
+    } else if (di->itemType == _ITEM_VALUE) {
+        const value_item_t* vi = (const value_item_t*)m->itemPtr;
+        if (vi->print) {
+            for (unsigned n = 0; n < vi->width; n++)
+                pc.putc(' ');
+
+            pc.printf("\e[%u;%uf", m->pos.row, valCol);  // set (force) cursor to row;column
+            if (selected)
+                pc.printf("\e[7m");
+            vi->print();
+            if (selected)
+                pc.printf("\e[0m");
+        }
+    } else if (di->itemType == _ITEM_BUTTON) {
+        const button_item_t* bi = (const button_item_t*)m->itemPtr;
+        if (bi->label) {
+            if (selected)
+                pc.printf("\e[7m%s\e[0m", bi->label);
+            else
+                pc.printf("%s", bi->label);
+        }
+    } else if (di->itemType == _ITEM_TOGGLE) {
+        const toggle_item_t* ti = (const toggle_item_t *)m->itemPtr;
+        bool on = ti->read();
+        if (ti->label1) {
+            const char* const cptr = on ? ti->label1 : ti->label0;
+            if (selected)
+                pc.printf("\e[7m%s\e[0m", cptr);
+            else
+                pc.printf("%s", cptr);
+        } else {
+            if (on)
+                pc.printf("\e[1m");
+            if (selected)
+                pc.printf("\e[7m");
+
+            pc.printf("%s", ti->label0);
+
+            if (selected || on)
+                pc.printf("\e[0m");
+        }
+    }
+} // ..read_menu_item()
+
+void draw_menu()
+{
+    unsigned table_row;
+
+    for (table_row = 0; table_row < MAX_MENU_ROWS; table_row++) {
+        int table_col;
+        for (table_col = 0; table_col < StopMenuCols[table_row]; table_col++) {
+            read_menu_item(menu_table[table_row][table_col], false);
+        } // ..table column iterator
+    } // ..table row iterator
+
+    read_menu_item(menu_table[curpos.row][curpos.tableCol], true);
+
+} // ..draw_menu()
+
+typedef struct {
+    int row;
+    int col;
+} tablexy_t;
+
+void
+menu_init_(const menu_t* in, tablexy_t* tc)
+{
+    unsigned n;
+
+    for (n = 0; in[n].pos.row > 0; n++) {
+        const menu_t* m = &in[n];
+        if (tc->row != m->pos.row - 1) {
+            tc->row = m->pos.row - 1;
+            tc->col = 0;
+        } else
+            tc->col++;
+
+        menu_table[tc->row][tc->col] = m;
+#ifdef MENU_DEBUG
+        pc.printf("table:%u,%u ", tc->row, tc->col);
+        pc.printf(" %d<%d? ", StopMenuCols[tc->row], tc->col);
+#endif /* MENU_DEBUG */
+        if (StopMenuCols[tc->row] < tc->col)
+            StopMenuCols[tc->row] = tc->col;
+#ifdef MENU_DEBUG
+        pc.printf("{%u %u}", tc->row, tc->col);
+        pc.printf("in:%p[%u] screen:%u,%u ", in, n, m->pos.row, m->pos.col);
+        //pc.printf(" loc:%p ", &in[n].itemPtr);
+        if (in[n].itemPtr) {
+            const dropdown_item_t* di = (const dropdown_item_t*)m->itemPtr;
+            pc.printf(" itemPtr:%p type:%02x ", di, di->itemType);
+        }
+        pc.printf("stopMenuCols[%u]: %d ", tc->row, StopMenuCols[tc->row]);
+        if (m->label)
+            pc.printf("label:%s", m->label);
+        else
+            pc.printf("noLabel");
+        pc.printf("\r\n");
+#endif /* MENU_DEBUG */
+    }
+#ifdef MENU_DEBUG
+    pc.printf("hit key:");
+    wait_uart_rx(); //pc.getc();
+    
+#endif /* MENU_DEBUG */
+
+}
+
+void navigate_dropdown(uint8_t ch)
+{
+    unsigned n;
+    const menu_t* m = menuState.sm;
+    const dropdown_item_t* di = (const dropdown_item_t*)m->itemPtr;
+
+    switch (ch) {
+        case 'A':   // cursor UP
+            if (menuState.sel_idx > 0) {
+                menuState.sel_idx--;
+            }
+            break;
+        case 'B':   // cursor DOWN
+            if (di->selectable_strs[menuState.sel_idx+1] != NULL)
+                menuState.sel_idx++;
+            break;
+    } // ..switch (ch)
+
+    for (n = 0; di->selectable_strs[n] != NULL; n++) {
+        pc.printf("\e[%u;%uf", m->pos.row+n, menuState.dropdown_col);
+        if (n == menuState.sel_idx)
+            pc.printf("\e[7m");
+        pc.printf(di->selectable_strs[n]);
+        if (n == menuState.sel_idx)
+            pc.printf("\e[0m");
+    }
+    pc.printf("\e[%u;%uf", m->pos.row + menuState.sel_idx, menuState.dropdown_col + strlen(di->selectable_strs[menuState.sel_idx]));
+}
+
+bool is_item_selectable(const menu_t* m)
+{
+    const dropdown_item_t* di = (const dropdown_item_t*)m->itemPtr;
+
+    if (di->itemType == _ITEM_BUTTON) {
+        const button_item_t* bi = (const button_item_t*)m->itemPtr;
+        if (bi->push == NULL)
+            return false;
+    } else if (di->itemType == _ITEM_TOGGLE) {
+        const toggle_item_t* ti = (const toggle_item_t*)m->itemPtr;
+        if (ti->push == NULL)
+            return false;
+    }
+
+    return true;
+}
+
+void navigate_menu(uint8_t ch)
+{
+    read_menu_item(menu_table[curpos.row][curpos.tableCol], false);
+
+    switch (ch) {
+        case 'A':   // cursor UP
+            if (curpos.row == 0)
+                break;
+
+            {   // find previous row up with column
+                int8_t row;
+                for (row = curpos.row - 1; row >= 0; row--) {
+                    if (StopMenuCols[row] > -1) {
+                        curpos.row = row;
+                        break;
+                    }
+                }
+                if (row == 0 && StopMenuCols[0] < 0)
+                    break;  // nothing found
+            }
+
+            if (curpos.tableCol >= StopMenuCols[curpos.row]) {
+                curpos.tableCol = StopMenuCols[curpos.row]-1;
+            }
+
+            break;
+        case 'B':   // cursor DOWN
+            if (curpos.row >= MAX_MENU_ROWS)
+                break;
+
+            {   // find next row down with column
+                uint8_t row;
+                for (row = curpos.row + 1; row < MAX_MENU_ROWS; row++) {
+                    if (StopMenuCols[row] != -1) {
+                        curpos.row = row;
+                        break;
+                    }
+                }
+                if (row == MAX_MENU_ROWS-1 && StopMenuCols[row] == -1)
+                    break;  // nothing found
+            }
+
+            if (curpos.tableCol >= StopMenuCols[curpos.row]) {
+                curpos.tableCol = StopMenuCols[curpos.row]-1;
+            }
+
+
+            break;
+        case 'C':    // cursor LEFT
+            if (curpos.tableCol >= StopMenuCols[curpos.row]-1)
+                break;
+
+            { // find next row left with editable
+                uint8_t tcol;
+                for (tcol = curpos.tableCol + 1; tcol < StopMenuCols[curpos.row]; tcol++) {
+                    if (is_menu_item_changable(curpos.row, tcol)) {
+                        curpos.tableCol = tcol;
+                        break;
+                    }
+                }
+            }
+
+            break;
+        case 'D':   // cursor RIGHT
+            if (curpos.tableCol == 0)
+                break;
+
+            {
+                int8_t tcol;
+                for (tcol = curpos.tableCol - 1;  tcol >= 0; tcol--) {
+                    if (is_menu_item_changable(curpos.row, tcol)) {
+                        curpos.tableCol = tcol;
+                        break;
+                    }
+                }
+            }
+
+            break;
+        default:
+            //pc.printf("unhancled-csi:%02x\eE", ch);
+            break;
+    } // ..switch (ch)
+
+    if (!is_item_selectable(menu_table[curpos.row][curpos.tableCol])) {
+        int c;
+        for (c = 0; c < StopMenuCols[curpos.row]; c++) {
+            if (is_item_selectable(menu_table[curpos.row][c])) {
+                curpos.tableCol = c;
+                break;
+            }
+        }
+        if (c == StopMenuCols[curpos.row])
+            return;
+    }
+
+#ifdef MENU_DEBUG
+    log_printf("table:%u,%u screen:%u,%u      \r\n", curpos.row, curpos.tableCol, 
+        menu_table[curpos.row][curpos.tableCol]->pos.row,
+        menu_table[curpos.row][curpos.tableCol]->pos.col
+    );
+#endif /* MENU_DEBUG */
+
+    read_menu_item(menu_table[curpos.row][curpos.tableCol], true);
+} // ..navigate_menu
+
+void commit_menu_item_change()
+{
+    const menu_t* m = menu_table[curpos.row][curpos.tableCol];
+    const dropdown_item_t* di = (const dropdown_item_t*)m->itemPtr;
+
+    if (di->itemType == _ITEM_DROPDOWN) {
+        menuState.mode = di->write(menuState.sel_idx);
+
+        pc.printf("\e[%u;%uf", m->pos.row, m->pos.col-2);
+    } else if (di->itemType == _ITEM_VALUE) {
+        const value_item_t* vi = (const value_item_t*)m->itemPtr;
+        /* commit value entry */
+        if (vi->write) {
+            if (vi->write(entry_buf))
+                menuState.mode = MENUMODE_REDRAW;
+            else
+                menuState.mode = MENUMODE_NONE;
+        } else
+            menuState.mode = MENUMODE_NONE;
+
+        if (menuState.mode == MENUMODE_NONE) {
+            read_menu_item(menu_table[curpos.row][curpos.tableCol], true);
+        }
+    }
+} // ..commit_menu_item_change()
+
+void refresh_item_in_table(const void* item)
+{
+    unsigned table_row;
+
+    if (item == NULL)
+        return;
+
+    for (table_row = 0; table_row < MAX_MENU_ROWS; table_row++) {
+        int table_col;
+        for (table_col = 0; table_col < StopMenuCols[table_row]; table_col++) {
+            //log_printf("%u %u %p\r\n", table_row, table_col, menu_table[table_row][table_col]->itemPtr);
+            if (item == menu_table[table_row][table_col]->itemPtr) {
+                read_menu_item(menu_table[table_row][table_col], false);
+                return;
+            }
+        }
+    }
+}
+
+void
+start_value_entry(const menu_t* m)
+{
+    const value_item_t* vi = (const value_item_t*)m->itemPtr;
+    uint8_t col = m->pos.col;
+
+    if (m->label)
+        col += strlen(m->label);
+
+    pc.printf("\e[%u;%uf", m->pos.row, col);
+    for (unsigned i = 0; i < vi->width; i++)
+        pc.putc(' ');   // clear displayed value for user entry
+
+    pc.printf("\e[%u;%uf", m->pos.row, col);
+    menuState.mode = MENUMODE_ENTRY;
+    entry_buf_idx = 0;
+}
+
+void start_menu_item_change()
+{
+    const menu_t* m = menu_table[curpos.row][curpos.tableCol];
+    const dropdown_item_t* di = (const dropdown_item_t*)m->itemPtr;
+    bool checkRefresh = false;
+
+    if (di->itemType == _ITEM_DROPDOWN && di->selectable_strs) {
+        menuState.dropdown_col = m->pos.col;
+        unsigned n, sidx = 0;
+        /* start dropdown */
+        if (di->read)
+            sidx = di->read(true);
+
+        if (m->label)
+            menuState.dropdown_col += strlen(m->label);
+
+        for (n = 0; di->selectable_strs[n] != NULL; n++) {
+            uint8_t col = menuState.dropdown_col;
+            bool leftPad = false;
+            if (col > 3 && n > 0) { // dropdown left side padding
+                col -= 2;
+                leftPad = true;
+            }
+            pc.printf("\e[%u;%uf", m->pos.row+n, col);
+            if (leftPad ) {
+                pc.putc(' ');
+                pc.putc(' ');
+            }
+            if (n == sidx)
+                pc.printf("\e[7m");
+            pc.printf(di->selectable_strs[n]);
+            if (n == sidx)
+                pc.printf("\e[0m");
+            pc.putc(' ');   // right side padding
+            pc.putc(' ');
+        }
+        pc.printf("\e[%u;%uf", m->pos.row, menuState.dropdown_col-2);
+
+        menuState.mode = MENUMODE_DROPDOWN;
+        menuState.sel_idx = sidx;
+        menuState.sm = m;
+    } else if (di->itemType == _ITEM_VALUE) {
+        /* start value entry */
+        start_value_entry(m);
+    } else if (di->itemType == _ITEM_BUTTON) {
+        const button_item_t* bi = (const button_item_t*)m->itemPtr;
+        if (bi->push) {
+            bi->push();
+            checkRefresh = true;
+        }
+    } else if (di->itemType == _ITEM_TOGGLE) {
+        const toggle_item_t* ti = (const toggle_item_t*)m->itemPtr;
+        if (ti->push) {
+            bool on = ti->push();
+            uint8_t col = m->pos.col;
+
+            if (m->label)
+                col += strlen(m->label);
+
+            pc.printf("\e[%u;%uf", m->pos.row, col);
+            if (ti->label1) {
+                pc.printf("\e[7m%s\e[0m", on ? ti->label1 : ti->label0);
+            } else {
+                if (on)
+                    pc.printf("\e[1;7m%s\e[0m", ti->label0);
+                else
+                    pc.printf("\e[7m%s\e[0m", ti->label0);
+            }
+            checkRefresh = true;
+        }
+    }
+
+    if (checkRefresh) {
+        if (m->refreshReadItem) {
+            refresh_item_in_table(m->refreshReadItem);  // read associated
+            read_menu_item(m, true);   // restore cursor
+        }
+    }
+} // ..start_menu_item_change()
+
+void full_menu_init()
+{
+    unsigned n;
+    const menu_t *m;
+    tablexy_t txy;
+
+    txy.row = INT_MAX;
+    txy.col = 0;
+
+    for (n = 0; n < MAX_MENU_ROWS; n++) {
+        StopMenuCols[n] = -1;
+    }
+
+    menu_init_(common_menu, &txy);
+
+    menu_init_(Radio::common_menu, &txy);
+
+    m = Radio::get_modem_menu();
+    if (m == NULL) {
+        log_printf("NULL-modemMenu\r\n");
+        for (;;) asm("nop");
+    }
+#ifdef MENU_DEBUG
+    pc.printf("modemmenuInit\r\n");
+#endif
+    menu_init_(m, &txy);
+
+    m = Radio::get_modem_sub_menu();
+    if (m) {
+#ifdef MENU_DEBUG
+        pc.printf("modemsubmenuInit\r\n");
+#endif
+        menu_init_(m, &txy);
+    }
+#ifdef MENU_DEBUG
+    else
+        pc.printf("no-modemsubmenu\r\n");
+#endif
+
+#ifdef PHOTOS_PIN
+    menu_init_(photos_menu, &txy);
+#endif /* PHOTOS_PIN */
+    menu_init_(lis12dh12_menu, &txy);
+    menu_init_(lps22hh_menu, &txy);
+
+    m = get_msg_menu();
+    if (m == NULL) {
+        log_printf("NULL-msgMenu\r\n");
+        for (;;) asm("nop");
+    }
+    menu_init_(m, &txy);
+
+    for (n = 0; n < MAX_MENU_ROWS; n++) {
+        if (StopMenuCols[n] != -1)
+            StopMenuCols[n]++;
+    }
+}
+
+bool ishexchar(char ch)
+{
+    if (((ch >= 'a' && ch <= 'f') || (ch >= 'A' && ch <= 'F')) || (ch >= '0' && ch <= '9'))
+        return true;
+    else
+        return false;
+}
+
+enum _urx_ {
+    URX_STATE_NONE = 0,
+    URX_STATE_ESCAPE,
+    URX_STATE_CSI,
+} uart_rx_state;
+
+Timeout uartRxTimeout;
+
+void uart_rx_timeout()
+{
+    /* escape by itself: abort change on item */
+    menuState.mode = MENUMODE_REDRAW;
+
+    uart_rx_state = URX_STATE_NONE;
+}
+
+void serial_callback(char ch)
+{
+    switch (uart_rx_state) {
+        case URX_STATE_NONE:
+            if (ch == 0x1b) {
+                if (menuState.mode == MENUMODE_ENTRY) {
+                    /* abort entry mode */
+                    menuState.mode = MENUMODE_NONE;
+                    read_menu_item(menu_table[curpos.row][curpos.tableCol], true);
+                } else {
+                    uart_rx_state = URX_STATE_ESCAPE;
+                    if (menuState.mode != MENUMODE_NONE) {
+                        /* is this escape by itself, user wants to abort? */
+                        uartRxTimeout.attach(uart_rx_timeout, 0.03);
+                    }
+                }
+            } else if (ch == 2) { // ctrl-B
+                log_printf("--------------\r\n");
+            } else if (ch == '\r') {
+                if (menuState.mode == MENUMODE_NONE) {
+                    start_menu_item_change();
+                } else {
+                    entry_buf[entry_buf_idx] = 0;
+                    commit_menu_item_change();
+                }
+            } else if (menuState.mode == MENUMODE_ENTRY) {
+                if (ch == 8) {
+                    if (entry_buf_idx > 0) {
+                        pc.putc(8);
+                        pc.putc(' ');
+                        pc.putc(8);
+                        entry_buf_idx--;
+                    }
+                } else if (ch == 3) {    // ctrl-C
+                    menuState.mode = MENUMODE_NONE;
+                } else if (entry_buf_idx < sizeof(entry_buf)) {
+                    entry_buf[entry_buf_idx++] = ch;
+                    pc.putc(ch);
+                }
+            } else if (menuState.mode == MENUMODE_NONE) {
+                if (ishexchar(ch) || ch == '-') {   // characters which start entry
+                    const value_item_t* vi = (const value_item_t*)menu_table[curpos.row][curpos.tableCol]->itemPtr;
+                    if (vi->itemType == _ITEM_VALUE) {
+                        start_value_entry(menu_table[curpos.row][curpos.tableCol]);
+                        entry_buf[entry_buf_idx++] = ch;
+                        pc.putc(ch);
+                    }
+                } else if (ch == 'r') {
+                    menuState.mode = MENUMODE_REDRAW;
+                } else if (ch == '.') {
+                    Radio::test();
+                }
+
+            }
+            break;
+        case URX_STATE_ESCAPE:
+            uartRxTimeout.detach();
+            if (ch == '[')
+                uart_rx_state = URX_STATE_CSI;
+            else {
+#ifdef MENU_DEBUG
+                log_printf("unhancled-esc:%02x\r\n", ch);
+#endif /* MENU_DEBUG */
+                uart_rx_state = URX_STATE_NONE;
+            }
+            break;
+        case URX_STATE_CSI:
+            if (menuState.mode == MENUMODE_NONE)
+                navigate_menu(ch);
+            else if (menuState.mode == MENUMODE_DROPDOWN)
+                navigate_dropdown(ch);
+
+            uart_rx_state = URX_STATE_NONE;
+            //pc.printf("\e[18;1f");  // set (force) cursor to row;column
+            break;
+    } // ..switch (uart_rx_state)
+}
+
+void ev_uart_rx()
+{
+    log_printf("ev_uart_rx %u %u\r\n", uart_rx_buf_in, uart_rx_buf_out);
+    while (uart_rx_buf_in != uart_rx_buf_out) {
+        log_printf("X ev_uart_rx %u %u\r\n", uart_rx_buf_in, uart_rx_buf_out);
+        serial_callback(uart_rx_buf[uart_rx_buf_out++]);
+        if (uart_rx_buf_out == UART_RX_BUF_LEN)
+            uart_rx_buf_out = 0;
+    }
+}
+
+//volatile unsigned rxCnt = 0;
+void rx_isr()
+{
+    //rxCnt++;
+    uart_rx_buf[uart_rx_buf_in++] = pc.getc();
+    if (uart_rx_buf_in == UART_RX_BUF_LEN)
+        uart_rx_buf_in = 0;
+
+    //queue.call(ev_uart_rx);
+}
+
+
+void txDone()
+{
+
+    if (msg_type == MSG_TYPE_PER) {
+        log_printf("CntPacketTx%u, max:%u  ipd%u\r\n", CntPacketTx, MaxNumPacket, tx_ipd_ms);
+        if (++CntPacketTx <= MaxNumPacket)
+            mbedTimeout.attach_us(next_tx_callback, tx_ipd_ms * 1000);
+    } else if (msg_type == MSG_TYPE_PINGPONG) {
+        if (flags.ping_master) {
+            ++CntPacketTx;
+        }
+
+        Radio::Rx();
+    }
+}
+
+static void
+printRxPkt(uint8_t size)
+{
+    char str[80];
+    char *ptr, *endPtr;
+    unsigned n = 0;
+    endPtr = str + sizeof(str);
+    ptr = str;
+    while (ptr < endPtr) {
+        sprintf(ptr, "%02x ", Radio::radio.rx_buf[n]);
+        ptr += 3;
+        if (++n >= size)
+            break;
+    }
+    log_printf("%s\r\n", str);
+}
+
+void rxDone(uint8_t size, float rssi, float snr)
+{
+    log_printf("rxDone %u, %.1fdBm  %.1fdB\r\n", size, rssi, snr);
+    if (msg_type == MSG_TYPE_PACKET) {
+        switch (Radio::radio.rx_buf[0]) {
+            displayFloatToInt_t val;
+            case CMD_TEMP:
+                memcpy(&val, &Radio::radio.rx_buf[1], sizeof(displayFloatToInt_t));
+                log_printf("TEMP: %c%d.%02d\r\n", ((val.sign) ? '-' : '+'),
+                    (int)val.out_int, (int)val.out_dec);
+                break;
+            case CMD_PRES:
+                memcpy(&val, &Radio::radio.rx_buf[1], sizeof(displayFloatToInt_t));
+                log_printf("PRESS: %c%d.%02d\r\n", ((val.sign) ? '-' : '+'),
+                    (int)val.out_int, (int)val.out_dec);
+                break;
+            case CMD_ACCEL:
+                SensorAxes_t acc;
+                memcpy(&acc, &Radio::radio.rx_buf[1], sizeof(SensorAxes_t));
+                log_printf("ACC  %5ld  %5ld  %5ld\r\n", acc.AXIS_X, acc.AXIS_Y, acc.AXIS_Z);
+                break;
+            case CMD_PHOTOS:
+                uint16_t pv;
+                memcpy(&pv, &Radio::radio.rx_buf[1], sizeof(uint16_t));
+                log_printf("photos: %u\r\n", pv);
+                break;
+            default:
+                printRxPkt(size);
+                break;
+        }
+    } else if (msg_type == MSG_TYPE_PER) {
+        if (memcmp(Radio::radio.rx_buf+4, PerMsg, 3) == 0) {
+            unsigned i, PacketRxSequence = Radio::radio.rx_buf[0];
+            PacketRxSequence <<= 8;
+            PacketRxSequence += Radio::radio.rx_buf[1];
+            PacketRxSequence <<= 8;
+            PacketRxSequence += Radio::radio.rx_buf[2];
+            PacketRxSequence <<= 8;
+            PacketRxSequence += Radio::radio.rx_buf[3];
+
+            CntPacketRxOK++;
+
+            if (PacketRxSequence <= PacketRxSequencePrev || PacketRxSequencePrev == 0)
+                i = 0;  // sequence reset to resync, dont count missed packets this time
+            else
+                i = PacketRxSequence - PacketRxSequencePrev - 1;
+
+
+            CntPacketRxKO += i;
+            RxTimeOutCount = 0;
+            log_printf("PER rx%u  ok%u   ko%u\r\n", PacketRxSequence , CntPacketRxOK, CntPacketRxKO);
+
+            PacketRxSequencePrev = PacketRxSequence;
+        } // ..if PerMsg
+        else {
+            log_printf("per?\r\n");
+            printRxPkt(size);
+        }
+    } else if (msg_type == MSG_TYPE_PINGPONG) {
+        if (memcmp(Radio::radio.rx_buf+4, PingMsg, 4) == 0) {
+            /* ping slave rx */
+            Radio::setFS();
+            receivedCntPacket = Radio::radio.rx_buf[0];
+            receivedCntPacket <<= 8;
+            receivedCntPacket += Radio::radio.rx_buf[1];
+            receivedCntPacket <<= 8;
+            receivedCntPacket += Radio::radio.rx_buf[2];
+            receivedCntPacket <<= 8;
+            receivedCntPacket += Radio::radio.rx_buf[3];
+            log_printf("%u rxPing->txPong\r\n", receivedCntPacket);
+
+            flags.ping_master = 0;
+            flags.send_pong = 1;
+
+        } else if (memcmp(Radio::radio.rx_buf+8, PongMsg, 4) == 0) {
+            unsigned cnt;
+            /* ping master rx */
+            Radio::setFS();
+            cnt = Radio::radio.rx_buf[0];
+            cnt <<= 8;
+            cnt += Radio::radio.rx_buf[1];
+            cnt <<= 8;
+            cnt += Radio::radio.rx_buf[2];
+            cnt <<= 8;
+            cnt += Radio::radio.rx_buf[3];
+            log_printf("%u rxPong->txPing\r\n", cnt);
+            flags.send_ping = 1;
+        } else {
+            log_printf("pingpong?\r\n");
+            printRxPkt(size);
+        }
+    } else {
+        /*for (unsigned n = 0; n < size; n++)
+            log_printf("%02x\r\n", Radio::radio.rx_buf[n]);*/
+        log_printf("msg_type %u\r\n", msg_type);
+    }
+
+}
+
+const RadioEvents_t rev = {
+    txDone,
+    rxDone
+};
+
+static DrvStatusTypeDef LIS2DH12_Read_Single_FIFO_Data(uint16_t sampleIndex, SensorAxes_t* acceleration)
+{
+  //SensorAxes_t acceleration;
+
+  /* Read single FIFO data (acceleration in 3 axes) */
+  if (lis2dh12_get_axes(acceleration) == COMPONENT_ERROR)
+  {
+    return COMPONENT_ERROR;
+  }
+
+  if (sampleIndex < SAMPLE_LIST_MAX)
+  {
+    log_printf("[DATA %02d]  %5ld  %5ld  %5ld\r\n", sampleIndex + 1, acceleration->AXIS_X,
+             acceleration->AXIS_Y,
+             acceleration->AXIS_Z);
+  }
+
+  return COMPONENT_OK;
+}
+
+void uart_rx_service()
+{
+    while (uart_rx_buf_in != uart_rx_buf_out) {
+        serial_callback(uart_rx_buf[uart_rx_buf_out++]);
+        if (uart_rx_buf_out == UART_RX_BUF_LEN)
+            uart_rx_buf_out = 0;
+    }
+}
+
+static DrvStatusTypeDef LIS2DH12_Read_All_FIFO_Data(void)
+{
+    uint16_t samplesToRead = accel_get_num_samples();
+    SensorAxes_t acc;
+
+    /* 'samplesToRead' actually contains number of words in FIFO but each FIFO sample (data set) consists of 3 words
+    so the 'samplesToRead' has to be divided by 3 */
+    samplesToRead /= 3;
+
+    log_printf("\r\n%d samples in FIFO.\r\n\r\nStarted downloading data from FIFO ...\r\n", samplesToRead);
+
+    //wait_ms(1000);
+
+    log_printf("\r\n[DATA ##]  ACC_X  ACC_Y  ACC_Z  [mg]\r\n");
+
+    for (int i = 0; i < samplesToRead; i++)
+    {
+        uart_rx_service();
+
+        if (LIS2DH12_Read_Single_FIFO_Data(i, &acc) == COMPONENT_ERROR)
+        {
+            return COMPONENT_ERROR;
+        } else {
+        }
+    }
+
+    if (accel_tx_en) {
+        Radio::radio.tx_buf[0] = CMD_ACCEL;
+        Radio::set_payload_length(sizeof(SensorAxes_t)+1);
+        memcpy(&Radio::radio.tx_buf[1], &acc, sizeof(SensorAxes_t));
+        Radio::txPkt();
+    }
+
+    if (samplesToRead > SAMPLE_LIST_MAX)
+    {
+        log_printf("\r\nSample list limited to: %d\r\n", SAMPLE_LIST_MAX);
+    }
+
+    return COMPONENT_OK;
+}
+
+
+int main()
+{
+
+    lfsr = LFSR_INIT;
+    msg_type = MSG_TYPE_PACKET;
+
+    uart_rx_state = URX_STATE_NONE;
+    pc.attach(rx_isr);
+
+    Radio::boardInit(&rev);
+
+    {
+        unsigned n;
+        for (n = 0; n < MAX_MENU_ROWS; n++)
+            StopMenuCols[n] = -1;
+    }
+
+    botRow = MAX_MENU_ROWS + SCROLLING_ROWS;
+
+    pc.baud(115200);
+    pc.printf("\e[7h"); // enable line wrapping
+    pc.printf("\e[%u;%ur", MAX_MENU_ROWS, botRow); // set scrolling region
+    pc.printf("\e[2J"); // erase entire screen
+
+    full_menu_init();
+
+    pc.printf("\e[2J"); // erase entire screen
+
+    menuState.mode = MENUMODE_NONE;
+
+    draw_menu();
+
+    curpos.row = 0;
+    curpos.tableCol = 0;
+
+    tx_ipd_ms = 100;
+
+    if (demo_start()) {
+        log_printf("demo_start-Failed\r\n");
+        for (;;) { asm("nop"); }
+    } else
+        log_printf("demo_start-OK\r\n");
+
+
+    svc_lis2dh12 = true;
+
+    for (;;) {
+        int ret;
+
+        uart_rx_service();
+
+        if (flags.send_ping) {
+            if (flags.pingpongEnable)
+                SendPing();
+            flags.send_ping = 0;
+        }
+
+        if (flags.send_pong) {
+            if (flags.pingpongEnable)
+                SendPong();
+            flags.send_pong = 0;
+        }
+
+        if (flags.do_next_tx) {
+            do_next_tx();
+            flags.do_next_tx = 0;
+        }
+
+        if (menuState.mode == MENUMODE_REINIT_MENU) {
+            full_menu_init();
+            menuState.mode = MENUMODE_REDRAW;
+        }
+
+        if (menuState.mode == MENUMODE_REDRAW) {
+            // erase entire screen, some dropdowns extend to scrolling area
+            pc.printf("\e[%u;%ur", MAX_MENU_ROWS, botRow); // set scrolling region, if terminal started after
+            pc.printf("\e[2J");
+            //pc.printf("\e[%u;1f\e[1J", MAX_MENU_ROWS);  // erase menu area
+
+            menuState.mode = MENUMODE_NONE;
+            draw_menu();
+        }
+
+        if (Radio::service(menuState.mode == MENUMODE_NONE ? LAST_CHIP_MENU_ROW : -1)) {
+            read_menu_item(menu_table[curpos.row][curpos.tableCol], true);
+        }
+
+        if (svc_lis2dh12) {
+            ret = lis2dh_mainloop();
+            switch (ret) {
+                case LIS2DH_FAIL_STATE:
+                    log_printf("lis2dh-stateFail\r\n");
+                    break;
+                case LIS2DH_FAIL:
+                    log_printf("lis2dh-Fail\r\n");
+                    break;
+                case LIS2DH_BSP_FAIL:
+                    log_printf("lis2dh bsp-fail\r\n");
+                    wait(0.05);
+                    break;
+                case LIS2DH_MAIN_SLEEP:
+                    lis2dh_set_fifo_mode();
+
+                    log_printf("lis2dh fifo-mode-sleep\r\n");
+                    svc_lis2dh12 = false;
+                    break;
+                case LIS2DH_MAIN_READ_FIFO:
+                    if (LIS2DH12_Read_All_FIFO_Data() == COMPONENT_ERROR)
+                    {
+                        return LIS2DH_FAIL;
+                    }
+
+                    /* Reset FIFO by setting FIFO mode to Bypass */
+                    if (lis2dh_set_fifo_bypass() < 0) {
+                        log_printf("fifo-bypass-fail\r\n");
+                    }
+                    break;
+                default:
+                    break;
+            }
+        } // ..if (svc_lis2dh12)
+        else {
+        }
+
+    } // ..for (;;)
+}
+
+void lis2dh_int1()
+{
+    log_printf("\r\nReceived FIFO Threshold Interrupt on INT1 pin ...\r\n"
+               "\r\nNucleo processor is waking up ...\r\n"
+    );
+
+    svc_lis2dh12 = true;
+}
+
+char strbuf[255];
+
+void c_log_printf(const char* format, ...)
+{
+    va_list arglist;
+
+    // put cursor at last scrolling-area line
+    pc.printf("\e[%u;1f", botRow);
+    va_start(arglist, format);
+    vsnprintf(strbuf, sizeof(strbuf), format, arglist);
+    va_end(arglist);
+
+    pc.printf(strbuf);
+}
+
+void log_printf(const char* format, ...)
+{
+    va_list arglist;
+
+    // put cursor at last scrolling-area line
+    pc.printf("\e[%u;1f", botRow);
+    va_start(arglist, format);
+    vsnprintf(strbuf, sizeof(strbuf), format, arglist);
+    va_end(arglist);
+
+    pc.printf(strbuf);
+}
+