// This is an interrupts routine that handles the button press.
// It has a 300ms debounce
void button_handler()
{
int8_t v;
// Make sure button is depressed for at least 50 us
nrk_spin_wait_us(50);
v=nrk_gpio_get(NRK_PORTD_0);
if(v!=0) return;
nrk_time_get(&button_cur_press);
nrk_time_sub(&button_tmp_press, button_cur_press, button_last_press );
if(button_tmp_press.secs>=1 || button_tmp_press.nano_secs>=(300*NANOS_PER_MS))
{
// Reboot the node...
socket_0_disable();
power_socket_disable(0);
nrk_int_disable();
while(1);
if(socket_0_active==1)
{
plug_led_green_clr();
power_socket_disable(0);
}
else
{
plug_led_green_set();
power_socket_enable(0);
}
button_last_press.secs=button_cur_press.secs;
button_last_press.nano_secs=button_cur_press.nano_secs;
}
}
void tdma_stats_ts_packet(uint8_t * tx_buf)
{
nrk_time_t old, cur, lat;
nrk_time_get(&cur);
nrk_time_compact_nanos(&cur);
//memcpy(&lat, &tx_buf[TDMA_DATA_START+8], sizeof(lat));
memcpy(&lat, &tx_buf[TDMA_DATA_START+8], sizeof(lat));
//memcpy(&old, &tx_buf[TDMA_DATA_START+16], sizeof(old));
memcpy(&old, &tx_buf[TDMA_DATA_START+16], sizeof(old));
//printf("MTTS %lu %lu %lu %lu\r\n", lat.secs, lat.nano_secs, old.secs, old.nano_secs);
// subtract current time from arrival time (old)
if (nrk_time_sub(&cur, cur, old) == NRK_ERROR)
{
cur.secs = 0;
cur.nano_secs = 0;
}
// add that difference to the total latency
nrk_time_add(&lat, lat, cur);
// finally, add the cumulative latency back in
memcpy(&tx_buf[TDMA_DATA_START+8], &lat, sizeof(lat));
}
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;
}
void tdma_stats_dump()
{
nrk_time_t ct;
nrk_time_get(&ct);
// PRINTS OUT...
//
// My MAC address
// time tdma task has spent awake (secs)
// time tdma task has spent awake (nanosecs)
// time tdma task used radio
// number of slot steals
// number of backoffs
// number of rx syncs
// number of sync slots
#ifdef NRK_STATS_TRACKER
nrk_stats_get(tdma_pid, &tdma_stat_struct);
tdma_cpu_time = _nrk_ticks_to_time(tdma_stat_struct.total_ticks);
//printf("TCT %lu %lu %lu %lu\r\n", tdma_cpu_time.secs, tdma_cpu_time.nano_secs,
// tree_creation_time.secs, tree_creation_time.nano_secs);
nrk_time_sub(&tdma_cpu_time, tdma_cpu_time, tree_creation_time);
#endif
printf("SD %d %lu %lu %lu %lu %lu %u %u\r\n",
tdma_mac_get(),
tdma_cpu_time.secs,
tdma_cpu_time.nano_secs,
ticks_rdo,
steal_cnt,
backoff_cnt,
sync_rx_cnt,
sync_slot_cnt);
printf("OSA %lu %lu %lu %lu\r\n",
osa_time.secs, osa_time.nano_secs,
ct.secs, ct.nano_secs);
#ifdef SYNC_HIST
printf("SH %d %d %d %d %d %d %d %d %d %d %d\r\n",
tdma_mac_get(),
sync_hist[0],
sync_hist[1],
sync_hist[2],
sync_hist[3],
sync_hist[4],
sync_hist[5],
sync_hist[6],
sync_hist[7],
sync_hist[8],
sync_hist[9]);
#endif
}
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 heart_rate_int()
{
uint8_t bpm;
nrk_led_set(BLUE_LED);
nrk_time_get(&c);
nrk_time_sub(&t,c,l);
bpm=(uint16_t)60000 / (uint16_t)(t.nano_secs/1000000);
if(bpm>20 && bpm<220 )
{
bpm_list[bpm_index]=bpm;
bpm_index++;
if(bpm_index==BPM_LIST_SIZE)bpm_index=0;
}
l.secs=c.secs;
l.nano_secs=c.nano_secs;
nrk_led_clr(BLUE_LED);
}
uint8_t hrm_get_value()
{
uint16_t acc;
uint8_t i,j,tmp;
nrk_time_get(&c);
nrk_time_sub(&t,c,l);
if(t.secs>5) {
for(i=0; i<BPM_LIST_SIZE; i++ ) bpm_list[i]=0;
return 0;
}
for(i=0; i<BPM_LIST_SIZE; i++ )
{
bpm_sort[i]=bpm_list[i];
if(bpm_list[i]==0) return 0;
}
for(i=0; i<BPM_LIST_SIZE; i++ )
{
for(j=0; j<BPM_LIST_SIZE-1; j++ )
{
if(bpm_sort[j]>bpm_sort[j+1] )
{
tmp=bpm_sort[j];
bpm_sort[j]=bpm_sort[j+1];
bpm_sort[j+1]=tmp;
}
}
}
/*
for(i=0; i<BPM_LIST_SIZE; i++ )
{
if(bpm_index==i) printf( ">" );
printf( "%u %d %d\r\n",i,bpm_sort[i],bpm_list[i]);
}
*/
acc=0;
for(i=(BPM_LIST_SIZE/2)-2; i<(BPM_LIST_SIZE/2)+2; i++ )
acc+=bpm_sort[BPM_LIST_SIZE/2];
//return bpm_sort[BPM_LIST_SIZE/2];
return (acc/4);
}
static void periodic_task()
{
nrk_sig_mask_t wait_mask, func_wait_mask;
nrk_time_t next_event, func_next_event;
periodic_func_t **funcp;
periodic_func_t *func;
nrk_time_t now, sleep_time;
int8_t rc;
funcp = &functions[0];
while (*funcp) {
func = *funcp;
LOG("init: "); LOGF(func->name); LOGNL();
if (func->init)
func->init();
funcp++;
}
rc = nrk_signal_register(func_signal);
if (rc == NRK_ERROR)
ABORT("reg sig: func\r\n");
while (1) {
LOG("awake\r\n");
TIME_CLEAR(next_event);
wait_mask = SIG(func_signal);
funcp = &functions[0];
while (*funcp) {
func = *funcp;
TIME_CLEAR(func_next_event);
func_wait_mask = 0;
if (func->enabled ||
func->enabled != func->last_enabled) {
LOG("proc: "); LOGF(func->name); LOGNL();
ASSERT(func->proc);
func->proc(func->enabled,
&func_next_event, &func_wait_mask);
}
func->last_enabled = func->enabled;
wait_mask |= func_wait_mask;
if (IS_VALID_TIME(func_next_event) &&
(!IS_VALID_TIME(next_event) ||
time_cmp(&func_next_event, &next_event) < 0)) {
next_event = func_next_event;
}
funcp++;
}
if (IS_VALID_TIME(next_event)) {
nrk_time_get(&now);
rc = nrk_time_sub(&sleep_time, next_event, now);
if (rc != NRK_OK) {
LOG("next event in the past\r\n");
continue;
}
LOG("sleeping for: ");
LOGP("%lu ms\r\n", TIME_TO_MS(sleep_time));
nrk_set_next_wakeup(sleep_time);
wait_mask |= SIG(nrk_wakeup_signal);
}
LOG("waiting\r\n");
nrk_event_wait( wait_mask );
}
ABORT("periodic task exited\r\n");
}
void nl_tx_task()
{
TransmitBuffer *ptr = NULL; // pointer to the buffer to be transmitted
nrk_sig_t tx_done_signal; // to hold the tx_done signal from the link layer
int8_t ret; // to hold the return value of various functions
int8_t port_index; // to store the index of the corresponding port element
int8_t sent; // to count the number of times the HELLO msg was sent
int8_t isApplication; // flag to indicate whether the packet in the transmit
// buffer is an APPLICATION / NW_CONTROL packet
nrk_time_t timeout;
nrk_time_t start; // used for sending network control messages
nrk_time_t end;
nrk_time_t elapsed;
// wait for the nl_rx_task to start bmac
while(!bmac_started()) nrk_wait_until_next_period();
// retrieve and register for the 'transmit done' signal from bmac
tx_done_signal = bmac_get_tx_done_signal();
if( nrk_signal_register(tx_done_signal) == NRK_ERROR )
{
nrk_int_disable();
nrk_led_set(RED_LED);
while(1)
nrk_kprintf(PSTR("NL: Error while registering for the bmax_tx_done_signal\r\n"));
}
// initialise the timer
nrk_time_get(&start);
end.secs = start.secs;
end.nano_secs = start.nano_secs;
sent = 0;
// set the radio power
if( bmac_set_rf_power(10) == NRK_ERROR)
{
nrk_led_set(RED_LED);
nrk_int_disable();
while(1)
nrk_kprintf(PSTR("Error setting the transmit power\r\n"));
}
while(1)
{
isApplication = FALSE; // assume at the beginning that a nw_ctrl pkt will be transmitted
ret = nrk_time_sub(&elapsed, end, start);
if(ret == 0)
{
nrk_int_disable();
nrk_led_set(RED_LED);
while(1)
nrk_kprintf(PSTR("NL: Error returned by nrk_time_sub\r\n"));
}
nrk_time_compact_nanos(&elapsed);
if(elapsed.secs >= HELLO_PERIOD)
{
sent++;
build_Msg_Hello(&mhe); // build the 'HELLO' message
if(DEBUG_NL == 2)
{
nrk_kprintf(PSTR("After building Msg_Hello, packet = "));
print_pkt(&pkt_tx);
}
enter_cr(bm_sem, 34);
ret = insert_tx_aq(&pkt_tx); // insert it into the transmit queue
leave_cr(bm_sem, 34);
if(DEBUG_NL == 2)
{
nrk_kprintf(PSTR("build_Msg_Hello() inserted packet."));
print_tx_buffer();
//print_pkt_header(&pkt_tx);
}
if(ret == NRK_ERROR && DEBUG_NL == 2)
{
nrk_kprintf(PSTR("HELLO msg was not inserted into the transmit queue\r\n"));
}
start.secs = end.secs;
start.nano_secs = end.nano_secs; // reinitialise the timer
}
if(sent >= 3) //NGB_LIST_PERIOD / HELLO_PERIOD) // NGB_LIST period is always a multiple of HELLO_PERIOD
{
build_Msg_NgbList(&mn); // build the 'NGB_LIST' message
enter_cr(bm_sem, 34);
ret = insert_tx_aq(&pkt_tx); // insert it into the transmit queue
leave_cr(bm_sem, 34);
if(DEBUG_NL == 2)
{
nrk_kprintf(PSTR("build_Msg_NgbList() inserted packet."));
print_tx_buffer();
//print_pkt_header(&pkt_tx);
}
if(ret == NRK_ERROR && DEBUG_NL == 2)
{
nrk_kprintf(PSTR("NGB_LIST msg was not inserted into the transmit queue\r\n"));
}
sent = 0; // reset the value of 'sent'
}
if(rand() % 2 == 0) // random number generator
collect_queue_statistics();
enter_cr(bm_sem, 34);
ptr = remove_tx_aq();
leave_cr(bm_sem, 34);
if(ptr == NULL) // transmit queue is empty
{
if(DEBUG_NL == 2)
nrk_kprintf(PSTR("NL:Transmit queue is empty\r\n"));
// update the end time
nrk_time_get(&end);
nrk_wait_until_next_period(); // FIX ME
continue;
}
if(DEBUG_NL == 2)
{
nrk_kprintf(PSTR("NL: nl_tx_task(): Packet removed. Packet = "));
//print_pkt_header( &(ptr -> pkt) );
print_pkt( &(ptr -> pkt) );
//print_tx_buffer();
}
// check to see the type of packet. It should be of type APPLICATION and be sent by this
// node
if( (pkt_type(&(ptr -> pkt)) == APPLICATION) && ((ptr -> pkt).src == NODE_ADDR) )
{
// remove the encapsulated TL segment from the packet
unpack_TL_UDP_header(&seg, (ptr -> pkt).data);
memcpy(seg.data, (ptr -> pkt).data + SIZE_TRANSPORT_UDP_HEADER, MAX_APP_PAYLOAD);
if(DEBUG_NL == 2)
{
nrk_kprintf(PSTR("NL: nl_tx_task(): Segment Removed = "));
print_seg(&seg);
}
isApplication = TRUE;
}
// pack the network packet header into the transmit buffer
pack_NW_Packet_header(tx_buf, &(ptr -> pkt));
// append the network payload into the transmit buffer
memcpy(tx_buf + SIZE_NW_PACKET_HEADER, (ptr -> pkt).data, MAX_NETWORK_PAYLOAD);
enter_cr(bm_sem, 34);
insert_tx_fq(ptr); // release the transmit buffer into the free queue
leave_cr(bm_sem, 34);
if(DEBUG_NL == 2)
{
nrk_kprintf(PSTR("NL: nl_tx_task(): Released transmit buffer back into queue\n"));
print_tx_buffer();
}
do
{
ret = bmac_tx_pkt_nonblocking(tx_buf, SIZE_NW_PACKET); // try to queue the buffer in link layer
if(ret == NRK_ERROR)
if(nrk_event_wait(SIG(tx_done_signal)) == 0)
{
nrk_int_disable();
nrk_led_set(RED_LED);
while(1)
nrk_kprintf(PSTR("NL: Error returned by nrk_event_wait(tx_done_signal)\r\n"));
}
}while(ret == NRK_ERROR);
// packet is queued at link layer
timeout.secs = 10; // set a wait period of maximum 10 seconds
timeout.nano_secs = 0;
if( nrk_signal_register(nrk_wakeup_signal) == NRK_ERROR )
{
nrk_int_disable();
nrk_led_set(RED_LED);
while(1)
nrk_kprintf(PSTR("NL:nl_tx(): Error registering for nrk_wakeup_signal\r\n"));
}
if( nrk_set_next_wakeup(timeout) == NRK_ERROR)
{
nrk_int_disable();
nrk_led_set(RED_LED);
while(1)
nrk_kprintf(PSTR("NL: nl_tx(): Error returned by nrk_set_next_wakeup()\r\n"));
}
nrk_led_set(BLUE_LED);
ret = nrk_event_wait (SIG(tx_done_signal) | SIG(nrk_wakeup_signal)); // wait for its transmission
if(ret == 0)
{
nrk_int_disable();
nrk_led_set(RED_LED);
while(1)
nrk_kprintf(PSTR("NL: Error returned by nrk_event_wait(tx_done_signal)\r\n"));
}
if(ret & SIG(tx_done_signal)) // bmac has successfully sent the packet over the radio
{
if(isApplication == TRUE) // it was an application layer packet
{
enter_cr(tl_sem, 34);
port_index = port_to_port_index(seg.srcPort);
if(port_index == NRK_ERROR) // sanity check
{
nrk_int_disable();
nrk_led_set(RED_LED);
while(1)
nrk_kprintf(PSTR("NL: nl_tx_task: Bug detected in implementation of port element array\r\n"));
}
// signal 'send done' signal
if(nrk_event_signal(ports[port_index].send_done_signal) == NRK_ERROR)
{
if(nrk_errno_get() == 1) // sanity check. This means signal was not created
{
nrk_int_disable();
nrk_led_set(RED_LED);
while(1)
nrk_kprintf(PSTR("NL: nl_tx_task: Bug detected in creating signals in port element array\r\n"));
}
}
leave_cr(tl_sem, 34);
}// end if(isApplication == TRUE)
else // a network control message was transmitted. Nothing to signal
; // do nothing
} // end if(signal received = tx_done_signal)
else if(ret & SIG(nrk_wakeup_signal))
{
//nrk_led_set(RED_LED);
//nrk_int_disable();
//while(1)
//{
nrk_kprintf(PSTR("BMAC did not transmit the packet within specified time\r\n"));
//}
}
else // unknown signal caught
{
nrk_int_disable();
nrk_led_set(RED_LED);
while(1)
nrk_kprintf(PSTR("NL: nl_tx_task(): Unknown signal caught\r\n"));
}
nrk_led_clr(BLUE_LED);
// update the end time
nrk_time_get(&end);
} // end while(1)
return;
}
void nl_rx_task()
{
uint8_t len; // to hold the size of the received packet, always = sizeof(NW_Packet)
int8_t rssi; // to hold rssi of received packet
uint8_t *local_rx_buf; // pointer to receive buffer of link layer
int8_t val; // status variable to hold the return type of function calls
int8_t flag;
nrk_time_t start, end, elapsed; // needed by nl_rx_task()
// to decide when to send own NGB_LIST
if(DEBUG_NL >= 1)
{
nrk_kprintf(PSTR("NL_RX_TASK PID = "));
printf("%d\r\n",nrk_get_pid());
}
// initialise the timer
nrk_time_get(&start);
end.secs = start.secs;
end.nano_secs = start.nano_secs;
// initialise the link layer
val = bmac_init(25);
if(val == NRK_ERROR)
{
nrk_int_disable();
nrk_led_set(RED_LED);
while(1)
nrk_kprintf(PSTR("NL: Error returned by bmac_init()\r\n"));
}
// give the link layer a rx buffer
val = bmac_rx_pkt_set_buffer(rx_buf, RF_BUFFER_SIZE);
if(val == NRK_ERROR)
{
nrk_int_disable();
nrk_led_set(RED_LED);
while(1)
nrk_kprintf(PSTR("NL: Error returned by bmac_rx_pkt_set_buffer()\r\n"));
}
// start processing forever
while(1)
{
// decide whether it is time to send your own Ngb_List message
if(CONNECTED_TO_GATEWAY == TRUE)
{
nrk_time_get(&end);
val = nrk_time_sub(&elapsed, end, start);
nrk_time_compact_nanos(&elapsed);
if(elapsed.secs >= NGB_LIST_PERIOD)
{
ntg_pkt.type = SERIAL_NGB_LIST;
ntg_pkt.length = SIZE_MSG_NGB_LIST;
enter_cr(nl_sem, 34);
pack_Msg_NgbList(ntg_pkt.data, &nl);
leave_cr(nl_sem, 34);
pack_NodeToGatewaySerial_Packet_header(to_gw_buf, &ntg_pkt);
memcpy(to_gw_buf + SIZE_NODETOGATEWAYSERIAL_PACKET_HEADER, ntg_pkt.data, MAX_SERIAL_PAYLOAD);
if(DEBUG_NL == 2)
{
nrk_kprintf(PSTR("Sending own NGB_LIST message to gateway\r\n"));
}
sendToSerial(to_gw_buf, SIZE_NODETOGATEWAYSERIAL_PACKET);
// reset the timer
start.secs = end.secs;
start.nano_secs = end.nano_secs;
} // end if
} // end if
if(DEBUG_NL >= 1)
{
nrk_kprintf(PSTR("Waiting for next pkt from link layer\r\n"));
}
flag = 0;
// wait for the next packet
while(bmac_rx_pkt_ready() == 0)
{
val = bmac_wait_until_rx_pkt();
if(DEBUG_NL == 2)
{
nrk_kprintf(PSTR("NL: bmac_wait_until_rx_packet() returned "));
printf("%d\n", val);
}
}
// Get the packet
do
{
local_rx_buf = bmac_rx_pkt_get(&len,&rssi);
if(local_rx_buf == NULL)
{
nrk_kprintf(PSTR("NL: NULL returned by bmac_rx_pkt_get()\r\n"));
}
} while(local_rx_buf == NULL);
// sanity check for debugging
if(len != SIZE_NW_PACKET) // this should not happen
{
/*
nrk_int_disable();
nrk_led_set(RED_LED);
while(1)
{
nrk_kprintf(PSTR("NL: Wrong length of packet received: "));
printf("%d\r\n", len);
}
*/
if(DEBUG_NL >= 1)
{
nrk_kprintf(PSTR("NL: nl_rx_task(): Wrong length of packet received: "));
printf("%d\r\n", len);
}
flag = 1;
}
nrk_led_set(GREEN_LED);
if(DEBUG_NL == 2)// || flag == 1)
{
int8_t i;
nrk_kprintf(PSTR("NL: Contents of received packet are\r\n"));
printf("[");
for(i = 0; i < len; i++)
printf("%d ", local_rx_buf[i]);
printf("]\r\n");
}
if(flag == 1)
{
bmac_rx_pkt_release(); // drop the packet and go receive another
nrk_led_clr(GREEN_LED);
continue;
}
// unpack the packet header from the received buffer
unpack_NW_Packet_header(&pkt_rx, local_rx_buf);
// copy the packet payload to the data field of the local packet
memcpy(pkt_rx.data, local_rx_buf + SIZE_NW_PACKET_HEADER, MAX_NETWORK_PAYLOAD);
// Release the RX buffer quickly so future packets can arrive
bmac_rx_pkt_release();
// begin processing this packet
if(pkt_type(&pkt_rx) == APPLICATION) // its an application layer packet
{
// case 1: Destination is NODE_ADDR or BCAST_ADDR
if(pkt_rx.dest == NODE_ADDR || pkt_rx.dest == BCAST_ADDR)
process_app_pkt(&pkt_rx, rssi);
// case 2: I am enroute to the destination
else if(pkt_rx.nextHop == NODE_ADDR)
{
if(pkt_rx.src == NODE_ADDR) // routing table corrupted
{
nrk_int_disable();
nrk_led_set(RED_LED);
while(1)
{
nrk_kprintf(PSTR("Routing table corrupted at "));
printf("%d\r\n", NODE_ADDR);
} // end while
} // end if
else
route_packet(&pkt_rx);
} // end if
// case 3: Routing tables still not made
else if(pkt_rx.nextHop == BCAST_ADDR)
route_packet(&pkt_rx);
else
; // drop all other packets
} // end if(type == APPLICATION)
else
{
if(pkt_type(&pkt_rx) == NW_CONTROL) // its a network control packet
{
// case 1: Destination is NODE_ADDR or BCAST_ADDR
if(pkt_rx.dest == NODE_ADDR || pkt_rx.dest == BCAST_ADDR)
process_nw_ctrl_pkt(&pkt_rx, rssi);
// case 2: I am enroute to a destination
else if(pkt_rx.nextHop == NODE_ADDR)
{
if(pkt_rx.src == NODE_ADDR) // routing table corrupted
{
nrk_int_disable();
nrk_led_set(RED_LED);
while(1)
{
nrk_kprintf(PSTR("Routing table corrupted at "));
printf("%d\r\n", NODE_ADDR);
} // end while
} // end if
else
route_packet(&pkt_rx);
} // end if
// case 3: Routing tables still not made
else if(pkt_rx.nextHop == BCAST_ADDR)
route_packet(&pkt_rx);
else
; // drop all other packets
} // end if(type == NW_CONTROL)
else // unknown packet type
{
nrk_kprintf(PSTR("NL: Unknown pkt type received = "));
printf("%d\r\n", pkt_type(&pkt_rx));
}
}
nrk_led_clr(GREEN_LED);
} // end while(1)
return;
} // end nl_rx_task
void inline _nrk_scheduler()
{
int8_t task_ID;
uint16_t next_wake;
//uint16_t start_time_stamp;
/* _nrk_precision_os_timer_reset(); */
/* nrk_int_disable(); */ // this should be removed... Not needed
#ifndef NRK_NO_BOUNDED_CONTEXT_SWAP
_nrk_high_speed_timer_reset();
start_time_stamp=_nrk_high_speed_timer_get();
#endif
_nrk_set_next_wakeup(MAX_SCHED_WAKEUP_TIME);
// Set to huge number which will later get set to min
next_wake=60000;
// Safety zone starts here....
#ifdef NRK_WATCHDOG
nrk_watchdog_reset();
#endif
#ifdef NRK_SW_WDT
_nrk_sw_wdt_check();
#endif
#ifdef NRK_KERNEL_TEST
//nrk_kprintf( PSTR("*"));
//Check if OS tick was delayed...
// if(_nrk_cpu_state!=CPU_SLEEP && _nrk_os_timer_get()!=0) {
// nrk_kprintf( PSTR("X" ));
//printf( "%u ",_nrk_os_timer_get());
// }
//printf( "%u\r\n",_nrk_prev_timer_val);
if((_nrk_cpu_state!=CPU_ACTIVE) && (_nrk_os_timer_get()>nrk_max_sleep_wakeup_time))
nrk_max_sleep_wakeup_time=_nrk_os_timer_get();
#endif
//while(_nrk_time_trigger>0)
//{
nrk_system_time.nano_secs+=((uint32_t)_nrk_prev_timer_val*NANOS_PER_TICK);
nrk_system_time.nano_secs-=(nrk_system_time.nano_secs%(uint32_t)NANOS_PER_TICK);
#ifdef NRK_STATS_TRACKER
if(nrk_cur_task_TCB->task_ID==NRK_IDLE_TASK_ID)
{
if(_nrk_cpu_state==CPU_SLEEP) _nrk_stats_sleep(_nrk_prev_timer_val);
_nrk_stats_task_preempted(nrk_cur_task_TCB->task_ID, _nrk_prev_timer_val);
// Add 0 time since the preempted call before set the correct value
_nrk_stats_task_suspend(nrk_cur_task_TCB->task_ID, 0);
}
else
{
if(nrk_cur_task_TCB->suspend_flag==1)
_nrk_stats_task_suspend(nrk_cur_task_TCB->task_ID, _nrk_prev_timer_val);
else
_nrk_stats_task_preempted(nrk_cur_task_TCB->task_ID, _nrk_prev_timer_val);
}
#endif
while(nrk_system_time.nano_secs>=NANOS_PER_SEC)
{
nrk_system_time.nano_secs-=NANOS_PER_SEC;
nrk_system_time.secs++;
nrk_system_time.nano_secs-=(nrk_system_time.nano_secs%(uint32_t)NANOS_PER_TICK);
}
// _nrk_time_trigger--;
//}
if(nrk_cur_task_TCB->suspend_flag==1 && nrk_cur_task_TCB->task_state!=FINISHED)
{
// nrk_cur_task_TCB->task_state = EVENT_SUSPENDED;
if(nrk_cur_task_TCB->event_suspend==RSRC_EVENT_SUSPENDED)
nrk_cur_task_TCB->task_state = EVENT_SUSPENDED;
else if( nrk_cur_task_TCB->event_suspend>0 && nrk_cur_task_TCB->nw_flag==0)
nrk_cur_task_TCB->task_state = EVENT_SUSPENDED;
else if( nrk_cur_task_TCB->event_suspend>0 && nrk_cur_task_TCB->nw_flag==1)
nrk_cur_task_TCB->task_state = SUSPENDED;
else
{
nrk_cur_task_TCB->task_state = SUSPENDED;
nrk_cur_task_TCB->event_suspend=0;
nrk_cur_task_TCB->nw_flag=0;
}
nrk_rem_from_readyQ(nrk_cur_task_TCB->task_ID);
}
// nrk_print_readyQ();
// Update cpu used value for ended task
// If the task has used its reserve, suspend task
// Don't disable IdleTask which is 0
// Don't decrease cpu_remaining if reserve is 0 and hence disabled
if(nrk_cur_task_TCB->cpu_reserve!=0 && nrk_cur_task_TCB->task_ID!=NRK_IDLE_TASK_ID)
{
// Update CASH and cpu_remaining
// First use up any available CASH budget
uint8_t ticksToAccountFor = _nrk_prev_timer_val;
nrk_budget_t *budgetFromCASH = nrk_peek_budget();
while (ticksToAccountFor > 0 && budgetFromCASH) {
// We've found some cash budget
uint8_t availableCASH = budgetFromCASH->amount_left;
nrk_time_t system_time;
nrk_time_get(&system_time);
// We need to look at the deadline for the cash budget
// If it has passed, we can only use the portion that came before the deadline
if (nrk_time_compare(&system_time, &budgetFromCASH->expire_time) == 1) {
nrk_time_t difference;
nrk_time_sub(&difference, system_time, budgetFromCASH->expire_time);
uint8_t differenceInTicks = _nrk_time_to_ticks(&difference);
// Check if it expired before we got a chance to use it
if (differenceInTicks > ticksToAccountFor) {
availableCASH = 0;
} else {
uint8_t usableBudget = ticksToAccountFor - _nrk_time_to_ticks(&difference);
// Take the minimum of available vs usable
availableCASH = usableBudget < availableCASH ? usableBudget : availableCASH;
}
// pop it off the queue. Expired now.
nrk_get_budget();
}
if (availableCASH > ticksToAccountFor) {
budgetFromCASH->amount_left -= ticksToAccountFor;
ticksToAccountFor = 0;
} else {
ticksToAccountFor -= availableCASH;
// Pop the now empty cash budget off the queue
nrk_get_budget();
}
budgetFromCASH = budgetFromCASH->Next;
}
// If we still have ticks to account for, take them off cpu_remaining
if (ticksToAccountFor > 0) {
nrk_cur_task_TCB->cpu_remaining -= ticksToAccountFor;
}
// For finished tasks that still have cpu remaining, give it to the CASH queue
if (nrk_cur_task_TCB->task_state==FINISHED && nrk_cur_task_TCB->cpu_remaining > 0) {
// Add to CASH queue
nrk_add_nrk_budget(nrk_cur_task_TCB->absolute_deadline, nrk_cur_task_TCB->cpu_remaining);
} else if (nrk_cur_task_TCB->cpu_remaining ==0 ) {
// Support for Constant Bandwith Servers
if(nrk_cur_task_TCB->task_type == CBS_TASK)
{
// Recharge budget
nrk_cur_task_TCB->cpu_remaining = nrk_cur_task_TCB->cpu_reserve;
// Increase the absolute deadline
nrk_time_t increase = _nrk_ticks_to_time(nrk_cur_task_TCB->period);
nrk_time_add(&nrk_cur_task_TCB->absolute_deadline, nrk_cur_task_TCB->absolute_deadline, increase);
// Remove/re-add from ready queue to re-sort based on new absolute deadline
nrk_rem_from_readyQ(nrk_cur_task_TCB->task_ID);
nrk_add_to_readyQ(nrk_cur_task_TCB->task_ID);
}
else
{
#ifdef NRK_STATS_TRACKER
_nrk_stats_add_violation(nrk_cur_task_TCB->task_ID);
#endif
nrk_kernel_error_add(NRK_RESERVE_VIOLATED,nrk_cur_task_TCB->task_ID);
nrk_cur_task_TCB->task_state = SUSPENDED;
nrk_rem_from_readyQ(nrk_cur_task_TCB->task_ID);
}
}
}
// Check I/O nrk_queues to add tasks with remaining cpu back...
// Add eligable tasks back to the ready Queue
// At the same time find the next earliest wakeup
for (task_ID=0; task_ID < NRK_MAX_TASKS; task_ID++)
{
if(nrk_task_TCB[task_ID].task_ID==-1) continue;
nrk_task_TCB[task_ID].suspend_flag=0;
if( nrk_task_TCB[task_ID].task_ID!=NRK_IDLE_TASK_ID && nrk_task_TCB[task_ID].task_state!=FINISHED )
{
if( nrk_task_TCB[task_ID].next_wakeup >= _nrk_prev_timer_val )
nrk_task_TCB[task_ID].next_wakeup-=_nrk_prev_timer_val;
else
{
nrk_task_TCB[task_ID].next_wakeup=0;
}
// Do next period book keeping.
// next_period needs to be set such that the period is kept consistent even if other
// wait until functions are called.
if( nrk_task_TCB[task_ID].next_period >= _nrk_prev_timer_val )
nrk_task_TCB[task_ID].next_period-=_nrk_prev_timer_val;
else
{
if(nrk_task_TCB[task_ID].period>_nrk_prev_timer_val)
nrk_task_TCB[task_ID].next_period= nrk_task_TCB[task_ID].period-_nrk_prev_timer_val;
else
nrk_task_TCB[task_ID].next_period= _nrk_prev_timer_val % nrk_task_TCB[task_ID].period;
}
if(nrk_task_TCB[task_ID].next_period==0) nrk_task_TCB[task_ID].next_period=nrk_task_TCB[task_ID].period;
}
// Look for Next Task that Might Wakeup to interrupt current task
if (nrk_task_TCB[task_ID].task_state == SUSPENDED )
{
// printf( "Task: %d nw: %d\n",task_ID,nrk_task_TCB[task_ID].next_wakeup);
// If a task needs to become READY, make it ready
if (nrk_task_TCB[task_ID].next_wakeup == 0)
{
// printf( "Adding back %d\n",task_ID );
if(nrk_task_TCB[task_ID].event_suspend>0 && nrk_task_TCB[task_ID].nw_flag==1) nrk_task_TCB[task_ID].active_signal_mask=SIG(nrk_wakeup_signal);
//if(nrk_task_TCB[task_ID].event_suspend==0) nrk_task_TCB[task_ID].active_signal_mask=0;
nrk_task_TCB[task_ID].event_suspend=0;
nrk_task_TCB[task_ID].nw_flag=0;
nrk_task_TCB[task_ID].suspend_flag=0;
if(nrk_task_TCB[task_ID].num_periods==1)
{
nrk_task_TCB[task_ID].cpu_remaining = nrk_task_TCB[task_ID].cpu_reserve;
nrk_task_TCB[task_ID].task_state = READY;
nrk_task_TCB[task_ID].next_wakeup = nrk_task_TCB[task_ID].next_period;
// If there is no period set, don't wakeup periodically
if(nrk_task_TCB[task_ID].period==0) nrk_task_TCB[task_ID].next_wakeup = MAX_SCHED_WAKEUP_TIME;
nrk_add_to_readyQ(task_ID);
}
else
{
nrk_task_TCB[task_ID].cpu_remaining = nrk_task_TCB[task_ID].cpu_reserve;
//nrk_task_TCB[task_ID].next_wakeup = nrk_task_TCB[task_ID].next_period;
//nrk_task_TCB[task_ID].num_periods--;
nrk_task_TCB[task_ID].next_wakeup = (nrk_task_TCB[task_ID].period*(nrk_task_TCB[task_ID].num_periods-1));
nrk_task_TCB[task_ID].next_period = (nrk_task_TCB[task_ID].period*(nrk_task_TCB[task_ID].num_periods-1));
if(nrk_task_TCB[task_ID].period==0) nrk_task_TCB[task_ID].next_wakeup = MAX_SCHED_WAKEUP_TIME;
nrk_task_TCB[task_ID].num_periods=1;
// printf( "np = %d\r\n",nrk_task_TCB[task_ID].next_wakeup);
// nrk_task_TCB[task_ID].num_periods=1;
}
}
if(nrk_task_TCB[task_ID].next_wakeup!=0 &&
nrk_task_TCB[task_ID].next_wakeup<next_wake )
{
// Find closest next_wake task
next_wake=nrk_task_TCB[task_ID].next_wakeup;
}
}
}
#ifdef NRK_STATS_TRACKER
_nrk_stats_task_start(nrk_cur_task_TCB->task_ID);
#endif
task_ID = nrk_get_high_ready_task_ID();
nrk_high_ready_prio = nrk_task_TCB[task_ID].task_prio;
nrk_high_ready_TCB = &nrk_task_TCB[task_ID];
// next_wake should hold next time when a suspended task might get run
// task_ID holds the highest priority READY task ID
// So nrk_task_TCB[task_ID].cpu_remaining holds the READY task's end time
// Now we pick the next wakeup (either the end of the current task, or the possible resume
// of a suspended task)
if(task_ID!=NRK_IDLE_TASK_ID)
{
// You are a non-Idle Task
if(nrk_task_TCB[task_ID].cpu_reserve!=0 && nrk_task_TCB[task_ID].cpu_remaining<MAX_SCHED_WAKEUP_TIME)
{
if(next_wake>nrk_task_TCB[task_ID].cpu_remaining)
next_wake=nrk_task_TCB[task_ID].cpu_remaining;
}
else
{
if(next_wake>MAX_SCHED_WAKEUP_TIME) next_wake=MAX_SCHED_WAKEUP_TIME;
}
}
else
{
// This is the idle task
// Make sure you wake up from the idle task a little earlier
// if you would go into deep sleep...
// After waking from deep sleep, the next context swap must be at least
// NRK_SLEEP_WAKEUP_TIME-1 away to make sure the CPU wakes up in time.
#ifndef NRK_NO_POWER_DOWN
if(next_wake>NRK_SLEEP_WAKEUP_TIME)
{
if(next_wake-NRK_SLEEP_WAKEUP_TIME<MAX_SCHED_WAKEUP_TIME)
{
if(next_wake-NRK_SLEEP_WAKEUP_TIME<NRK_SLEEP_WAKEUP_TIME)
{
next_wake=NRK_SLEEP_WAKEUP_TIME-1;
}
else
{
next_wake=next_wake-NRK_SLEEP_WAKEUP_TIME;
}
}
else if(next_wake>NRK_SLEEP_WAKEUP_TIME+MAX_SCHED_WAKEUP_TIME)
{
next_wake=MAX_SCHED_WAKEUP_TIME;
}
else
{
next_wake=MAX_SCHED_WAKEUP_TIME-NRK_SLEEP_WAKEUP_TIME;
}
}
#endif
}
/*
// Some code to catch the case when the scheduler wakes up
// from deep sleep and has to execute again before NRK_SLEEP_WAKEUP_TIME-1
if(_nrk_cpu_state==2 && next_wake<NRK_SLEEP_WAKEUP_TIME-1)
{
nrk_int_disable();
while(1)
{
nrk_spin_wait_us(60000);
nrk_led_toggle(RED_LED);
nrk_spin_wait_us(60000);
nrk_led_toggle(GREEN_LED);
printf( "crash: %d %d %d\r\n",task_ID,next_wake,_nrk_cpu_state);
}
}*/
// If we disable power down, we still need to wakeup before the overflow
#ifdef NRK_NO_POWER_DOWN
if(next_wake>MAX_SCHED_WAKEUP_TIME) next_wake=MAX_SCHED_WAKEUP_TIME;
#endif
//printf( "nw = %d %d %d\r\n",task_ID,_nrk_cpu_state,next_wake);
nrk_cur_task_prio = nrk_high_ready_prio;
nrk_cur_task_TCB = nrk_high_ready_TCB;
#ifdef NRK_KERNEL_TEST
if(nrk_high_ready_TCB==NULL)
{
nrk_kprintf( PSTR( "KERNEL TEST: BAD TCB!\r\n" ));
}
#endif
//printf( "n %u %u %u %u\r\n",task_ID, _nrk_prev_timer_val, next_wake,_nrk_os_timer_get());
_nrk_prev_timer_val=next_wake;
if((_nrk_os_timer_get()+1)>=next_wake) // just bigger then, or equal?
{
// FIXME: Terrible Terrible...
// Need to find out why this is happening...
#ifdef NRK_KERNEL_TEST
// Ignore if you are the idle task coming from deep sleep
if(!(task_ID==NRK_IDLE_TASK_ID && _nrk_cpu_state==CPU_SLEEP))
nrk_kernel_error_add(NRK_WAKEUP_MISSED,task_ID);
#endif
// This is bad news, but keeps things running
// +2 just in case we are on the edge of the last tick
next_wake=_nrk_os_timer_get()+2;
_nrk_prev_timer_val=next_wake;
}
if(task_ID!=NRK_IDLE_TASK_ID) _nrk_cpu_state=CPU_ACTIVE;
_nrk_set_next_wakeup(next_wake);
#ifndef NRK_NO_BOUNDED_CONTEXT_SWAP
// Bound Context Swap to 100us
nrk_high_speed_timer_wait(start_time_stamp,CONTEXT_SWAP_TIME_BOUND);
#endif
nrk_stack_pointer_restore();
//nrk_int_enable();
nrk_start_high_ready_task();
}
static void discover_task ()
{
uint8_t periods_in_idle = discover_period_s / DISCOVER_TASK_PERIOD_S;
nrk_time_t now, elapsed;
int8_t rc;
while (1) {
switch (discover_state) {
case DISCOVER_IDLE:
if (auto_discover) {
if (periods_in_idle < periods_in_idle)
break;
set_state(DISCOVER_SCHEDULED);
}
break;
case DISCOVER_SCHEDULED:
#if ENABLE_LED
pulse_led(led_discover);
#endif
set_state(DISCOVER_PENDING);
nrk_event_signal(discover_signal);
break;
case DISCOVER_IN_PROGRESS:
nrk_time_get(&now);
nrk_time_sub(&elapsed, now, last_activity);
if (time_cmp(&elapsed, &discover_time_out) < 0) {
LOG("silent for ");
LOGP("%lu ms\r\n", TIME_TO_MS(elapsed));
break;
}
LOG("finished, distrib routes: seq ");
LOGP("%d\r\n", outstanding_seq);
print_graph(&network);
rc = calc_routes(&network, &routes);
if (rc == NRK_OK) {
print_routes(&routes);
rc = broadcast_routes(&routes, outstanding_seq);
if (rc != NRK_OK)
LOG("WARN: failed to bcast routes\r\n");
} else {
LOG("WARN: failed to calc routes\r\n");
}
set_state(DISCOVER_COMPLETED);
nrk_event_signal(discover_signal);
break;
case DISCOVER_PENDING:
case DISCOVER_COMPLETED:
/* the router failed to do its part in one task period */
set_state(DISCOVER_IDLE);
break;
default:
ABORT("unexpected state\r\n");
}
periods_in_state++;
nrk_wait_until_next_period();
}
ABORT("discover task exited\r\n");
}
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;
}
