static void check_microstrain
(
uint8_t XDATA *context
)
{
if(!microstrain_available)
{
printf("\r\nMicrostrain not connected\r\n");
return;
}
microstrain_request_all_data();
microstrain_update_angular_velocity();
microstrain_available-=1;
if(!gaugeData.initialized)
{
if(gaugeData.lastYaw[0] != INVALID_YAW)
{
gaugeData.initialized = 1;
//sch_create_timeout( rtc_get_ticks()+ANG_VEL_UPD_PERIOD, update_microstrain, NULL);
next_update_time = rtc_get_ticks()+ANG_VEL_UPD_PERIOD;
}
else
{
sch_create_timeout( rtc_get_ticks()+MICROSTRAIN_CHECK_PERIOD, check_microstrain, NULL);
}
}
}
/**
* cc_loop() - executes main loop block (BUT DOES NOT LOOP ITSELF!!!)
*/
void cc_loop( void )
{
#ifdef FEAT_ENABLE_CC
// Check if need to do the periodic update of Congestion Control
if ( ( cc_processing_timeout_ < rtc_get_ticks() ) )
{
cc_end_of_period ();
}
#endif
#ifdef INCLUDE_PCC
if ( ( 1 == DATA_waiting_4_BO_ ) && ( phy_backoff_timeout_ < rtc_get_ticks() ) )
{
DATA_waiting_4_BO_ = 0;
#ifdef _ENABLE_NETWORK_STACK_
phy_transmit_packet();
#endif // _ENABLE_NETWORK_STACK_
}
#endif
#if (0)
//FEAT_ENABLE_CH_SWITCHING
if ( (CS_ENABLED == cs_enabled_) && (cs_switching_timeout_ < rtc_get_ticks() ) )
{
cs_switching_timeout();
cs_send_join ( 0xFE, 0x11 );
}
#endif
}
void cc_end_of_period ()
{
float throughput;
float alpha_delta ;
// Check if received duplicates of the period request
if ( rtc_get_ticks() - cc_last_period_tick_ < CC_MIN_PERIOD )
{
return;
}
cc_period_length_ = rtc_get_ticks() - cc_last_period_tick_;
cc_last_period_tick_ = rtc_get_ticks();
// throughput = cc_sent_pkts_ / cc_timeout_divider_;
throughput = (float)(cc_sent_pkts_ * DEFAULT_ONE_SECOND) / (float)cc_period_length_;
cc_flow_error_ = throughput - ( float ) cc_predicted_throughput_;
alpha_delta = cc_flow_gamma_ * throughput * cc_flow_error_;
// alpha_delta = cc_flow_gamma_ * cc_throughput_ * cc_flow_error_;
cc_flow_alpha_ += alpha_delta;
cc_flow_alpha_ = MAX ( cc_flow_alpha_, MIN_FLOW_ALPHA );
cc_flow_alpha_ = MIN ( cc_flow_alpha_, MAX_FLOW_ALPHA );
cc_predicted_throughput_ = ( uint16_t ) ( throughput * cc_flow_alpha_ );
cc_previous_throughput_ = cc_throughput_;
cc_throughput_ = ( uint16_t ) throughput;
cc_droprate_ = cc_drop_pkts_ * DEFAULT_ONE_SECOND / cc_period_length_;
cc_processing_timeout_ = rtc_get_ticks() + cc_timeout_length_;
cc_generated_pkts_ = 0;
cc_sent_pkts_ = 0;
cc_recv_pkts_ = 0;
cc_drop_pkts_ = 0;
// Delayed setting of the timeoutr divider to accomodate for
// change of timeout in the middle of timeout period
cc_timeout_divider_ = cc_timeout_length_ / DEFAULT_ONE_SECOND;
/////// !!!!!! how to enable case when the PCC upstrema is comming a bit earlier then own PERIOD end
// cc_recv_outflow_limit_ = CC_MAX_OUT_FLOW; // reset limit -> if will receive the update then keep it
// Calculate and send the info only when the PCC backoff is used
if (PHY_BACKOFF_PCC == my_backoff_)
{
// Calculate the allowed incomming traffic
cc_calculate_allowed_inflow();
// Prepare the ConCon updates for all incomming/own flows
cc_apply_cc_to_own_flows();
cc_prepare_cc_messages_for_inflows();
cc_recalculate_BO_params();
}
}
void microstrain_init
(
void
)
{
sch_add_loop((sch_loop_func_t)microstrain_loop);
sch_create_timeout( rtc_get_ticks()+1, check_microstrain, NULL);
}
/**
* ssn_time_offset(start_ticks) -
*
*/
uint16_t ssn_time_offset(uint32_t start_ticks)
{
uint16_t off;
uint32_t d = rtc_get_ticks() - start_ticks;
d = d * ssn_mem_p->rate_ / 1000;
off = (uint16_t)d;
return off;
}
/**
* ssn_loop() - executes main loop block (BUT DOES NOT LOOP ITSELF!!!)
*/
void ssn_loop( void )
{
// ssn_mem_p->sample_interval_ = (ssn_mem_p->last_tick_ - ssn_mem_p->tail_tick_);
if ( 1 == ssn_mem_p->pause_)
{
if ( ssn_mem_p->pause_timeout_ < rtc_get_ticks())
{
ssn_mem_p->pause_ = 0;
}
}
else if ( 0 < ssn_mem_p->burst_enabled_)
{
if ( ( 0 < (ssn_mem_p->head_ - ssn_mem_p->next_pkt_) )
&& ( 0xFF00 > (ssn_mem_p->head_ - ssn_mem_p->next_pkt_) ) )
{
if (0 == ssn_mem_p->start_time_)
{
ssn_mem_p->start_time_ = rtc_get_ticks();
}
if (0 == ssn_send_pkt(SSN_SPP))
{
ssn_mem_p->pause_ = 1;
ssn_mem_p->pause_timeout_ = rtc_get_ticks() + SSN_PAUSE_DELAY;
}
else
{
ssn_mem_p->burst_enabled_--;
ssn_mem_p->next_pkt_ += SSN_SPP;
ssn_mem_p->start_index_ = ssn_mem_p->next_pkt_ - SSN_SPP ;
//ssn_mem_p->start_time_ = rtc_get_ticks() - (SSN_SPP*1000/ssn_mem_p->rate_);
ssn_mem_p->start_time_ = ssn_mem_p->start_time_ + (1000UL*SSN_SPP/(uint32_t)ssn_mem_p->rate_);
}
}
}
else
{
if ( ssn_mem_p->burst_timeout_ > rtc_get_ticks())
{
ssn_mem_p->burst_enabled_ = 10;
}
// adc_suspendADC();
}
// If packet is filled then sent out
}
static void microstrain_loop
(
void
)
{
if(is_microstrain_active())
{
uint8_t ser_data;
if ( serialReadByte( &ser_data ) )
{
microstrain_fsm(ser_data);
}
if(gaugeData.initialized)
{
if(next_update_time < rtc_get_ticks())
{
next_update_time = rtc_get_ticks()+ANG_VEL_UPD_PERIOD;
update_microstrain(NULL);
}
}
}
}
/**
* ssn_execute_command(packet) - executes a command received in "packet"
*/
void ssn_execute_command ( uint8_t *packet)
{
ssn_command_mod_v1_t *cmd = (ssn_command_mod_v1_t*)packet;
switch (cmd->command)
{
case SSN_CMD_SET:
// set interval and other params
return;
//break;
case SSN_CMD_REQ:
// ssn_mem_p->start_time_ = cmd->ticks;
// ssn_mem_p->start_index_ = ssn_mem_p->head_ - ssn_time_offset(cmd->ticks);
//adc_set_rate_divider((uint8_t)((uint16_t)20000 / cmd->rate));
ssn_mem_p->rate_ = cmd->rate;
ssn_mem_p->start_time_ = rtc_get_ticks();
ssn_mem_p->start_index_ = ssn_mem_p->head_;
ssn_mem_p->burst_timeout_ = rtc_get_ticks() + 1000UL * cmd->mod_data;
ssn_mem_p->next_pkt_ = ssn_mem_p->start_index_ + SSN_SPP;
// ssn_send_pkt(cmd->mod_data); // sample count to send
break;
}
}
static void microstrain_update_angular_velocity
(
void
)
{
float sampT;
if(gaugeData.newYaw)
{
sampT = rtc_get_ticks() - gaugeData.time;
gaugeData.time = rtc_get_ticks();
sampT /= 1000;
gaugeData.lastYaw[0] = gaugeData.lastYaw[1];
gaugeData.lastYaw[1] = gaugeData.yaw;
if( (gaugeData.lastYaw[0] > TWO_PI/4.0) && (gaugeData.lastYaw[1] < -TWO_PI/4.0)) gaugeData.lastYaw[0] -= TWO_PI;
else if( (gaugeData.lastYaw[0] < -TWO_PI/4.0) && (gaugeData.lastYaw[1] > TWO_PI/4.0)) gaugeData.lastYaw[0] += TWO_PI;
gaugeData.AngV = (gaugeData.lastYaw[1] - gaugeData.lastYaw[0])/sampT;
gaugeData.newYaw = 0;
}
return;
}
int main( int argc, char* argv[] ) {
// setup COM2/terminal
bwsetfifo( COM2, OFF );
// setup peripherals and handlers
kernel_init();
// Create idle task
struct task* idle_task = task_create(&idle_task_entry, PRIORITY_IDLE, -1);
scheduler_add(idle_task);
// bootstrap first user task
struct task* first_task = task_create(&first_task_entry, PRIORITY_HIGHEST, -1);
scheduler_add(first_task);
long idleTicks = 0;
long userTicks = 0;
struct task *current_task;
int request;
long elapsed;
while (1) {
current_task = scheduler_get_next();
if (current_task == 0) {
break;
}
ACTIVATE:
// Time the execution of the task
elapsed = rtc_get_ticks();
// run the user task
request = task_activate(current_task);
elapsed = rtc_get_ticks() - elapsed;
if (current_task == idle_task) {
idleTicks += elapsed;
}
userTicks += elapsed;
switch (__interrupt_type) {
case INT_TYPE_HWI:
// handle the interrupt request and determine if we should preempt the running task
if (interrupts_handle_irq(current_task) == HWI_CONTINUE && current_task != idle_task) {
goto ACTIVATE;
} else {
scheduler_add(current_task);
}
break;
case INT_TYPE_SWI:
// handle the system call request and determine if we should shutdown
if (sys_handle_request(current_task, request) == -1) {
goto shutdown;
}
break;
default:
KASSERT(0, "We got a weird interrupt type... %d", __interrupt_type);
break;
}
}
shutdown:
kernel_shutdown();
// reset the scrolling window, in case it exists!
bwprintf(COM2, CURSOR_SCROLL, 0, 999);
bwprintf(COM2, CURSOR_CLEAR_SCREEN);
// int i;
// for (i = 0; i < MAX_NUM_TASKS; i++) {
// task_print(COM2, i);
// }
printSensorTicks();
bwprintf(COM2, "Idle time: %d%%\r\n", ((idleTicks * 100) / userTicks));
bwprintf(COM2, "Kernel exiting.\r\n");
return 0;
}
/*******************************************************************************
* Function Name : radio_loop
* Description : When Radio is idle then checks for packets to be transmitted
* Input : None
* Output : None
* Return : None
*******************************************************************************/
void radio_loop()
{
// if the TX is idle then dequeue next packet and start TX
CLEAR_LED(RLED);
if ( 1 == stradio_retransmit_req_)
{
#if defined(_ENABLE_XBEE_COMPAT_4BS_TX_) || defined(_FORCE_XBEE_COMPAT_TX_)
#ifdef _FORCE_XBEE_COMPAT_TX_
if (1)
#else
if (BS_ADDR == txPacket[6])
#endif //_FORCE_XBEE_COMPAT_TX_
{
//txPacket[10]++;// = txPacket[3]; /* XBee COMPATIBILITY - increment sequence number */
txPacket[3]++;
}
#endif // defined(_ENABLE_XBEE_COMPAT_4BS_TX_) || defined(_FORCE_XBEE_COMPAT_TX_)
int temp_ret = ST_RadioTransmit(txPacket);
if (ST_SUCCESS != temp_ret)
{
txComplete = TRUE; // FAILED!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!
stradio_retransmision_result = temp_ret;
}
else
{
txComplete = FALSE;
stradio_count_failed_retransmissions_++;
}
stradio_retransmit_req_ = 0;
} else
if (FALSE == txComplete)
{
SET_LED(RLED);
count_stalled++;
if (count_stalled > stalled_reset)
{
//stalled_reset = count_stalled + STALLED_THRESHOLD;
//txComplete = TRUE;
}
} else if ( 0 != stradio_pending_len_ )
{
sendPacketData( stradio_pending_len_, stradio_pending_data_, stradio_pending_dst_);
stradio_pending_len_ = 0;
} else if ( ( 0 == pkt_to_sent_len ) && ( 0xFF == pkt_to_sent_id ) )
{
stalled_reset++;
pkt_to_sent_id = que_deQpackets();
if ( 0xFF != pkt_to_sent_id )
{
char routed = 0;
unsigned int base = QBUFF_BASE ( pkt_to_sent_id );
// fill the TX-related variables
pkt_to_sent_len = PAK_GET_TOTAL_LENGTH ( pkt_to_sent_id );
// routing decisions for the packet
routed = routing_send_DATA_base ( QBUFF_BASE ( pkt_to_sent_id ) );
// If routing OK then start transmission process -> backoff
if ( 1 == routed )
{
unsigned int mac_d = get_dst_mac_base ( base );
sent_DATA_ = 1;
sendPacketData(pkt_to_sent_len, (sint8_t*)&(QBUFF_ACCESS(base, 0)) , mac_d ); // send via the backoff implementation
phy_sent_timeout_ = rtc_get_ticks() + my_tx_timeout_;
SET_LED(YLED);
}
else
{
if (ROUTING_BEGAN_ROUTE_DISCOVERY == routed)
{
// re-enqueue the packet
if (0 == que_enQpacket (pkt_to_sent_id))
{
release_pkt_in_tx();
}
else
{
pkt_to_sent_len = 0;
pkt_to_sent_id = 0xFF;
}
}
else
{
#ifdef _ENABLE_APP_MOD_
app_drop_pkt ( pkt_to_sent_id, MODULE_RTR, REASON_NOROUTE, EVENT_DSEND );
#endif // _ENABLE_APP_MOD_
// drop packet if not routable
release_pkt_in_tx();
}
}
}
}
}
void cc_init ( bit full_reset )
{
if ( full_reset )
{
cc_timeout_length_ = DEFAULT_CC_TIMEOUT;
}
cc_processing_timeout_ = rtc_get_ticks() + cc_timeout_length_;
cc_generated_pkts_ = 0;
cc_period_length_ = 1;
cc_last_period_tick_ = rtc_get_ticks();
cc_sent_pkts_ = 0;
cc_recv_pkts_ = 0;
cc_drop_pkts_ = 0;
cc_timeout_divider_ = cc_timeout_length_ / DEFAULT_ONE_SECOND;
cc_throughput_ = 0;
cc_predicted_throughput_ = 0;
cc_previous_throughput_ = 0;
cc_droprate_ = 0;
cc_flow_alpha_ = 1.0;
cc_flow_gamma_ = 0.001;
cc_kv_queue_gain_ = 0.9;
cc_ideal_queue_ = CC_TARGET_QUEUE;
cc_recv_outflow_limit_ = CC_MAX_OUT_FLOW;
cc_calculated_inflow_limit_ = cc_ideal_queue_; // initially count in only the ideal queue level
cc_ownflow_limit_ = CC_MAX_OUT_FLOW;
cc_target_outflow_ = cc_calculated_inflow_limit_;
cc_previous_target_outflow_ = cc_calculated_inflow_limit_;
cc_bo_alpha_ = 1.0;
cc_bo_sigma_ = 0.01;
cc_bo_outflow_error_ = 0;
cc_bo_kv_ = 0.1;
cc_bo_value_ = PCC_DEFAULT_BO_VALUE;
cc_need_to_send_upstream_ = PCC_JNL_NUMBER_OF_UPSTREAM;
cc_upstream_nodes_count_ = PCC_JNL_NUMBER_OF_UPSTREAM; // number of nodes in the below tables
#ifdef SOURCE
cc_upstream_nodes_count_--;
#endif
{
uint8_t i;
for ( i = 0; i < MAX_UPSTREAM_NODES; i++ )
{
cc_upstream_nodes_[MAX_UPSTREAM_NODES] = 0x0F; // MAC address of upstraem nodes
cc_upstream_nodes_status_[MAX_UPSTREAM_NODES] = 0x0F; // status (need to send limit, sending limit, done sending)
cc_upstream_nodes_frame_id_[MAX_UPSTREAM_NODES] = 0x0F;
}
}
#ifdef NODE_5_PCC_RELAY
cc_upstream_nodes_[0] = 0x01;
cc_upstream_nodes_[1] = 0x02;
#endif
#ifdef NODE_7_PCC_SOURCE
cc_upstream_nodes_[0] = 0x02;
#endif
sch_add_loop((sch_loop_func_t)cc_loop);
}
