void handle_discover_request(pkt_t *pkt)
{
node_id_t origin;
uint8_t seq, depth;
nrk_time_t delay;
uint8_t attempt;
int8_t rc;
origin = pkt->payload[PKT_REQUEST_ORIGIN_OFFSET];
seq = pkt->payload[PKT_REQUEST_SEQ_OFFSET];
depth = pkt->payload[PKT_REQUEST_DEPTH_OFFSET];
if (!IS_VALID_NODE_ID(origin)) {
LOG("WARN: invalid origin node id in response: ");
LOGP("%d\r\n", origin);
return;
}
LOG("request:");
LOGA(" src "); LOGP("%u", pkt->src);
LOGA(" orig "); LOGP("%u", origin);
LOGA(" seq "); LOGP("%d (%d)", seq, discovered_seq);
LOGA(" depth "); LOGP("%d\r\n", depth);
nrk_time_get(&last_activity);
choose_delay(&delay, &discover_req_delay);
nrk_wait(delay);
/* Discover this node: mark and broadcast to neighbors */
if (discovered_seq != seq) {
discovered_seq = seq;
route_to_origin = pkt->src;
LOG("discovered: orig "); LOGP("%d", origin);
LOGA(" seq "); LOGP("%d", discovered_seq);
LOGA(" hop "); LOGP("%d\r\n", route_to_origin);
if (depth == 0 || pkt->hops < depth) {
attempt = 0;
do {
rc = broadcast_request(origin, seq, depth, attempt);
if (rc != NRK_OK)
LOG("WARN: failed to broadcast req\r\n");
nrk_wait(delay);
} while (++attempt < discover_send_attempts);
} else {
LOG("max depth reached: "); LOGP("%d\r\n", depth);
}
} else {
LOG("already discovered: seq "); LOGP("%u\r\n", discovered_seq);
}
attempt = 0;
do {
send_response(origin, pkt->src, seq, attempt);
nrk_wait(delay);
} while (++attempt < discover_send_attempts);
}
// This function will put the node into a low-duty checking mode to save power
void tdma_snooze()
{
int8_t v;
uint8_t i;
// stop the software watchdog timer so it doesn't interrupt
nrk_sw_wdt_stop(0);
// This flag is cleared only by the button interrupt
snoozing=1;
// Setup the button interrupt
nrk_ext_int_configure( NRK_EXT_INT_1,NRK_LEVEL_TRIGGER, &wakeup_func);
// Clear it so it doesn't fire instantly
EIFR=0xff;
nrk_ext_int_enable( NRK_EXT_INT_1);
// Now loop and periodically check for packets
while(1)
{
nrk_led_clr(RED_LED);
nrk_led_clr(GREEN_LED);
rf_power_up();
rf_rx_on ();
// check return from button interrupt on next cycle
if(snoozing==0 ) return;
tmp_time.secs=0;
tmp_time.nano_secs=10*NANOS_PER_MS;
nrk_led_set(RED_LED);
// Leave radio on for two loops of 10ms for a total of 20ms every 10 seconds
for(i=0; i<2; i++ )
{
v = _tdma_rx ();
if(v==NRK_OK) {
nrk_led_set(RED_LED);
nrk_ext_int_disable( NRK_EXT_INT_1);
nrk_sw_wdt_update(0);
nrk_sw_wdt_start(0);
return NRK_OK;
}
nrk_wait(tmp_time);
}
nrk_led_clr(RED_LED);
rf_power_down();
tmp_time.secs=10;
tmp_time.nano_secs=0;
nrk_sw_wdt_stop(0);
nrk_wait(tmp_time);
}
}
void handle_discover_response(pkt_t *pkt)
{
node_id_t origin = pkt->payload[PKT_RESPONSE_ORIGIN_OFFSET];
uint8_t seq = pkt->payload[PKT_RESPONSE_SEQ_OFFSET];
nrk_time_t delay;
uint8_t attempt;
if (!IS_VALID_NODE_ID(origin)) {
LOG("WARN: invalid origin in response: ");
LOGP("%d\r\n", origin);
return;
}
LOG("response: orig "); LOGP("%u", origin);
LOGA(" seq "); LOGP("%u", seq);
LOGA(" src "); LOGP("%u", pkt->src);
LOGA(": ");
nrk_time_get(&last_activity);
if (origin == this_node_id) {
LOGA("reached origin\r\n");
add_path_to_graph(pkt);
print_graph(&network);
} else { /* we're not the destination: forward to gateway */
attempt = 0;
do {
forward_response(pkt, attempt);
choose_delay(&delay, &discover_req_delay);
nrk_wait(delay);
} while (++attempt < discover_send_attempts);
}
}
int8_t nrk_wait_until (nrk_time_t t)
{
nrk_time_t ct;
int8_t v;
// uint8_t c;
//c = _nrk_os_timer_get ();
//do{
//}while(_nrk_os_timer_get()==c);
//ttt=c+1;
nrk_time_get (&ct);
v = nrk_time_sub (&t, t, ct);
//nrk_time_compact_nanos(&t);
if (v == NRK_ERROR)
{
return NRK_ERROR;
}
//if(t.secs<ct.secs) return 0;
//if(t.secs==ct.secs && t.nano_secs<ct.nano_secs) return 0;
//t.secs-=ct.secs;
//t.nano_secs-=ct.nano_secs;
//
nrk_wait (t);
return NRK_OK;
}
static int8_t proc_ping(node_id_t requester,
uint8_t *req_buf, uint8_t req_len,
uint8_t *reply_buf, uint8_t reply_size,
uint8_t *reply_len)
{
uint8_t token;
if (req_len != RPC_PING_REQ_LEN) {
LOG("WARN: req of unexpected length: ");
LOGP("%u/%u\r\n", req_len, RPC_PING_REQ_LEN);
return NRK_ERROR;
}
if (reply_size < RPC_PING_REPLY_LEN) {
LOG("WARN: reply buf too small: ");
LOGP("%u/%u\r\n", reply_size, RPC_PING_REPLY_LEN);
return NRK_ERROR;
}
nrk_led_set(led_proc_ping);
nrk_wait(pong_delay);
nrk_led_clr(led_proc_ping);
token = req_buf[RPC_PING_REQ_TOKEN_OFFSET];
reply_buf[RPC_PING_REPLY_TOKEN_OFFSET] = token;
*reply_len = RPC_PING_REPLY_LEN;
return NRK_OK;
}
int8_t _bmac_rx()
{
int8_t n;
uint8_t cnt;
rf_set_rx (&bmac_rfRxInfo, g_chan);
rf_polling_rx_on ();
cnt=0;
while ((n = rf_rx_check_fifop()) == 0)
{
cnt++;
nrk_wait(_bmac_check_period);
if(cnt>2) {
#ifdef DEBUG
printf( "rx timeout 1 %d\r\n",cnt );
#endif
if(rx_failure_cnt<65535) rx_failure_cnt++;
rf_rx_off();
return 0;
}
}
if (n != 0) {
n = 0;
// Packet on its way
cnt=0;
while ((n = rf_polling_rx_packet ()) == 0) {
cnt++;
nrk_spin_wait_us(100);
if (cnt > 50) {
#ifdef DEBUG
printf( "rx timeout 2\r\n" );
#endif
rx_failure_cnt++;
rf_rx_off();
return 0;
}
}
}
rf_rx_off();
if (n == 1) {
// CRC and checksum passed
rx_buf_empty=0;
#ifdef DEBUG
printf( "BMAC: SNR= %d [",bmac_rfRxInfo.rssi );
for(uint8_t i=0; i<bmac_rfRxInfo.length; i++ )
printf( "%c", bmac_rfRxInfo.pPayload[i]);
printf( "]\r\n" );
#endif
return 1;
} else
{
#ifdef DEBUG
printf( "CRC failed!\r\n" );
#endif
rx_failure_cnt++;
return 0;
}
rx_failure_cnt++;
return 0;
}
void bmac_nw_task ()
{
int8_t v;
int8_t e;
uint8_t backoff;
nrk_sig_mask_t event;
while(bmac_started()==0) nrk_wait_until_next_period();
//register the signal after bmac_init has been called
v=nrk_signal_register(bmac_enable_signal);
if(v==NRK_ERROR) nrk_kprintf( PSTR("Failed to register signal\r\n"));
backoff=0;
while (1) {
#ifdef NRK_SW_WDT
#ifdef BMAC_SW_WDT_ID
nrk_sw_wdt_update(BMAC_SW_WDT_ID);
#endif
#endif
if(is_enabled ) {
v=1;
if(rx_buf_empty==1) v=_bmac_channel_check();
// If the buffer is full, signal the receiving task again.
else e=nrk_event_signal (bmac_rx_pkt_signal);
// bmac_channel check turns on radio, don't turn off if
// data is coming.
if(v==0)
{
if(_bmac_rx()==1)
{
e=nrk_event_signal (bmac_rx_pkt_signal);
//if(e==NRK_ERROR) {
// nrk_kprintf( PSTR("bmac rx pkt signal failed\r\n"));
// printf( "errno: %u \r\n",nrk_errno_get() );
//}
}
//else nrk_kprintf( PSTR("Pkt failed, buf could be corrupt\r\n" ));
}
if(/*rx_buf_empty==1 &&*/ tx_data_ready==1)
{
rf_rx_off();
_bmac_tx();
}
//do {
nrk_wait(_bmac_check_period);
// if(rx_buf_empty!=1) nrk_event_signal (bmac_rx_pkt_signal);
//} while(rx_buf_empty!=1);
} else {
event=0;
do {
v=nrk_signal_register(bmac_enable_signal);
event=nrk_event_wait (SIG(bmac_enable_signal));
} while((event & SIG(bmac_enable_signal))==0);
}
//nrk_wait_until_next_period();
}
}
int8_t _bmac_tx()
{
uint8_t v,backoff, backoff_count;
uint16_t b;
#ifdef DEBUG
nrk_kprintf( PSTR("_bmac_tx()\r\n"));
#endif
if(cca_active)
{
// Add random time here to stop nodes from synchronizing with eachother
b=_nrk_time_to_ticks(&_bmac_check_period);
b=b/((rand()%10)+1);
//printf( "waiting %d\r\n",b );
nrk_wait_until_ticks(b);
//nrk_wait_ticks(b);
backoff_count=1;
do{
v=_bmac_channel_check();
rf_rx_off();
if(v==1) break;
// Channel is busy
backoff=rand()%(_b_pow(backoff_count));
#ifdef DEBUG
printf( "backoff %d\r\n",backoff );
#endif
// printf( "backoff %d\r\n",backoff );
nrk_wait_until_next_n_periods(backoff);
backoff_count++;
if(backoff_count>6) backoff_count=6; // cap it at 64
b=_nrk_time_to_ticks(&_bmac_check_period);
b=b/((rand()%10)+1);
// printf( "waiting %d\r\n",b );
nrk_wait_until_ticks(b);
// nrk_wait_ticks(b);
} while(v==0);
}
rf_test_mode();
rf_carrier_on();
nrk_wait(_bmac_check_period);
//nrk_wait_until_next_period();
rf_carrier_off();
rf_data_mode();
// send packet
rf_rx_off();
pkt_got_ack=rf_tx_packet (&bmac_rfTxInfo);
rf_rx_off();
tx_data_ready=0;
nrk_event_signal (bmac_tx_pkt_done_signal);
return NRK_OK;
}
void oled_on()
{
nrk_time_t t;
nrk_gpio_set(NRK_DEBUG_1);
t.secs=1;
t.nano_secs=500*NANOS_PER_MS;
nrk_kprintf( PSTR( "start wait\r\n" ));
nrk_wait(t);
nrk_kprintf( PSTR( "stop wait\r\n" ));
putc0(0x55);
}
//*************************PING MODE************************************************
void getDestMac()
{
mac_addr[0] = '\0';
nrk_kprintf(PSTR("\r\n*************************************************************\r\n"));
nrk_kprintf(PSTR(" Please Enter Address of Node to Program \r\n"));
nrk_kprintf(PSTR(" MAC (Subnet + Dest) 4 Bytes Hex E.g. 00000004 \r\n"));
nrk_kprintf(PSTR("*************************************************************\r\n"));
printf("Enter MAC:");
charCount = 0;
// Enter Mac
do
{
// Wait for UART signal
while(1)
{
if(nrk_uart_data_ready(NRK_DEFAULT_UART)!=0)
{
while(nrk_uart_data_ready(NRK_DEFAULT_UART)!=0)
{
charCount++;
// Read Character
c=getchar();
printf( "%c",c);
sprintf(mac_addr,"%s%c",mac_addr,c);
}
break;
}
timeout.secs = 0;
timeout.nano_secs = 20 * NANOS_PER_MS;
nrk_wait(timeout);
}
}while(charCount < 8);
buffer0[0]=mac_addr[0]; buffer0[1]=mac_addr[1]; buffer0[3]='\0';
buffer1[0]=mac_addr[2]; buffer1[1]=mac_addr[3]; buffer1[3]='\0';
buffer2[0]=mac_addr[4]; buffer2[1]=mac_addr[5]; buffer2[3]='\0';
buffer3[0]=mac_addr[6]; buffer3[1]=mac_addr[7]; buffer3[3]='\0';
val=sscanf( buffer0,"%x",&dest_mac[3]);
val+=sscanf( buffer1,"%x",&dest_mac[2]);
val+=sscanf( buffer2,"%x",&dest_mac[1]);
val+=sscanf( buffer3,"%x",&dest_mac[0]);
nrk_kprintf(PSTR("\r\n*************************************************************\r\n"));
nrk_kprintf(PSTR(" Node MAC Accepted \r\n"));
printf(" Initiating Update for Node 0 x %X %X %X %X \r\n", dest_mac[3],dest_mac[2],dest_mac[1], dest_mac[0]);
nrk_kprintf(PSTR("*************************************************************\r\n"));
}
void bmac_nw_task()
{
int8_t v;
int8_t e;
uint8_t backoff;
nrk_sig_mask_t event;
while(bmac_started() == 0) {
nrk_wait_until_next_period();
}
while(1) {
if(is_enabled)
{
v = 1;
rf_rx_on();
if(rx_buf_empty) {
v = _bmac_channel_check();
} else {
e = nrk_event_signal(bmac_rx_pkt_signal); // Mb
// Should signal user task here as a "reminder"
}
if(v == 0) {
// Channel detected as busy, attept to Rx
_bmac_rx();
// Should signal user task here to notify of Rx
}
else if(tx_data_ready) {
// Only try to Tx if the channel is free
rf_rx_off(); // Mb
_bmac_tx();
}
//rf_rx_off(); // Mb
nrk_wait(_bmac_check_period);
}
else
{
event =0;
do
{
v = nrk_signal_register(bmac_enable_signal);
event = nrk_event_wait(SIG(bmac_enable_signal));
}
while((event & SIG(bmac_enable_signal))==0);
}
}
}
int8_t _bmac_rx()
{
int8_t result;
uint8_t cnt;
rf_rx_on();
cnt = 0;
while((result = rf_rx_check_cca()) != 0) {
cnt++;
nrk_wait(_bmac_check_period);
if(cnt > 2) {
if(rx_failure_cnt < 65535) rx_failure_cnt++;
rf_rx_off();
//printf("failure1=%d\r\n",rx_failure_cnt);
return 0;
}
}
// Packet on its way
cnt = 0;
while((result = rf_rx_packet()) == 0) {
cnt++;
nrk_wait(_bmac_check_period);
//nrk_spin_wait_us(100);
if(cnt > 1) {
if(rx_failure_cnt < 65535) rx_failure_cnt++;
rf_rx_off();
//printf("failure2=%d\r\n",rx_failure_cnt);
return 0;
}
}
//printf("failure3=%d\r\n",rx_failure_cnt);
rf_rx_off();
rx_buf_empty = 0;
return 1;
// Note: CRC is checked in hardware
}
int8_t _bmac_tx()
{
uint8_t v,result, backoff, backoff_count;
uint16_t b;
if(cca_active) {
// Add random time here to stop nodes from synchronizing with eachother
b=_nrk_time_to_ticks(&_bmac_check_period);
b=b/((rand()%10)+1);
//printf( "waiting %d\r\n",b );
nrk_wait_until_ticks(b);
//nrk_wait_ticks(b);
backoff_count=1;
do
{
v=_bmac_channel_check();
rf_rx_off();
if(v==1) break;
// Channel is busy
backoff=rand()%(_b_pow(backoff_count));
// printf( "backoff %d\r\n",backoff );
nrk_wait_until_next_n_periods(backoff);
backoff_count++;
if(backoff_count>6) backoff_count=6; // cap it at 64
b=_nrk_time_to_ticks(&_bmac_check_period);
b=b/((rand()%10)+1);
// printf( "waiting %d\r\n",b );
nrk_wait_until_ticks(b);
// nrk_wait_ticks(b);
}
while(v==0);
}
// Give check_period worth of channel activity
rf_test_mode();
rf_carrier_on();
nrk_wait(_bmac_check_period);
rf_carrier_off();
rf_data_mode();
rf_rx_off();
pkt_got_ack = rf_tx_packet(&bmac_rfTxInfo);
rf_rx_off();
tx_data_ready = 0;
// Should signal user task here that Tx is done
return NRK_OK;
}
// Assuming that CCA returned 1 and a packet is on its way
// Receive the packet or timeout and error
int8_t _bmac_rx ()
{
int8_t n;
uint8_t cnt;
rf_rx_on ();
cnt = 0;
//printf( "calling rx\r\n" );
dummy_t.secs = 0;
dummy_t.nano_secs = 5 * NANOS_PER_MS;
nrk_wait (dummy_t);
n = rf_rx_packet_nonblock ();
if (n != NRK_OK) {
if (rx_failure_cnt < 65535)
rx_failure_cnt++;
rf_rx_off ();
return 0;
}
/*while ( rf_rx_packet_nonblock() != NRK_OK )
{
cnt++;
nrk_wait(_bmac_check_period);
if(cnt>2) {
#ifdef DEBUG
printf( "rx timeout 1 %d\r\n",cnt );
#endif
if(rx_failure_cnt<65535) rx_failure_cnt++;
rf_rx_off();
return 0;
}
}
*/
rx_buf_empty = 0;
#ifdef DEBUG
printf ("BMAC: SNR= %d [", bmac_rfRxInfo.rssi);
for (uint8_t i = 0; i < bmac_rfRxInfo.length; i++)
printf ("%c", bmac_rfRxInfo.pPayload[i]);
printf ("]\r\n");
#endif
rf_rx_off ();
return 1;
}
int8_t nrk_wait_until(nrk_time_t t) {
nrk_time_t ct;
uint8_t v;
nrk_time_get(&ct);
v = nrk_time_sub(&t, t, ct);
if (v == 0)
return NRK_ERROR;
//if(t.secs<ct.secs) return 0;
//if(t.secs==ct.secs && t.nano_secs<ct.nano_secs) return 0;
//t.secs-=ct.secs;
//t.nano_secs-=ct.nano_secs;
nrk_wait(t);
return NRK_OK;
}
void tx_task ()
{
// Get the signal for UART RX
//uart_rx_signal=nrk_uart_rx_signal_get();
// Register your task to wakeup on RX Data
//if(uart_rx_signal==NRK_ERROR) nrk_kprintf( PSTR("Get Signal ERROR!\r\n") );
//nrk_signal_register(uart_rx_signal);
//printf ("tx_task PID=%d\r\n", nrk_get_pid ());
// Wait until the tx_task starts up bmac
// This should be called by all tasks using bmac that
// do not call bmac_init()...
bmac_init (26);
bmac_encryption_set_key(aes_key,16);
bmac_encryption_enable();
bmac_rx_pkt_set_buffer (rx_buf, RF_MAX_PAYLOAD_SIZE);
//nrk_kprintf (PSTR ("bmac_started()\r\n"));
bmac_set_cca_thresh (-45);
check_period.secs = 0;
check_period.nano_secs = 100 * NANOS_PER_MS;
val = bmac_set_rx_check_rate (check_period);
// Get and register the tx_done_signal if you want to
// do non-blocking transmits
tx_done_signal = bmac_get_tx_done_signal ();
nrk_signal_register (tx_done_signal);
rx_signal = bmac_get_rx_pkt_signal ();
nrk_signal_register (rx_signal);
cnt = 0;
while (1)
{
nrk_kprintf(PSTR("\r\n*************************************************************\r\n"));
nrk_kprintf(PSTR(" PHOENIX WIRELESS UPDATE SYSTEM \r\n"));
nrk_kprintf(PSTR("*************************************************************\r\n"));
nrk_kprintf(PSTR("Press 'p' : To PING Nodes in Vicinity \r\n"));
nrk_kprintf(PSTR("Press 'u' : To Begin Node Update \r\n"));
nrk_kprintf(PSTR(" \r\n"));
nrk_kprintf(PSTR("*************************************************************\r\n"));
printf("Enter Choice: ");
//sm=nrk_event_wait(SIG(uart_rx_signal));
//if(sm != SIG(uart_rx_signal))
//{
// nrk_kprintf( PSTR("UART signal error\r\n") );
// while(1);
//}
// Wait for UART signal
while(1)
{
if(nrk_uart_data_ready(NRK_DEFAULT_UART)!=0)
{
// Read Character
c=getchar();
printf( "%c\r\n",c);
break;
}
timeout.secs = 0;
timeout.nano_secs = 20 * NANOS_PER_MS;
nrk_wait(timeout);
}
// Choose mode
switch(c){
case 'p':
programState = PING;
break;
case 'u':
getDestMac();
phoenix_init();
programState = UPDATE;
break;
default:
programState = NONE;
nrk_kprintf(PSTR("Invalid Command! Please Try Again\r\n"));
}
// Reset c
c = 0;
nrk_wait_until_next_period();
// Execute protocol
switch(programState)
{
case PING:
pingMode();
break;
case UPDATE:
updateMode();
break;
case NONE:;// Do nothing
break;
default:
nrk_kprintf(PSTR("Invalid Program State\r\n"));
break;
}
nrk_wait_until_next_period ();
}
}
int8_t get_heading(int16_t *heading)
{
uint8_t status = 0;
nrk_time_t start, elapsed, now;
int8_t rc;
uint8_t i;
uint8_t lsb, msb;
float heading_rad;
if (!have_compass) {
LOG("ERROR: no compass\r\n");
return NRK_ERROR;
}
/* trigger a measurement */
rc = twi_write(COMPASS_ADDR, REG_MODE, 0x01);
if (rc != NRK_OK) {
LOG("ERROR: failed to set mode\r\n");
return rc;
}
/* wait for data ready */
nrk_time_get(&start);
do {
nrk_wait(compass_poll_interval);
status = 0;
rc = twi_read(COMPASS_ADDR, REG_STATUS, &status);
if (rc != NRK_OK) {
LOG("ERROR: failed to read status\r\n");
return rc;
}
nrk_time_get(&now);
nrk_time_sub(&elapsed, now, start);
} while (!(status & BIT_STATUS_READY) &&
time_cmp(&elapsed, &compass_measure_timeout) < 0);
if (!(status & BIT_STATUS_READY)) {
LOG("ERROR: compass measure timed out\r\n");
return NRK_ERROR;
}
for (i = 0; i < NUM_AXES; ++i) {
rc = twi_read(COMPASS_ADDR, REG_DATA_OUT_X_MSB + 2 * i, &msb);
if (rc != NRK_OK) break;
rc = twi_read(COMPASS_ADDR, REG_DATA_OUT_X_LSB + 2 * i, &lsb);
if (rc != NRK_OK) break;
mag[i] = (msb << 8) | lsb;
}
if (rc != NRK_OK) {
LOG("ERROR: compass read failed\r\n");
return rc;
}
/* Re-order to (X, Y, Z) and cast to float */
mag_uT[AXIS_X] = mag[RAW_AXIS_X];
mag_uT[AXIS_Y] = mag[RAW_AXIS_Y];
mag_uT[AXIS_Z] = mag[RAW_AXIS_Z];
/* Convert to uT units */
for (i = 0; i < NUM_AXES; ++i)
mag_uT[i] = mag_uT[i] / GAUSS_LSB * GAUSS_TO_MICROTESLA;
heading_rad = atan2(mag_uT[AXIS_Y], mag_uT[AXIS_X]);
if (heading_rad < 0)
heading_rad += 2.0 * M_PI;
heading_rad *= 180.0 / M_PI;
*heading = heading_rad;
LOG("raw: ");
for (i = 0; i < NUM_AXES; ++i)
LOGP("%x:%x ", (mag[i] >> 8) & 0xFF, mag[i] & 0xFF);
LOGA("dec: ");
for (i = 0; i < NUM_AXES; ++i)
LOGP("%d ", mag[i]);
LOGA("\r\n");
LOG("uT: ");
for (i = 0; i < NUM_AXES; ++i)
LOGP("%d ", (int16_t)mag_uT[i]);
LOGP("=> %d deg\r\n", *heading);
return NRK_OK;
}
