File content as of revision 2:972a5704f152:

#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,
    CMD_NONE
};

#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 = MSG_TYPE_PACKET;

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
    uint8_t txBusy          : 1; // 5
    uint8_t measure_tx      : 1; // 5
} flags;

uint8_t botRow;
int regAddr = -1;

#define PHOTOS_PIN          PA_0
/*
 * 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
*/
#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) {
        asm("nop");
    }

    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 },
    { {0, 0}, NULL, NULL }
};


#ifdef PHOTOS_PIN
void photos_push()
{
    uint16_t val = photos.read_u16();
    log_printf("photos %u\r\n", val);
}

void photos_tx()
{
    uint16_t val;
    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));

    flags.txBusy = true;
    Radio::txPkt();
}

unsigned pollTickerRate;
Ticker pollTicker;
uint8_t enabledSensor = CMD_NONE;

void tx_photos_push()
{
    if (pollTickerRate > 0) {
        if (enabledSensor != CMD_PHOTOS)
            enabledSensor = CMD_PHOTOS;
        else {
            enabledSensor = CMD_NONE;
        }
        return;
    }

    photos_tx();
}

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 },
    { {0, 0}, NULL, NULL }
};
#endif /* PHOTOS_PIN */

void temperature_push()
{
    displayFloatToInt_t val;
    demo_sample_temp(&val);
}

void pressure_push()
{
    displayFloatToInt_t val;
    demo_sample_pressure(&val);
}

void temp_tx()
{
    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));

    flags.txBusy = true;
    Radio::txPkt();
}

void tx_temp_push()
{
    if (pollTickerRate > 0) {
        if (enabledSensor != CMD_TEMP)
            enabledSensor = CMD_TEMP;
        else {
            enabledSensor = CMD_NONE;
        }
        return;
    }

    temp_tx();
}

void pres_tx()
{
    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));

    flags.txBusy = true;
    Radio::txPkt();
}

void tx_pressure_push()
{
    if (pollTickerRate > 0) {
        if (enabledSensor != CMD_PRES)
            enabledSensor = CMD_PRES;
        else {
            enabledSensor = CMD_NONE;
        }
        return;
    }

    pres_tx();
}

void sensorPoll()
{
    if (!flags.txBusy && menuState.mode == MENUMODE_NONE)
        flags.measure_tx = 1;
}

void pollRate_print()
{
    pc.printf("%ums", pollTickerRate / 1000);
}

bool pollRate_write(const char* str)
{
    sscanf(str, "%u", &pollTickerRate);

    if (pollTickerRate > 0) {
        pollTickerRate *= 1000;
        pollTicker.attach_us(sensorPoll, pollTickerRate);
    } else
        pollTicker.detach();

    log_printf("pollTickerRate %uus\r\n", pollTickerRate);

    return false;
}

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 value_item_t pollRate_item = { _ITEM_VALUE, 8, pollRate_print, pollRate_write };

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 },
    { {LAST_CHIP_MENU_ROW, 53}, "poll rate:", &pollRate_item, FLAG_MSGTYPE_ALL },
    { {0, 0}, NULL, NULL }
};

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);   // accel
    menu_init_(lps22hh_menu, &txy);     // temp, pressure

    m = get_msg_menu();
    if (m == NULL) {
        log_printf("NULL-msgMenu %d\r\n", msg_type);
        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();
    }

    flags.txBusy = false;
}

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);

    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();
    //wait_uart_rx(); //pc.getc();

    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 {
        }

        if (flags.measure_tx) {
            switch (enabledSensor) {
                case CMD_PHOTOS:
                    photos_tx();
                    break;
                case CMD_PRES:
                    pres_tx();
                    break;
                case CMD_TEMP:
                    temp_tx();
                    break;
            }
            flags.measure_tx = 0;
        }

    } // ..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);
}