bool dispatch(Msg& msg) {
PT_BEGIN()
_gpio.init();
_gpio.setMode(Gpio::OUTPUT_PP);
while (true) {
timeout(_msecInterval);
PT_YIELD_UNTIL(
msg.is(_mqtt, SIG_CONNECTED) || msg.is(_mqtt, SIG_DISCONNECTED)
|| timeout());
switch (msg.signal) {
case SIG_TICK: {
_gpio.write(_isOn);
_isOn = !_isOn;
break;
}
case SIG_CONNECTED: {
_msecInterval = 500;
break;
}
case SIG_DISCONNECTED: {
_msecInterval = 100;
break;
}
default: {
}
}
}
PT_END()
;
}
IROM bool LedBlink::dispatch(Msg& msg) {
PT_BEGIN()
PT_WAIT_UNTIL(msg.is(0, SIG_INIT));
init();
while (true) {
timeout(_msecInterval);
PT_YIELD_UNTIL(
msg.is(_src, SIG_CONNECTED) || msg.is(_src, SIG_DISCONNECTED)
|| timeout());
switch (msg.signal()) {
case SIG_TICK: {
gpio16_output_set(_isOn);
_isOn = !_isOn;
break;
}
case SIG_CONNECTED: {
_msecInterval = 1000;
break;
}
case SIG_DISCONNECTED: {
_msecInterval = 200;
break;
}
default: {
}
}
}
PT_END();
return false;
}
bool dispatch(Msg& msg)
{
PT_BEGIN ( );
while(true)
{
PT_YIELD_UNTIL( msg.is(_tcp,SIG_RXD) || msg.is(_usb,SIG_RXD) );
if ( msg.is(_tcp,SIG_RXD))
{
MqttIn* mqttIn = (MqttIn*)msg.data;
Str str(256);
str << "MQTT TCP->USB:";
mqttIn->toString(str);
logger.info()<< str;
logger.flush();
usb.send(*mqttIn->getBytes());
}
else if ( msg.is(_usb,SIG_RXD))
{
MqttIn* mqttIn = (MqttIn*)msg.data;
Str str(256);
str << "MQTT USB->TCP:";
mqttIn->toString(str);
logger.info()<< str;
logger.flush();
if ( _tcp->isConnected() )
{
if ( mqttIn->type() == MQTT_MSG_CONNECT ) // simulate a reply
{
MqttOut m(10);
m.ConnAck(0);
// uint8_t CONNACK[]={0x20,0x02,0x00,0x00};
logger.info()<< "CONNACK virtual,already tcp connected";
logger.flush();
usb.send(m);
}
else
{
tcp.send(*mqttIn->getBytes());
}
}
else
{
if ( mqttIn->type() == MQTT_MSG_CONNECT )
{
tcp.connect();
tcp.send(*mqttIn->getBytes());
}
else
{
logger.info()<< "dropped packet, not connected.";
logger.flush();
usb.disconnect();
}
}
}
}
PT_END ( );
}
bool dispatch ( Msg& msg )
{
PT_BEGIN ( );
while(true)
{
while ( usb.connect() != E_OK )
{
wakeTime=Sys::upTime()+5000;
PT_YIELD_UNTIL ( wakeTime < Sys::upTime()) ;
}
PT_YIELD_UNTIL ( msg.is(_usb,SIG_DISCONNECTED,usb.fd(),0) );
}
PT_END ( );
}
bool DWM1000_Anchor::dispatch(Msg& msg) {
PT_BEGIN()
PT_WAIT_UNTIL(msg.is(0, SIG_INIT));
init();
while (true) {
WAIT_POLL: {
dwt_setrxtimeout(0); /* Clear reception timeout to start next ranging process. */
dwt_rxenable(0); /* Activate reception immediately. */
// dwt_setinterrupt(DWT_INT_RFCG, 1); // enable RXD interrupt
while (true) { /* Poll for reception of a frame or error/timeout. See NOTE 7 below. */
timeout(1000);/* This is the delay from the end of the frame transmission to the enable of the receiver, as programmed for the DW1000's wait for response feature. */
clearInterrupt();
PT_YIELD_UNTIL(timeout() || isInterruptDetected());
status_reg = _status_reg;
LOG<< HEX << " status reg.:" << status_reg << " ,interrupts : " << interruptCount << FLUSH;
status_reg = dwt_read32bitreg(SYS_STATUS_ID);
LOG<< HEX << " IRQ pin : " << digitalRead(D2) << " status_reg DWM1000 " << status_reg << FLUSH;// PULL LOW
if (status_reg & (SYS_STATUS_RXFCG | SYS_STATUS_ALL_RX_ERR))
break;
}
}
///____________________________________________________________________________
if (status_reg & SYS_STATUS_RXFCG) {
LOG<< " $ "<<FLUSH;
uint32 frame_len;
dwt_write32bitreg(SYS_STATUS_ID, SYS_STATUS_RXFCG); /* Clear good RX frame event in the DW1000 status register. */
/* A frame has been received, read it into the local buffer. */
frame_len = dwt_read32bitreg(RX_FINFO_ID) & RX_FINFO_RXFL_MASK_1023;
if (frame_len <= RX_BUFFER_LEN) {
dwt_readrxdata(rx_buffer, frame_len, 0);
}
/* Check that the frame is a poll sent by "DS TWR initiator" example.
* As the sequence number field of the frame is not relevant, it is cleared to simplify the validation of the frame. */
rx_buffer[ALL_MSG_SN_IDX] = 0;
if (memcmp(rx_buffer, rx_poll_msg, ALL_MSG_COMMON_LEN) == 0) {
LOG<< " $$ "<<FLUSH;
uint32 resp_tx_time;
poll_rx_ts = get_rx_timestamp_u64(); /* Retrieve poll reception timestamp. */
/* Set send time for response. See NOTE 8 below. */
resp_tx_time = (poll_rx_ts
+ (POLL_RX_TO_RESP_TX_DLY_UUS * UUS_TO_DWT_TIME)) >> 8;
dwt_setdelayedtrxtime(resp_tx_time);
/* Set expected delay and timeout for final message reception. */
dwt_setrxaftertxdelay(RESP_TX_TO_FINAL_RX_DLY_UUS);
dwt_setrxtimeout(FINAL_RX_TIMEOUT_UUS);
/* Write and send the response message. See NOTE 9 below.*/
tx_resp_msg[ALL_MSG_SN_IDX] = frame_seq_nb;
dwt_writetxdata(sizeof(tx_resp_msg), tx_resp_msg, 0);
dwt_writetxfctrl(sizeof(tx_resp_msg), 0);
dwt_starttx(DWT_START_TX_DELAYED | DWT_RESPONSE_EXPECTED);
/* We assume that the transmission is achieved correctly, now poll for reception of expected "final" frame or error/timeout.
* See NOTE 7 below. */
// while (true) { /* Poll for reception of a frame or error/timeout. See NOTE 7 below. */
timeout(10);
dwt_setinterrupt(DWT_INT_RFCG, 1);// enable
clearInterrupt();
// PT_YIELD_UNTIL(timeout() || isInterruptDetected());
status_reg = dwt_read32bitreg(SYS_STATUS_ID);
// status_reg = _status_reg;
LOG<< HEX << " status reg2:" << status_reg << FLUSH;
// if (status_reg & (SYS_STATUS_RXFCG | SYS_STATUS_ALL_RX_ERR))
// break;
// }
// while (!((status_reg = dwt_read32bitreg(SYS_STATUS_ID)) & (SYS_STATUS_RXFCG | SYS_STATUS_ALL_RX_ERR)))
// { };
/* Increment frame sequence number after transmission of the response message (modulo 256). */
frame_seq_nb++;
if (status_reg & SYS_STATUS_RXFCG) {
LOG<< " $$$ "<<FLUSH;
/* Clear good RX frame event and TX frame sent in the DW1000 status register. */
dwt_write32bitreg(SYS_STATUS_ID,
SYS_STATUS_RXFCG | SYS_STATUS_TXFRS);
/* A frame has been received, read it into the local buffer. */
frame_len = dwt_read32bitreg(
RX_FINFO_ID) & RX_FINFO_RXFLEN_MASK;
if (frame_len <= RX_BUF_LEN) {
dwt_readrxdata(rx_buffer, frame_len, 0);
}
/* Check that the frame is a final message sent by "DS TWR initiator" example.
* As the sequence number field of the frame is not used in this example, it can be zeroed to ease the validation of the frame. */
rx_buffer[ALL_MSG_SN_IDX] = 0;
if (memcmp(rx_buffer, rx_final_msg, ALL_MSG_COMMON_LEN)
== 0) {
uint32 poll_tx_ts, resp_rx_ts, final_tx_ts;
uint32 poll_rx_ts_32, resp_tx_ts_32, final_rx_ts_32;
double Ra, Rb, Da, Db;
int64 tof_dtu;
/* Retrieve response transmission and final reception timestamps. */
resp_tx_ts = get_tx_timestamp_u64();
final_rx_ts = get_rx_timestamp_u64();
/* Get timestamps embedded in the final message. */
final_msg_get_ts(&rx_buffer[FINAL_MSG_POLL_TX_TS_IDX],
&poll_tx_ts);
final_msg_get_ts(&rx_buffer[FINAL_MSG_RESP_RX_TS_IDX],
&resp_rx_ts);
final_msg_get_ts(&rx_buffer[FINAL_MSG_FINAL_TX_TS_IDX],
&final_tx_ts);
/* Compute time of flight. 32-bit subtractions give correct answers even if clock has wrapped. See NOTE 10 below. */
poll_rx_ts_32 = (uint32) poll_rx_ts;
resp_tx_ts_32 = (uint32) resp_tx_ts;
final_rx_ts_32 = (uint32) final_rx_ts;
Ra = (double) (resp_rx_ts - poll_tx_ts);
Rb = (double) (final_rx_ts_32 - resp_tx_ts_32);
Da = (double) (final_tx_ts - resp_rx_ts);
Db = (double) (resp_tx_ts_32 - poll_rx_ts_32);
tof_dtu = (int64) ((Ra * Rb - Da * Db)
/ (Ra + Rb + Da + Db));
tof = tof_dtu * DWT_TIME_UNITS;
distance = tof * SPEED_OF_LIGHT;
/* Display computed distance on LCD. */
// char dist_str[20];
// sprintf(dist_str,"%3.2f", distance);
// lcd_display_str(dist_str);
LOG<< " distance : " << (float)distance << "m. " << FLUSH;
}
} else {
/* Clear RX error events in the DW1000 status register. */
dwt_write32bitreg(SYS_STATUS_ID, SYS_STATUS_ALL_RX_ERR);
}
}
} else {
bool Wifi::dispatch(Msg& msg) {
// INFO("line : %d ",_ptLine);
// INFO("msg : %d:%d",msg.src(),msg.signal());
PT_BEGIN();
INIT : {
PT_WAIT_UNTIL(msg.is(0,SIG_INIT));
struct station_config stationConf;
INFO("WIFI_INIT");
if ( wifi_set_opmode(STATION_MODE) ){
; // STATIONAP_MODE was STATION_MODE
INFO("line : %d",__LINE__);
if ( wifi_set_phy_mode(PHY_MODE_11B)) {
os_memset(&stationConf, 0, sizeof(struct station_config));
ets_strncpy((char*)stationConf.ssid,_ssid,sizeof(stationConf.ssid));
ets_strncpy((char*)stationConf.password,_pswd,sizeof(stationConf.password));
stationConf.bssid_set=0;
INFO("line : %d",__LINE__);
if ( wifi_station_set_config(&stationConf) ){
if ( wifi_station_connect() ){
INFO("line : %d",__LINE__);
goto DISCONNECTED;// wifi_station_set_auto_connect(TRUE);
}
}
}
}
// wifi_station_set_auto_connect(FALSE);
INFO(" WIFI INIT failed , retrying... ");
goto INIT;
};
DISCONNECTED: {
while(true) {
timeout(1000);
PT_YIELD_UNTIL(timeout());
struct ip_info ipConfig;
wifi_get_ip_info(STATION_IF, &ipConfig);
wifiStatus = wifi_station_get_connect_status();
if ( wifi_station_get_connect_status()== STATION_NO_AP_FOUND || wifi_station_get_connect_status()==STATION_WRONG_PASSWORD || wifi_station_get_connect_status()==STATION_CONNECT_FAIL)
{
INFO(" NOT CONNECTED ");
wifi_station_connect();
} else if (wifiStatus == STATION_GOT_IP && ipConfig.ip.addr != 0) {
_connections++;
union {
uint32_t addr;
uint8_t ip[4];
} v;
v.addr = ipConfig.ip.addr;
INFO(" IP Address : %d.%d.%d.%d ",v.ip[0],v.ip[1],v.ip[2],v.ip[3]);
INFO(" CONNECTED ");
Msg::publish(this,SIG_CONNECTED);
_connected=true;
timeout(2000);
goto CONNECTED;
} else {
INFO(" STATION_IDLE ");
}
timeout(500);
}
};
CONNECTED : {
while(true) {
PT_YIELD_UNTIL(timeout());
struct ip_info ipConfig;
wifi_get_ip_info(STATION_IF, &ipConfig);
wifiStatus = wifi_station_get_connect_status();
if (wifiStatus != STATION_GOT_IP ) {
Msg::publish(this,SIG_DISCONNECTED);
timeout(500);
_connected=false;
goto DISCONNECTED;
}
timeout(2000);
}
};
PT_END();
}
bool DWM1000_Tag::dispatch(Msg& msg) {
PT_BEGIN()
PT_WAIT_UNTIL(msg.is(0, SIG_INIT));
init();
POLL_SEND: {
while (true) {
timeout(1000); // delay between POLL
PT_YIELD_UNTIL(timeout());
/* Write frame data to DW1000 and prepare transmission. See NOTE 7 below. */
tx_poll_msg[ALL_MSG_SN_IDX] = frame_seq_nb;
dwt_writetxdata(sizeof(tx_poll_msg), tx_poll_msg, 0);
dwt_writetxfctrl(sizeof(tx_poll_msg), 0);
/* Start transmission, indicating that a response is expected so that reception is enabled automatically after the frame is sent and the delay
* set by dwt_setrxaftertxdelay() has elapsed. */
LOG<< " Start TXF " << FLUSH;
dwt_starttx(DWT_START_TX_IMMEDIATE | DWT_RESPONSE_EXPECTED);// SEND POLL MSG
dwt_setinterrupt(DWT_INT_TFRS, 0);
dwt_setinterrupt(DWT_INT_RFCG, 1); // enable
clearInterrupt();
_timeoutCounter = 0;
/* We assume that the transmission is achieved correctly, poll for reception of a frame or error/timeout. See NOTE 8 below. */
timeout(10);
PT_YIELD_UNTIL(timeout() || isInterruptDetected()); // WAIT RESP MSG
if (isInterruptDetected())
LOG<< " INTERRUPT DETECTED " << FLUSH;
status_reg = dwt_read32bitreg(SYS_STATUS_ID);
LOG<< HEX <<" SYS_STATUS " << status_reg << FLUSH;
if (status_reg == 0xDEADDEAD) {
init();
} else if (status_reg & SYS_STATUS_RXFCG)
goto RESP_RECEIVED;
else if (status_reg & SYS_STATUS_ALL_RX_ERR) {
if (status_reg & SYS_STATUS_RXRFTO)
INFO(" RX Timeout");
else
INFO(" RX error ");
dwt_write32bitreg(SYS_STATUS_ID, SYS_STATUS_ALL_RX_ERR); /* Clear RX error events in the DW1000 status register. */
}
}
}
RESP_RECEIVED: {
LOG<< " Received " <<FLUSH;
frame_seq_nb++; /* Increment frame sequence number after transmission of the poll message (modulo 256). */
uint32 frame_len;
/* Clear good RX frame event and TX frame sent in the DW1000 status register. */
dwt_write32bitreg(SYS_STATUS_ID,
SYS_STATUS_RXFCG | SYS_STATUS_TXFRS);
/* A frame has been received, read iCHANGEt into the local buffer. */
frame_len =
dwt_read32bitreg(RX_FINFO_ID) & RX_FINFO_RXFLEN_MASK;
if (frame_len <= RX_BUF_LEN) {
dwt_readrxdata(rx_buffer, frame_len, 0);
}
/* Check that the frame is the expected response from the companion "DS TWR responder" example.
* As the sequence number field of the frame is not relevant, it is cleared to simplify the validation of the frame. */
rx_buffer[ALL_MSG_SN_IDX] = 0;
if (memcmp(rx_buffer, rx_resp_msg, ALL_MSG_COMMON_LEN) == 0) { // CHECK RESP MSG
uint32 final_tx_time;
/* Retrieve poll transmission and response reception timestamp. */
poll_tx_ts = get_tx_timestamp_u64();
resp_rx_ts = get_rx_timestamp_u64();
/* Compute final message transmission time. See NOTE 9 below. */
final_tx_time = (resp_rx_ts
+ (RESP_RX_TO_FINAL_TX_DLY_UUS * UUS_TO_DWT_TIME))
>> 8;
dwt_setdelayedtrxtime(final_tx_time);
/* Final TX timestamp is the transmission time we programmed plus the TX antenna delay. */
final_tx_ts = (((uint64) (final_tx_time & 0xFFFFFFFE)) << 8)
+ TX_ANT_DLY;
/* Write all timestamps in the final message. See NOTE 10 below. */
final_msg_set_ts(&tx_final_msg[FINAL_MSG_POLL_TX_TS_IDX],
poll_tx_ts);
final_msg_set_ts(&tx_final_msg[FINAL_MSG_RESP_RX_TS_IDX],
resp_rx_ts);
final_msg_set_ts(&tx_final_msg[FINAL_MSG_FINAL_TX_TS_IDX],
final_tx_ts);
/* Write and send final message. See NOTE 7 below. */
tx_final_msg[ALL_MSG_SN_IDX] = frame_seq_nb;
dwt_writetxdata(sizeof(tx_final_msg), tx_final_msg, 0);
dwt_writetxfctrl(sizeof(tx_final_msg), 0);
dwt_starttx(DWT_START_TX_DELAYED); // SEND FINAL MSG
/* Poll DW1000 until TX frame sent event set. See NOTE 8 below. */
timeout(10);
PT_YIELD_UNTIL((dwt_read32bitreg(SYS_STATUS_ID) & SYS_STATUS_TXFRS) || timeout());;
/* Clear TXFRS event. */
dwt_write32bitreg(SYS_STATUS_ID, SYS_STATUS_TXFRS);
/* Increment frame sequence number after transmission of the final message (modulo 256). */
frame_seq_nb++;
} else {
bool Usb::dispatch(Msg& msg)
{
uint8_t b;
uint32_t i;
uint32_t count;
if ( msg.is(os,SIG_ERC,fd(),0))
{
logger.level(Logger::WARN) << " error occured. Reconnecting.";
logger.flush();
disconnect();
// connect();
return 0;
}
PT_BEGIN ( );
while(true)
{
PT_YIELD_UNTIL ( msg.is(this,SIG_CONNECTED));
while( true )
{
PT_YIELD_UNTIL(msg.is(os,SIG_RXD,fd(),0)|| msg.is(os,SIG_ERC,fd(),0));//event->is(RXD) || event->is(FREE) || ( inBuffer.hasData() && (_isComplete==false)) );
if ( msg.is(os,SIG_RXD,fd(),0) && hasData())
{
count =hasData();
for(i=0; i<count; i++)
{
b=read();
inBuffer.write(b);
}
logger.level(Logger::DEBUG)<< "recvd: " << inBuffer.size() << " bytes.";
logger.flush();
while( inBuffer.hasData() )
{
if ( _inBytes.Feed(inBuffer.read()))
{
Str l(256);
_inBytes.toString(l);
logger.level(Logger::DEBUG)<< "recv : " << l;
logger.flush();
_inBytes.Decode();
if ( _inBytes.isGoodCrc() )
{
_inBytes.RemoveCrc();
Str l(256);
_inBytes.toString(l);
logger.level(Logger::INFO)<<" recv clean : " <<l;
logger.flush();
MqttIn* _mqttIn=new MqttIn(256);
_inBytes.offset(0);
while(_inBytes.hasData())
_mqttIn->Feed(_inBytes.read());
if ( _mqttIn->parse())
{
MsgQueue::publish(this,SIG_RXD,_mqttIn->type(),_mqttIn); // _mqttIn will be deleted by msg process
}
else
{
Sys::warn(EINVAL, "MQTT");
delete _mqttIn;
}
}
else
{
logger.level(Logger::WARN)<<"Bad CRC. Dropped packet. ";
logger.flush();
_inBytes.clear(); // throw away bad data
}
_inBytes.clear();
}
}
}
else if ( msg.is(os,SIG_ERC,fd(),0) )
{
_inBytes.clear();
break;
}
PT_YIELD ( );
}
}
PT_END ( );
}