static int8_t nbr_msg_handler(void *state, Message *msg)
{
nbr_state_t *s = (nbr_state_t*)state;
/**
* Switch to the correct message handler
*/
switch (msg->type){
case MSG_INIT:
{
s->nb_list = NULL;
s->nb_cnt = 0;
s->est_ticks = 0;
s->gCurrentSeqNo = 0;
sys_shm_open( sys_shm_name(NBHOOD_PID, SHM_NBR_LIST), s->nb_list );
sys_timer_start(BACKOFF_TIMER, sys_rand() % 1024L, TIMER_ONE_SHOT);
break;
}
case MSG_FINAL:
{
break;
}
case MSG_TIMER_TIMEOUT:
{
if( timer_get_tid( msg ) == BACKOFF_TIMER ) {
sys_timer_start(NEIGHBOR_DISCOVERY_TIMER, NEIGHBOR_TIMER_INTERVAL, TIMER_REPEAT);
} else {
//
// Send beacon packets
//
update_table( s );
sys_shm_update( sys_shm_name(NBHOOD_PID, SHM_NBR_LIST), s->nb_list );
send_beacon( s );
nb_debug(s);
}
break;
}
case MSG_BEACON_PKT:
{
//
// Process beacon packets
//
recv_beacon( s, msg );
break;
}
default:
return -EINVAL;
}
/**
* Return SOS_OK for those handlers that have successfully been handled.
*/
return SOS_OK;
}
static int8_t pre_blink_msg_handler(void *state, Message *msg)
{
switch (msg->type){
case MSG_INIT:
{
sys_timer_start(BLINK_TID, BLINK_TIMER_INTERVAL, TIMER_REPEAT);
break;
}
case MSG_FINAL:
{
sys_timer_stop(BLINK_TID);
break;
}
case MSG_TIMER_TIMEOUT:
{
sys_led(LED_GREEN_TOGGLE);
break;
}
default:
return -EINVAL;
}
return SOS_OK;
}
static int8_t _sos_handler ( void *state, Message *msg )
{
switch ( msg->type ) {
case MSG_TIMER_TIMEOUT: {
sys_sensor_get_data(MTS310_PHOTO_SID);
return SOS_OK;
}
case MSG_DATA_READY: {
MsgParam* param = (MsgParam*) (msg->data);
uint16_t *data;
data = sys_malloc(sizeof(uint16_t));
*data = param->word;
sys_post_uart(DFLT_APP_ID0, 32, 2, data, SOS_MSG_RELEASE, BCAST_ADDRESS);
return SOS_OK;
}
case MSG_INIT: {
sys_timer_start ( 0, 64L, TIMER_REPEAT );
return SOS_OK;
}
case MSG_FINAL: {
return SOS_OK;
}
}
return -EINVAL;
}
static int8_t blink_msg_handler(Message *msg)
{
switch (msg->type){
case 1:
{
pid = msg->did;
blink_state = 0;
printf("LED_RED_TOGGLE\n");
sys_timer_start(0, 1024L, 1);
break;
}
case 2:
{
sys_timer_stop(0);
printf("Blink Stop\n");
break;
}
case 3:
{
printf("LED_GREEN_TOGGLE");
break;
}
default:
return -1;
}
return 0;
}
static void start_sync() {
app_state_t* s = (app_state_t*)sys_get_state();
LED_DBG(LED_YELLOW_TOGGLE);
tpsn_req_t* tpsn_req_ptr = (tpsn_req_t *) sys_malloc(sizeof(tpsn_req_t));
tpsn_req_ptr->type = TPSN_REQUEST;
tpsn_req_ptr->seq_no = s->current_seq_no;
DEBUG("TPSN_NET: Transmitting TIMESYNC packet to node %d with seq_no=%d\n", s->parent_id, tpsn_req_ptr->seq_no);
sys_post_net(s->pid, MSG_TIMESTAMP, sizeof(tpsn_req_t), tpsn_req_ptr, SOS_MSG_RELEASE, s->parent_id);
sys_timer_start(SYNC_TIMER_ID, 50, TIMER_ONE_SHOT);
}
static int8_t element_module(void *state, Message *msg) {
element_state_t *s = (element_state_t *)state;
switch (msg->type) {
case MSG_INIT:
{
LED_DBG(LED_RED_ON);
s->cnt = 0;
// Default parameter value = 5 sec.
s->sample_rate_in_sec = 5;
s->pid = msg->did;
sys_timer_start(TIMER_TID, ((uint32_t)s->sample_rate_in_sec) * 1024L, TIMER_ONE_SHOT);
break;
}
case MSG_TIMER_TIMEOUT:
{
LED_DBG(LED_RED_TOGGLE);
s->cnt++;
DEBUG("Timer fired. Put token %d on output port. Function CB output = 0x%x.\n",
s->cnt, s->output0);
token_type_t *my_token = create_token(&s->cnt, sizeof(uint8_t), s->pid);
if (my_token == NULL) return -ENOMEM;
//SOS_CALL(s->put_token, put_token_func_t, s->output0, my_token);
dispatch(s->output0, my_token);
destroy_token(my_token);
sys_timer_start(TIMER_TID, ((uint32_t)s->sample_rate_in_sec) * 1024L, TIMER_ONE_SHOT);
break;
}
case MSG_FINAL:
{
break;
}
default: return -EINVAL;
}
return SOS_OK;
}
static int8_t irtag_msg_handler(void *state, Message *msg)
{
app_state_t *s = (app_state_t*)state;
switch (msg->type){
case MSG_INIT:
{
s->pid = msg->did;
s->state = 0;
DEBUG("Rfidtag Start\n");
sys_timer_start(aRFID_TID,aRFID_TIMER_INTERVAL,TIMER_REPEAT);
break;
}
case MSG_FINAL:
{
DEBUG("Rfid Stop\n");
break;
}
case MSG_TIMER_TIMEOUT:
{
sys_led(LED_GREEN_TOGGLE);
if(GETBIT(code, counter) ==1)
{
sys_led(LED_RED_ON);
} else
{
sys_led(LED_RED_OFF);
}
counter --;
if(counter == -1)
{
counter = 11;
}
break;
}
default:
return -EINVAL;
}
return SOS_OK;
}
void static inline go_to_sleep(){
// stop / disable peripherals
cli(); // disable interrupts
sys_timer_stop();
_delay_us(300); // small delay in order for the colorduino to be ready for transmission
display_sleep();
while(uart_async_run()); // put display to sleep
_delay_us(60);
p_out_low();
interface_disable(); // turn leds + display off
// disable UART
UCSRnB = 0x00; //disable Rx and Tx
// enable external pin change interrupt on PA7: PCINT7
PCMSK0 |= (1<<PCINT7); // set interrupt mask for PA7
PCICR |= (1<<PCIE0); // enable pin change interrupt 0
// go to sleep
set_sleep_mode(SLEEP_MODE_STANDBY);
sleep_enable();
sei();
sleep_cpu();
sleep_disable();
// wake up
cli(); // disable interrupts
PCICR = 0x00; // disable pin change interrupt
PCMSK0 = 0x00;
// restart peripherals
UCSRnB = (1<<RXENn) | (1<<TXENn); //enable Rx and Tx
interface_init();
display_wake_up();
sys_timer_start();
green_led_on();
sei(); // reenable interrupts
}
static int8_t input0 (func_cb_ptr p, token_type_t *t) {
element_state_t *s = (element_state_t *)sys_get_state();
// Get token from port.
// If want to break the chain of calls here, then copy the token into a private
// data structure(global), and return appropriate value (SOS_OK).
// Process input: Extract 3 LSB's and pass it on to the next function.
// We need a separate place to hold the output as we are modifying the input.
// Remember, this module does not own the input token, so should not
// overwrite it.
uint8_t *data = (uint8_t *)capture_token_data(t, s->pid);
if (data == NULL) return -ENOMEM;
s->value = (*data) & s->bit_mask;
DEBUG("\n");
DEBUG("TRUNCATE_LONG ACCEPTED TOKEN %d.\n", *data);
DEBUG("\n");
destroy_token_data(data, t->type, t->length);
sys_timer_start(TIMER_PID, ((uint32_t)s->computation_time)*1024L, TIMER_ONE_SHOT);
return -EBUSY;
}
//--------------------------------------------------------------------
// EXTERNAL FUNCTIONS
//--------------------------------------------------------------------
int8_t execute(dvm_state_t* dvm_st, DvmState *eventState)
{
DVMBasiclib_state_t *s = (&dvm_st->basiclib_st);
DvmContext *context = &(eventState->context);
while ((context->state == DVM_STATE_RUN) && (context->num_executed < DVM_CPU_SLICE)) {
DvmOpcode instr = getOpcode(dvm_st, context->which, context->pc);
DEBUG("-----------------------------------------------\n");
DEBUG("[BASIC_LIB] execute: (%d) PC: %d. INSTR: %d\n",context->which, context->pc, instr);
if(instr & LIB_ID_BIT)
{ //Extension Library opcode encountered
return SOS_OK;
}
context->num_executed++;
switch(instr)
{
case OP_START:
{
__asm __volatile("__sleep_measure_start:");
context->pc += 1;
break;
}
case OP_STOP:
{
context->pc += 1;
break;
}
case OP_HALT:
{
DEBUG("<<<<<<<<<<<=====================>>>>>>>>>>>\n");
DEBUG("[BASIC_LIB] execute: (%d): HALT executed.\n", (int)context->which);
haltContext(dvm_st, context);
context->state = DVM_STATE_HALT;
context->pc = 0;
break;
}
case OP_LED:
{
DvmStackVariable* arg = popOperand( eventState);
led_op(arg->value.var);
context->pc += 1;
break;
}
case OP_GETVAR + 0:
case OP_GETVAR + 1:
case OP_GETVAR + 2:
case OP_GETVAR + 3:
case OP_GETVAR + 4:
case OP_GETVAR + 5:
case OP_GETVAR + 6:
case OP_GETVAR + 7:
{
uint8_t arg = instr - OP_GETVAR;
DEBUG("[BASIC_LIB] execute: OPGETVAR (%d):: Pushing value %d.\n", (int)arg,(int)s->shared_vars[arg].value.var);
pushOperand( eventState, &s->shared_vars[arg]);
context->pc += 1;
break;
}
case OP_SETVAR + 0:
case OP_SETVAR + 1:
case OP_SETVAR + 2:
case OP_SETVAR + 3:
case OP_SETVAR + 4:
case OP_SETVAR + 5:
case OP_SETVAR + 6:
case OP_SETVAR + 7:
{
uint8_t arg = instr - OP_SETVAR;
DvmStackVariable* var = popOperand( eventState);
DEBUG("[BASIC_LIB] execute: OPSETVAR (%d):: Setting value to %d.\n",(int)arg,(int)var->value.var);
s->shared_vars[arg] = *var;
context->pc += 1;
break;
}
case OP_GETVARF + 0:
case OP_GETVARF + 1:
case OP_GETVARF + 2:
case OP_GETVARF + 3:
case OP_GETVARF + 4:
case OP_GETVARF + 5:
case OP_GETVARF + 6:
case OP_GETVARF + 7:
{ // Use for type casting an integer shared var to float
uint8_t arg = instr - OP_GETVARF;
int32_t res = 0;
uint16_t res_part = 0;
DEBUG("[BASIC_LIB] execute: OPGETVARF (%d):: Pushing value %d.\n", (int)arg,(int)s->shared_vars[arg].value.var);
res = (int32_t)(s->shared_vars[arg].value.var * FLOAT_PRECISION);
res_part = res & 0xFFFF;
pushValue( eventState, res_part, DVM_TYPE_FLOAT_DEC);
res_part = res >> 16;
pushValue( eventState, res_part, DVM_TYPE_FLOAT);
context->pc += 1;
break;
}
case OP_SETVARF + 0:
case OP_SETVARF + 1:
case OP_SETVARF + 2:
case OP_SETVARF + 3:
case OP_SETVARF + 4:
case OP_SETVARF + 5:
case OP_SETVARF + 6:
{ // Type-casting an integer to float and saving it in shared var
uint8_t arg = instr - OP_SETVARF;
DvmStackVariable* var = popOperand( eventState);
int32_t res = 0;
uint16_t res_part;
DEBUG("[BASIC_LIB] execute: OPSETVARF (%d):: Setting value to %d.\n",(int)arg,(int)var->value.var);
res = (int32_t)(var->value.var * FLOAT_PRECISION);
res_part = res & 0xFFFF;
s->shared_vars[arg+1].type = DVM_TYPE_FLOAT_DEC;
s->shared_vars[arg+1].value.var = res_part;
res_part = res >> 16;
s->shared_vars[arg].type = DVM_TYPE_FLOAT;
s->shared_vars[arg].value.var = res_part;
context->pc += 1;
break;
}
case OP_SETTIMER + 0:
case OP_SETTIMER + 1:
case OP_SETTIMER + 2:
case OP_SETTIMER + 3:
case OP_SETTIMER + 4:
case OP_SETTIMER + 5:
case OP_SETTIMER + 6:
case OP_SETTIMER + 7:
{
uint32_t msec;
uint8_t timerID = instr - OP_SETTIMER;
DvmStackVariable* arg = popOperand( eventState);
DEBUG("[BASIC_LIB] execute: Setting Timer %d period to %d.\n", timerID, arg->value.var);
//msec = 102 * arg->value.var + (4 * arg->value.var) / 10;
// Set the timer timeout argument in seconds
msec = arg->value.var * 4;// * 1000;
DEBUG("[BASIC_LIB] execute: <<<<< WARNING - Timer %d not being stopped >>>>\n", timerID);
// sys_timer_stop(timerID);
if (msec > 0) {
// Ram - Where is the init ??
sys_timer_start(timerID, msec, TIMER_REPEAT);
DEBUG("[BASIC_LIB] execute: Timer ID: %d started. Period: %d msec.\n", timerID, msec);
}
context->pc += 1;
break;
}
case OP_RAND:
{
DvmStackVariable* arg = popOperand( eventState);
uint16_t rnd;
rnd = sys_rand() % arg->value.var;
pushValue( eventState, rnd, DVM_TYPE_INTEGER);
context->pc += 1;
break;
}
/*
case OP_JMP: case OP_JNZ: case OP_JZ: case OP_JG:
case OP_JGE: case OP_JL: case OP_JLE: case OP_JE:
case OP_JNE:
case OP_ADD: case OP_SUB: case OP_DIV: case OP_MUL:
case OP_ABS: case OP_MOD: case OP_INCR: case OP_DECR:
{
mathlib_executeDL(s->mathlib_execute, eventState, instr);
break;
}
*/
// Math Lib
case OP_ADD:
case OP_SUB:
case OP_DIV:
case OP_MUL:
{
DvmStackVariable* arg1 = popOperand( eventState);
DvmStackVariable* arg2 = popOperand( eventState);
DvmStackVariable* arg3 = NULL, *arg4 = NULL;
int32_t fl_arg1, fl_arg2;
int32_t res = 0;
uint16_t res_part;
int16_t int_res = 0;
if (arg1->type == DVM_TYPE_FLOAT) {
fl_arg1 = convert_to_float(arg1, arg2);
arg3 = popOperand( eventState);
if (arg3->type == DVM_TYPE_FLOAT) {
// FLOAT <op> FLOAT
arg4 = popOperand( eventState);
fl_arg2 = convert_to_float(arg3, arg4);
if(instr == OP_ADD) {
res = (int32_t)(fl_arg1 + fl_arg2);
DEBUG("[BASIC_LIB] execute: FLOAT ADD FLOAT %d\n", res);
} else if(instr == OP_SUB) {
res = (int32_t)(fl_arg1 - fl_arg2);
DEBUG("[BASIC_LIB] execute: FLOAT SUB FLOAT %d\n", res);
} else if(instr == OP_DIV) {
res = (int32_t)((fl_arg1 * FLOAT_PRECISION) / fl_arg2);
DEBUG("[BASIC_LIB] execute: FLOAT DIV FLOAT: %d\n", res);
} else if(instr == OP_MUL) {
res = (int32_t)((fl_arg1 * fl_arg2) / FLOAT_PRECISION);
DEBUG("[BASIC_LIB] execute: FLOAT MULT FLOAT %d\n", res);
}
} else {
// FLOAT <OP> INTEGER
if(instr == OP_ADD) {
res = (int32_t)(fl_arg1 + (arg3->value.var * FLOAT_PRECISION));
DEBUG("[BASIC_LIB] execute: FLOAT ADD INT: %d\n", res);
} else if(instr == OP_SUB) {
res = (int32_t)(fl_arg1 - (arg3->value.var * FLOAT_PRECISION));
DEBUG("[BASIC_LIB] execute: FLOAT SUB INT: %d\n", res);
} else if(instr == OP_DIV) {
res = (int32_t)(fl_arg1 / arg3->value.var);
DEBUG("[BASIC_LIB] execute: FLOAT DIV INT: %d\n", res);
sys_led(LED_RED_TOGGLE);
#ifdef OUTLIER_SCRIPT_DBG
int32_t* post_avg;
post_avg = (int32_t*)sys_malloc(sizeof(int32_t));
*post_avg = res;
sys_post_uart(OUTLIER_DETECTION_PID, MSG_AVERAGE, sizeof(int32_t),
post_avg, SOS_MSG_RELEASE, UART_ADDRESS);
#endif
} else if(instr == OP_MUL) {
res = (int32_t)(fl_arg1 * arg3->value.var);
DEBUG("[BASIC_LIB] execute: FLOAT MULT INT %d\n", res);
}
}
res_part = res & 0xFFFF;
pushValue( eventState, res_part, DVM_TYPE_FLOAT_DEC);
res_part = res >> 16;
pushValue( eventState, res_part, DVM_TYPE_FLOAT);
context->pc += 1;
break;
} else if (arg2->type == DVM_TYPE_FLOAT) {
arg3 = popOperand( eventState);
fl_arg2 = convert_to_float(arg2, arg3);
if(instr == OP_ADD) {
res = (int32_t)((arg1->value.var * FLOAT_PRECISION) + fl_arg2) ;
DEBUG("[BASIC_LIB] execute: INT ADD FLOAT: %d\n", res);
} else if(instr == OP_SUB) {
res = (int32_t)((arg1->value.var * FLOAT_PRECISION) - fl_arg2);
DEBUG("[BASIC_LIB] execute: INT SUB FLOAT: %d\n", res);
} else if(instr == OP_DIV) {
res = (int32_t)((arg1->value.var * FLOAT_PRECISION) / fl_arg2);
DEBUG("[BASIC_LIB] execute: INT DIV FLOAT: %d\n", res);
} else if(instr == OP_MUL) {
res = (int32_t)((arg1->value.var * FLOAT_PRECISION) * fl_arg2);
DEBUG("[BASIC_LIB] execute: INT MULT FLOAT: %d\n", res);
}
res_part = res & 0xFFFF;
pushValue( eventState, res_part, DVM_TYPE_FLOAT_DEC);
res_part = res >> 16;
pushValue( eventState, res_part, DVM_TYPE_FLOAT);
context->pc += 1;
break;
}
if(instr == OP_ADD) {
int_res = arg1->value.var + arg2->value.var;
DEBUG("[BASIC_LIB] execute: INT ADD INT: %d\n", int_res);
} else if(instr == OP_SUB) {
int_res = arg1->value.var - arg2->value.var;
DEBUG("[BASIC_LIB] execute: INT SUB INT: %d\n", int_res);
} else if(instr == OP_DIV) {
int_res = (int16_t)(arg1->value.var / arg2->value.var);
DEBUG("[BASIC_LIB] execute: INT DIV INT: %d\n", int_res);
} else if(instr == OP_MUL) {
int_res = (int16_t)(arg1->value.var * arg2->value.var);
DEBUG("[BASIC_LIB] execute: INT MULT INT: %d\n", int_res);
}
pushValue( eventState, int_res, DVM_TYPE_INTEGER);
context->pc += 1;
break;
}
static int8_t generic_test_msg_handler(void *state, Message *msg)
{
app_state_t *s = (app_state_t *) state;
switch ( msg->type ) {
/* do any initialization steps here,
* in general it is good to set all the leds to off so that you can analyze what happens later more accurately
* also be sure to start and enable any timers which your driver might need
*/
case MSG_INIT:
sys_led(LED_GREEN_OFF);
sys_led(LED_YELLOW_OFF);
sys_led(LED_RED_OFF);
s->state = TEST_APP_INIT;
s->count = 0;
s->order = 0;
s->pid = msg->did;
sys_timer_start(TEST_APP_TID, TEST_APP_INTERVAL, SLOW_TIMER_REPEAT);
send_new_data(START_DATA, 0);
break;
case MSG_FINAL:
sys_timer_stop(TEST_APP_TID);
s->state = TEST_APP_FINAL;
send_new_data(FINAL_DATA, 1);
break;
/* here we handle messages of type MSG_TEST_DATA
* in most cases, only the base station node should be doing this since it is the only one connected to the uart
* if your test does not use multiple nodes, or your messages are sent via another module, this is not needed
*/
case MSG_TEST_DATA:
{
uint8_t *payload;
uint8_t msg_len;
msg_len = msg->len;
payload = sys_msg_take_data(msg);
sys_post_uart(
s->pid,
MSG_TEST_DATA,
msg_len,
payload,
SOS_MSG_RELEASE,
BCAST_ADDRESS);
}
break;
case MSG_HP_DATA:
{
data_msg_t *d;
d = (data_msg_t*) sys_msg_take_data(msg);
if (d->state != 155 || s->order != 0){
send_new_data(55, d->data);
sys_led(LED_RED_TOGGLE);
}
s->order = 1;
sys_free(d);
}
break;
case MSG_LP_DATA:
{
data_msg_t *d;
d = (data_msg_t*) sys_msg_take_data(msg);
if (d->state != 255 || s->order != 1){
send_new_data(155, d->data);
sys_led(LED_RED_TOGGLE);
}else
send_new_data(255, d->data);
s->order = 0;
sys_free(d);
}
break;
default:
return -EINVAL;
break;
}
return SOS_OK;
}
static int8_t accel_test_msg_handler(void *state, Message *msg)
{
app_state_t *s = (app_state_t *) state;
switch ( msg->type ) {
case MSG_INIT:
s->state = ACCEL_TEST_APP_INIT;
s->pid = msg->did;
sys_timer_start(ACCEL_TEST_APP_TID, ACCEL_TEST_APP_INTERVAL, TIMER_REPEAT);
sys_sensor_enable(MTS310_ACCEL_0_SID);
break;
case MSG_FINAL:
sys_sensor_disable(MTS310_ACCEL_0_SID);
break;
case MSG_TIMER_TIMEOUT:
{
LED_DBG(LED_YELLOW_TOGGLE);
switch (s->state) {
case ACCEL_TEST_APP_INIT:
// do any necessary init here
s->state = ACCEL_TEST_APP_IDLE;
break;
case ACCEL_TEST_APP_IDLE:
s->state = ACCEL_TEST_APP_ACCEL_0;
break;
case ACCEL_TEST_APP_ACCEL_0:
s->state = ACCEL_TEST_APP_ACCEL_0_BUSY;
sys_sensor_get_data(MTS310_ACCEL_0_SID);
break;
case ACCEL_TEST_APP_ACCEL_0_BUSY:
//s->state = ACCEL_TEST_APP_ACCEL_1;
break;
case ACCEL_TEST_APP_ACCEL_1:
s->state = ACCEL_TEST_APP_ACCEL_1_BUSY;
sys_sensor_get_data(MTS310_ACCEL_1_SID);
break;
case ACCEL_TEST_APP_ACCEL_1_BUSY:
//s->state = ACCEL_TEST_APP_ACCEL_0;
break;
default:
LED_DBG(LED_RED_TOGGLE);
s->state = ACCEL_TEST_APP_INIT;
break;
}
}
break;
case MSG_DATA_READY:
{
uint8_t *data_msg;
LED_DBG(LED_GREEN_TOGGLE);
data_msg = sys_malloc ( UART_MSG_LEN );
if ( data_msg ) {
data_msg[0] = msg->data[0];
data_msg[1] = msg->data[2];
data_msg[2] = msg->data[1];
sys_post_uart ( s->pid,
MSG_DATA_READY,
UART_MSG_LEN,
data_msg,
SOS_MSG_RELEASE,
UART_ADDRESS);
}
if (s->state == ACCEL_TEST_APP_ACCEL_1_BUSY) {
s->state = ACCEL_TEST_APP_ACCEL_0;
} else {
s->state = ACCEL_TEST_APP_ACCEL_1;
}
}
break;
default:
return -EINVAL;
break;
}
return SOS_OK;
}
static int8_t generic_test_msg_handler(void *state, Message *msg)
{
app_state_t *s = (app_state_t *) state;
switch ( msg->type ) {
/* do any initialization steps here,
* in general it is good to set all the leds to off so that you can analyze what happens later more accurately
* also be sure to start and enable any timers which your driver might need
*/
case MSG_INIT:
sys_led(LED_GREEN_OFF);
sys_led(LED_YELLOW_OFF);
sys_led(LED_RED_OFF);
s->state = TEST_APP_INIT;
s->count = 0;
s->pid = msg->did;
sys_timer_start(TEST_APP_TID, TEST_APP_INTERVAL, SLOW_TIMER_REPEAT);
send_new_data(START_DATA, 0);
break;
case MSG_ERROR:
s->state = TEST_APP_INIT;
s->count = 0;
s->pid = msg->did;
sys_timer_start(TEST_APP_TID, TEST_APP_INTERVAL, SLOW_TIMER_REPEAT);
send_new_data(START_DATA, 0);
break;
case MSG_FINAL:
sys_timer_stop(TEST_APP_TID);
s->state = TEST_APP_FINAL;
send_new_data(FINAL_DATA, 1);
break;
/* here we handle messages of type MSG_TEST_DATA
* in most cases, only the base station node should be doing this since it is the only one connected to the uart
* if your test does not use multiple nodes, or your messages are sent via another module, this is not needed
*/
case MSG_TEST_DATA:
{
uint8_t *payload;
uint8_t msg_len;
msg_len = msg->len;
payload = sys_msg_take_data(msg);
sys_post_uart(
s->pid,
MSG_TEST_DATA,
msg_len,
payload,
SOS_MSG_RELEASE,
BCAST_ADDRESS);
}
break;
case MSG_WAITING:
s->state = TEST_APP_WAIT;
break;
case MSG_TIMER_TIMEOUT:
{
switch(s->state){
case TEST_APP_INIT:
{
uint8_t *d;
d = (uint8_t*) sys_malloc(sizeof(uint8_t));
*d = s->count;
sys_shm_open(sys_shm_name(TEST_PID, 0),d);
s->state = TEST_APP_FINAL;
}
break;
case TEST_APP_WAIT:
{
uint8_t *d;
d = (uint8_t *) sys_shm_get(sys_shm_name(TEST_PID, 0));
*d = s->count;
sys_shm_update(sys_shm_name(TEST_PID, 0), d);
s->count++;
if (s->count == 0){
s->state = TEST_APP_INIT;
sys_shm_close(sys_shm_name(TEST_PID, 0));
sys_free(d);
}
}
break;
case TEST_APP_FINAL:
{
sys_post_value(
OTHER_PID,
MSG_WAIT_READY,
0,
SOS_MSG_RELEASE);
}
break;
default:
return -EINVAL;
break;
}
}
break;
default:
return -EINVAL;
break;
}
return SOS_OK;
}
static int8_t test_tpsn_net_module_handler(void *state, Message *msg)
{
app_state_t *s = (app_state_t *) state;
MsgParam *p = (MsgParam*)(msg->data);
switch (msg->type)
{
case MSG_INIT:
{
s->pid = msg->did;
sys_register_isr(0, USERINT_FID);
s->state = 0;
// If master node, start the transmit_timer
if(sys_id() == 0) sys_timer_start(TRANSMIT_TIMER, TRANSMIT_INTERVAL, TIMER_REPEAT);
sys_led(LED_RED_OFF);
sys_led(LED_GREEN_OFF);
sys_led(LED_YELLOW_OFF);
break;
}
case MSG_GLOBAL_TIME_REPLY:
{
msg_global_time_t* msg_global_time = (msg_global_time_t*)msg->data;
s->time = msg_global_time->time;
s->refreshed = msg_global_time->refreshed;
sys_timer_start(DELAY_TIMER, sys_rand()%256, TIMER_ONE_SHOT);
break;
}
case MSG_GLOBAL_TIME_SEND:
{
msg_global_time_send_t* datamsg = (msg_global_time_send_t*) sys_msg_take_data(msg);
//sys_led(LED_YELLOW_TOGGLE);
sys_post_uart(s->pid, MSG_GLOBAL_TIME_SEND, sizeof(msg_global_time_send_t), datamsg, SOS_MSG_RELEASE, BCAST_ADDRESS);
break;
}
case MSG_TIMER_TIMEOUT:
{
switch(p->byte)
{
case TRANSMIT_TIMER:
{
if (s->state){
sys_led(LED_GREEN_OFF);
SETBITLOW(P2OUT, 3);
s->state = 0;
} else {
uint32_t timestamp;
msg_global_time_send_t* msg_global_time_send;
sys_led(LED_GREEN_ON);
SETBITHIGH(P2OUT, 3);
timestamp = sys_time32();
// Construct the packet and send it over uart
msg_global_time_send = (msg_global_time_send_t*)sys_malloc(sizeof(msg_global_time_send_t));
msg_global_time_send->addr = sys_id();
msg_global_time_send->time = timestamp;
msg_global_time_send->refreshed = 0;
sys_post_uart(s->pid, MSG_GLOBAL_TIME_SEND, sizeof(msg_global_time_send_t), msg_global_time_send, SOS_MSG_RELEASE, BCAST_ADDRESS);
s->state = 1;
}
break;
}
case DELAY_TIMER:
{
msg_global_time_send_t* msg_global_time_send = (msg_global_time_send_t*)sys_malloc(sizeof(msg_global_time_send_t));
sys_led(LED_GREEN_TOGGLE);
msg_global_time_send->addr = sys_id();
msg_global_time_send->time = s->time;
msg_global_time_send->refreshed = s->refreshed;
sys_post_net(s->pid, MSG_GLOBAL_TIME_SEND, sizeof(msg_global_time_send_t), msg_global_time_send, SOS_MSG_RELEASE, 0);
break;
}
}
break;
}
default:
return -EINVAL;
}
/**
* Return SOS_OK for those handlers that have successfully been handled.
*/
return SOS_OK;
}
int main(){
// Initialize Peripherals
interface_init();
red_led_on();
uart_init(BAUDRATE);
animation_manager_init();
sys_timer_start();
audio_init();
sei(); // enable global interrupts
// Load Default Animation
animation_manager_load_animation(START_ANIMATION);
// Enter Setup if Requested
_delay_ms(100);
if(deb_switch_1()){
setup_wb_run();
}
else if(deb_switch_2()){
setup_orientation_run();
}
// Load Default Animation
animation_manager_load_animation(START_ANIMATION);
// Set White Balance
_delay_ms(300);
display_wb_update();
while(uart_async_run()); // setup white balance
// Control Panel is Ready => Signal this by Turning the LED Green
red_led_off();
green_led_on();
while(1){
// Sleep Mode
if(!switch_on_off()){ // if switched off
go_to_sleep();
}
// Change animations
sw_check();
if(sw_check_pressed(SW_LEFT, 200, true)){
animation_manager_dec_animation();
}
else if(sw_check_pressed(SW_RIGHT, 200, true)){
animation_manager_inc_animation();
}
else if(sw_check_pressed(SW_RAND, 300, true)){
animation_manager_random_animation();
}
// Generate Image
animation_manager_run(0);
// Check Audio
audio_start();
while(audio_run());
audio_process();
// Display Image
while(uart_async_run()){
interface_async_run();
}
}
}
static int8_t module(void *state, Message *msg)
{
app_state_t *s = (app_state_t *) state;
MsgParam *p = (MsgParam*)(msg->data);
/**
* Switch to the correct message handler
*/
switch (msg->type){
case MSG_INIT:
{
DEBUG("RATS: node %d initializing\n", ker_id());
s->pid = msg->did;
s->ts_list = NULL;
s->ts_packet.type = NORMAL_PACKET;
//Notify neighbors that RATS is starting (in case node rebooted while it was
//synchronizing with another node
post_net(s->pid, s->pid, MSG_INVALIDATE_ENTRY, 0, NULL, 0, BCAST_ADDRESS);
return SOS_OK;
}
case MSG_RATS_CLIENT_START:
{
MsgParam *p = (MsgParam *)msg->data;
DEBUG("RATS: Received MSG_RATS_CLIENT_START for node %d\n", p->word);
uint8_t request_status = add_request(s, p->word, p->byte);
//If a new request was created, then send packet to parent
if(request_status != NO_REQUEST_CREATED)
{
DEBUG("RATS: Transmitting request to node %d\n", p->word);
LED_DBG(LED_RED_TOGGLE);
//If the current node is the parent of the target node, then the target node will
//reply by informing the parent, who will add the target to its list of children.
post_net(s->pid, s->pid, MSG_RATS_SERVER_START, 0, NULL, 0, p->word);
}
else
{
//Request already exists
DEBUG("RATS: Request already exists\n");
}
//If this was the first request that was created, we need to start the panic timer
if(request_status == CREATED_FIRST_REQUEST)
{
DEBUG("RATS: PANIC_TIMER started\n");
#ifdef USE_PANIC_PACKETS
sys_timer_start(PANIC_TIMER, MIN_SAMPLING_PERIOD*1024, TIMER_REPEAT);
#endif //USE_PANIC_PACKETS
}
return SOS_OK;
}
case MSG_RATS_SERVER_START:
{
timesync_t * temp_ts_ptr = get_timesync_ptr(s, msg->saddr);
DEBUG("RATS: Received request from node %d\n", msg->saddr);
if(temp_ts_ptr == NULL)
{
DEBUG("RATS: Starting timesync with node %d\n", msg->saddr);
LED_DBG(LED_RED_TOGGLE);
//If request is coming from node, with whom the node is not synchronizing, then
//synchronization is starting
sys_timer_stop(TRANSMIT_TIMER);
sys_timer_stop(VALIDATION_TIMER);
s->ts_packet.transmission_period = INITIAL_TRANSMISSION_PERIOD;
s->ts_packet.min_period_node_id = msg->saddr;
s->transmit_timer_counter = 1; //s->ts_packet.transmission_period/INITIAL_TRANSMISSION_PERIOD;
s->validation_timer_counter = 5; //s->transmit_timer_counter + 4;
s->validation_timer_retransmissions = TOTAL_VALIDATION_RETRANSMISSIONS;
sys_timer_start(TRANSMIT_TIMER, MIN_SAMPLING_PERIOD*1024, TIMER_REPEAT);
sys_timer_start(VALIDATION_TIMER, MIN_SAMPLING_PERIOD*1024, TIMER_REPEAT);
}
return SOS_OK;
}
case MSG_RATS_GET_TIME:
{
//If the module passed a NULL pointer or if the data size is wrong, then discard
if( (msg->data == NULL)
#ifndef PC_PLATFORM
|| (msg->len != sizeof(rats_t) )
#endif //PC_PLATFORM
)
{
DEBUG("RATS: Invalid data received in MSG_RATS_GET_TIME\n");
break;
}
rats_t * rats_ptr = (rats_t *)sys_msg_take_data(msg);
DEBUG("RATS: Received MSG_RATS_GET_TIME (mod_id=%d node=%d)\n", rats_ptr->mod_id, msg->saddr);
if(rats_ptr->source_node_id == ker_id())
{
timesync_t * temp_ts_ptr = get_timesync_ptr(s, rats_ptr->target_node_id);
if(temp_ts_ptr == NULL)
{
DEBUG("RATS: Target node %d is not time synced\n", rats_ptr->target_node_id);
sys_free(rats_ptr);
break;
}
else
{
DEBUG("RATS: Calculating time for target node %d locally\n", rats_ptr->target_node_id);
if(temp_ts_ptr->packet_count < BUFFER_SIZE) // learning state
{
rats_ptr->time_at_target_node = 0;
rats_ptr->error = 0;
}
else
{
rats_ptr->time_at_target_node = convert_from_mine_to_parent_time(rats_ptr->time_at_source_node, rats_ptr->target_node_id);
rats_ptr->error = getError(&temp_ts_ptr->timestamps[0], &temp_ts_ptr->my_time[0], BUFFER_SIZE,
temp_ts_ptr->window_size, BUFFER_SIZE - temp_ts_ptr->window_size, &temp_ts_ptr->a, &temp_ts_ptr->b, temp_ts_ptr->sampling_period, FALSE);
}
}
}
else if (rats_ptr->target_node_id == ker_id())
{
timesync_t * temp_ts_ptr = get_timesync_ptr(s, rats_ptr->source_node_id);
if(temp_ts_ptr == NULL)
{
DEBUG("RATS: Source node %d is not time synced\n", rats_ptr->source_node_id);
sys_free(rats_ptr);
break;
}
else
{
DEBUG("RATS: Calculating time for source node %d locally\n", rats_ptr->source_node_id);
if(temp_ts_ptr->packet_count < BUFFER_SIZE) // learning state
{
rats_ptr->time_at_target_node = 0;
rats_ptr->error = 0;
}
else
{
rats_ptr->time_at_target_node = convert_from_parent_to_my_time(rats_ptr->time_at_source_node, rats_ptr->source_node_id);
rats_ptr->error = getError(&temp_ts_ptr->timestamps[0], &temp_ts_ptr->my_time[0], BUFFER_SIZE,
temp_ts_ptr->window_size, BUFFER_SIZE - temp_ts_ptr->window_size, &temp_ts_ptr->a, &temp_ts_ptr->b, temp_ts_ptr->sampling_period, TRUE);
}
}
}
else
{
DEBUG("RATS: Invalid request (source = %d, target - %d)\n", rats_ptr->source_node_id, rats_ptr->target_node_id);
sys_free(rats_ptr);
break;
}
DEBUG("RATS: Sending reply to module %d\n", rats_ptr->mod_id);
post_long(rats_ptr->mod_id, s->pid, rats_ptr->msg_type, sizeof(rats_t), rats_ptr, SOS_MSG_RELEASE);
break;
}
case MSG_RATS_CLIENT_STOP:
{
MsgParam *p = (MsgParam *)msg->data;
uint16_t node_id = p->word;
//First we need to remove node from list of parents
/* Select node at head of list */
timesync_t * ts_list_ptr = s->ts_list;
timesync_t * ts_delete_list_ptr;
timesync_t * ts_previous_list_ptr = s->ts_list;
/* Loop until we've reached the end of the list */
while( ts_list_ptr != NULL )
{
if(ts_list_ptr->node_id == node_id)
{
if(--ts_list_ptr->ref_counter > 0)
return SOS_OK;
DEBUG("RATS: Removing node %d from list of parents. Sending MSG_RATS_SERVER_STOP.\n", node_id);
post_net(s->pid, s->pid, MSG_RATS_SERVER_STOP, 0, NULL, 0, node_id);
/* Found the item to be deleted,
re-link the list around it */
if( ts_list_ptr == s->ts_list )
/* We're deleting the head */
s->ts_list = ts_list_ptr->next;
else
ts_previous_list_ptr->next = ts_list_ptr->next;
ts_delete_list_ptr = ts_list_ptr;
ts_list_ptr = ts_list_ptr->next;
/* Free the node */
sys_free( ts_delete_list_ptr );
//If the parent list is empty, then we're stopping the panic timer
if(s->ts_list == NULL)
{
DEBUG("RATS: Parent list is empty. Stopping panic timer\n");
#ifdef USE_PANIC_PACKETS
sys_timer_stop(PANIC_TIMER);
#endif //USE_PANIC_PACKETS
}
return SOS_OK;
}
ts_previous_list_ptr = ts_list_ptr;
ts_list_ptr = ts_list_ptr->next;
}
DEBUG("RATS: Requested parent %d was not found\n", node_id);
break;
}
case MSG_RATS_SERVER_STOP:
{
DEBUG("RATS: Received MSG_RATS_SERVER_STOP from %d\n", msg->saddr);
//If node has minimum period, then go to validation protocol
if(msg->saddr == s->ts_packet.min_period_node_id)
{
DEBUG("RATS: Going to validation protocol\n");
s->validation_timer_counter = 1;
s->validation_timer_retransmissions = BROADCAST_VALIDATION_RETRANSMISSIONS;
s->validation_node_id = ker_id();
}
break;
}
case MSG_TIMER_TIMEOUT:
{
switch(p->byte)
{
case TRANSMIT_TIMER:
{
if( (--(s->transmit_timer_counter)) == 0)
{
DEBUG("RATS: Broadcasting MSG_TIMESTAMP packet\n");
LED_DBG(LED_GREEN_TOGGLE);
post_net(s->pid, s->pid, MSG_TIMESTAMP, sizeof(ts_packet_t), &s->ts_packet, 0, BCAST_ADDRESS);
#ifdef UART_DEBUG
post_uart(s->pid, s->pid, UART_TIMESTAMP, sizeof(ts_packet_t), &s->ts_packet, 0, BCAST_ADDRESS);
#endif //UART_DEBUG
s->transmit_timer_counter = (uint16_t)(s->ts_packet.transmission_period / MIN_SAMPLING_PERIOD);
}
break;
}
case VALIDATION_TIMER:
{
if( (--(s->validation_timer_counter)) == 0)
{
s->validation_timer_counter = 1;
//Send up to MSG_PERIOD_REQUEST packets (UNICAST_VALIDATION_RETRANSMISSIONS times) to node with minimum period.
//If the node doesn't respond until then, then broadcast BROADCAST_VALIDATION_RETRANSMISSIONS times
//After the transmitting BROADCAST_VALIDATION_RETRANSMISSIONS packets, use the minimum period that
//was sent during that interval
if( s->validation_timer_retransmissions > BROADCAST_VALIDATION_RETRANSMISSIONS )
{
--s->validation_timer_retransmissions;
DEBUG("RATS: Transmitting MSG_PERIOD_REQUEST (retries left = %d) to node %d\n", s->validation_timer_retransmissions, s->ts_packet.min_period_node_id);
post_net(s->pid, s->pid, MSG_PERIOD_REQUEST, 0, NULL, 0, s->ts_packet.min_period_node_id);
#ifdef UART_DEBUG
post_uart(s->pid, s->pid, UART_PERIOD_REQUEST, 0, NULL, 0, s->ts_packet.min_period_node_id);
#endif //UART_DEBUG
}
else if( s->validation_timer_retransmissions > 0)
{
--s->validation_timer_retransmissions;
DEBUG("RATS: Broadcasting MSG_PERIOD_REQUEST (retries left = %d)\n", s->validation_timer_retransmissions);
//Invalidate node with minimum period
s->validation_node_id = ker_id();
post_net(s->pid, s->pid, MSG_PERIOD_REQUEST, 0, NULL, 0, BCAST_ADDRESS);
#ifdef UART_DEBUG
post_uart(s->pid, s->pid, UART_PERIOD_REQUEST, 0, NULL, 0, BCAST_ADDRESS);
#endif //UART_DEBUG
}
else //s->validation_timer_retransmissions == 0
{
sys_timer_stop(TRANSMIT_TIMER);
sys_timer_stop(VALIDATION_TIMER);
//Restart normal procedure only if there was a reply
if(ker_id() != s->validation_node_id)
{
DEBUG("RATS: Setting node %d as the one with min period (%d)\n",
s->validation_node_id, s->validation_period);
s->ts_packet.min_period_node_id = s->validation_node_id;
s->ts_packet.transmission_period = s->validation_period;
s->transmit_timer_counter = s->ts_packet.transmission_period/INITIAL_TRANSMISSION_PERIOD;
s->validation_timer_counter = s->transmit_timer_counter + 4;
s->validation_timer_retransmissions = TOTAL_VALIDATION_RETRANSMISSIONS;
sys_timer_start(TRANSMIT_TIMER, MIN_SAMPLING_PERIOD*1024, TIMER_REPEAT);
sys_timer_start(VALIDATION_TIMER, MIN_SAMPLING_PERIOD*1024, TIMER_REPEAT);
}
else
{
DEBUG("RATS: Validation timer expired, without receiving any packets\n");
sys_timer_stop(TRANSMIT_TIMER);
sys_timer_stop(VALIDATION_TIMER);
}
}
}
break;
}
case PANIC_TIMER:
{
//There is a fixed number of retransmissions. If the corresponding counter
//reaches zero, then the child is removed from the list
/* Select node at head of list */
timesync_t * ts_list_ptr = s->ts_list;
timesync_t * ts_delete_list_ptr;
timesync_t * ts_previous_list_ptr = s->ts_list;
/* Loop until we've reached the end of the list */
while( ts_list_ptr != NULL )
{
if(--ts_list_ptr->panic_timer_counter == 0)
{
if(ts_list_ptr->panic_timer_retransmissions > 0)
{
//Transmit the packet
--ts_list_ptr->panic_timer_retransmissions;
DEBUG("RATS: Sending panic packet to node %d (retries=%d)\n", ts_list_ptr->node_id, ts_list_ptr->panic_timer_retransmissions);
post_net(s->pid, s->pid, MSG_PANIC, 0, NULL, 0, ts_list_ptr->node_id);
//The retransmission period should be INITIAL_TRANSMISSION_PERIOD
ts_list_ptr->panic_timer_counter = 1;
}
else
{
DEBUG("RATS: Removing node %d from list of parents\n", ts_list_ptr->node_id);
/* Found the item to be deleted,
re-link the list around it */
if( ts_list_ptr == s->ts_list )
/* We're deleting the head */
s->ts_list = ts_list_ptr->next;
else
ts_previous_list_ptr->next = ts_list_ptr->next;
ts_delete_list_ptr = ts_list_ptr;
ts_list_ptr = ts_list_ptr->next;
/* Free the node */
sys_free( ts_delete_list_ptr );
continue;
}
}
ts_previous_list_ptr = ts_list_ptr;
ts_list_ptr = ts_list_ptr->next;
}
//If the parent list is empty, then we're stopping the panic timer
if(s->ts_list == NULL)
{
DEBUG("RATS: Parent list is empty. Stopping panic timer\n");
#ifdef USE_PANIC_PACKETS
sys_timer_stop(PANIC_TIMER);
#endif //USE_PANIC_PACKETS
}
break;
}
default:
break;
}
return SOS_OK;
}
case MSG_PERIOD_CHANGE:
{
uint16_t temp_transmission_period;
DEBUG("RATS: Received packet for period change from %d\n", msg->saddr);
LED_DBG(LED_YELLOW_TOGGLE);
if((msg->data == NULL) || (msg->len != sizeof(uint16_t)) )
{
DEBUG("RATS: Invalid parameters in MSG_PERIOD_CHANGE\n");
break;
}
temp_transmission_period = (* (uint16_t*)(msg->data));
//Change period if:
//a)received period is smaller than period in use
//b)node that sent period is the one that has the current smallest period
//c)I am currently using myself as the node with the smallest period (used in the beginning and in transitive modes)
if((temp_transmission_period < s->ts_packet.transmission_period)
|| (s->ts_packet.min_period_node_id == msg->saddr)
|| (s->ts_packet.min_period_node_id == ker_id()) )
{
DEBUG("RATS: Changing period (new_period=%d new_node=%d). Sending to UART\n", temp_transmission_period, msg->saddr);
sys_timer_stop(TRANSMIT_TIMER);
sys_timer_stop(VALIDATION_TIMER);
#ifdef UART_DEBUG
period_packet_t * period_packet_ptr = sys_malloc(sizeof(period_packet_t));
period_packet_ptr->saddr = msg->saddr;
period_packet_ptr->old_period = s->ts_packet.transmission_period;
period_packet_ptr->new_period = temp_transmission_period;
post_uart(s->pid, s->pid, UART_PERIOD_CHANGE, sizeof(period_packet_t), period_packet_ptr, SOS_MSG_RELEASE, UART_ADDRESS);
#endif //UART_DEBUG
s->ts_packet.transmission_period = temp_transmission_period;
s->ts_packet.min_period_node_id = msg->saddr;
s->transmit_timer_counter = (uint16_t)(s->ts_packet.transmission_period / MIN_SAMPLING_PERIOD);
s->validation_timer_counter = s->transmit_timer_counter + 4;
s->validation_timer_retransmissions = TOTAL_VALIDATION_RETRANSMISSIONS;
sys_timer_start(TRANSMIT_TIMER, INITIAL_TRANSMISSION_PERIOD*1024, TIMER_REPEAT);
sys_timer_start(VALIDATION_TIMER, INITIAL_TRANSMISSION_PERIOD*1024, TIMER_REPEAT);
}
return SOS_OK;
}
case MSG_TIMESTAMP:
{
ts_packet_t *ts_packet_ptr = (ts_packet_t *)msg->data;
DEBUG("RATS: MSG_TIMESTAMP with type = %d\n", ts_packet_ptr->type);
if(ts_packet_ptr->type == NORMAL_PACKET)
{
DEBUG("RATS: Receiving timestamp data from node %d\n", msg->saddr);
if( add_values(s, msg) == TRUE)
{
LED_DBG(LED_GREEN_TOGGLE);
DEBUG("RATS: Accessed internal structure\n");
}
else
{
DEBUG("RATS: Discarding MSG_TIMESTAMP from node %d\n", msg->saddr);
}
}
else // TEST_PACKET
{
if(ker_id() == ROOT_NODE)
{
DEBUG("RATS: Receiving test data from node %d. Sending to UART\n", msg->saddr);
#ifdef UART_DEBUG
ext_packet_t * ext_packet_ptr = (ext_packet_t *)msg->data;
debug_packet_t * debug_packet_ptr = (debug_packet_t *)sys_malloc(sizeof(debug_packet_t));
debug_packet_ptr->time[0] = ticks_to_msec_float(ext_packet_ptr->time[0]);
debug_packet_ptr->time[1] = ticks_to_msec_float(ext_packet_ptr->time[1]);
debug_packet_ptr->node_id = ker_id();
debug_packet_ptr->int_parent_time = ext_packet_ptr->time[1];
post_uart(s->pid, s->pid, UART_FORWARD_EXT, sizeof(debug_packet_t), debug_packet_ptr, SOS_MSG_RELEASE, UART_ADDRESS);
#endif //UART_DEBUG
}
else
{
DEBUG("RATS: Receiving test data from node %d. Sending to parent\n", msg->saddr);
#ifdef UART_DEBUG
ext_packet_t * ext_packet_ptr = (ext_packet_t *)msg->data;
uint32_t parent_time = convert_from_mine_to_parent_time(ext_packet_ptr->time[1], ROOT_NODE);
//Break if the parent is not found in the timestamping list
if(parent_time == 0)
{
break;
}
debug_packet_t * debug_packet_ptr = (debug_packet_t *)sys_malloc(sizeof(debug_packet_t));
debug_packet_ptr->time[0] = ticks_to_msec_float(ext_packet_ptr->time[0]);
debug_packet_ptr->time[1] = ticks_to_msec_float(parent_time);
debug_packet_ptr->int_parent_time = parent_time;
debug_packet_ptr->node_id = ker_id();
post_uart(s->pid, s->pid, UART_FORWARD_EXT, sizeof(debug_packet_t), debug_packet_ptr, SOS_MSG_RELEASE, UART_ADDRESS);
#endif //UART_DEBUG
}
}
break;
}
case MSG_PERIOD_REQUEST:
{
DEBUG("RATS: Received MSG_PERIOD_REQUEST packet from node %d\n", msg->saddr);
timesync_t * temp_ts_ptr = get_timesync_ptr(s, msg->saddr);
if(temp_ts_ptr == NULL)
{
DEBUG("RATS: Discarding MSG_PERIOD_REQUEST\n");
break;
}
uint16_t *sampling_period = (uint16_t *)sys_malloc(sizeof(uint16_t));
if(sampling_period != NULL)
{
*sampling_period = temp_ts_ptr->sampling_period;
DEBUG("RATS: Sending MSG_PERIOD_REPLY packet (period=%d) to node %d\n", *sampling_period, msg->saddr);
post_net(s->pid, s->pid, MSG_PERIOD_REPLY, sizeof(uint16_t), sampling_period, SOS_MSG_RELEASE, msg->saddr);
}
break;
}
case MSG_PERIOD_REPLY:
{
uint16_t transmission_period;
DEBUG("RATS: Received MSG_PERIOD_REPLY packet from node %d\n", msg->saddr);
memcpy(&transmission_period, &msg->data[0], sizeof(transmission_period));
s->validation_timer_counter = s->transmit_timer_counter + 4;
s->validation_timer_retransmissions = TOTAL_VALIDATION_RETRANSMISSIONS;
if((transmission_period < s->validation_period) || (s->validation_node_id == ker_id() ) )
{
DEBUG("RATS: Changing VALIDATION period (new_period=%d new_node=%d)\n", transmission_period, msg->saddr);
s->validation_period = transmission_period;
s->validation_node_id = msg->saddr;
}
break;
}
case MSG_PANIC:
{
//Transmit MSG_TIMESTAMP, restart timer, recalculate value for transmit_timer_counter
sys_timer_stop(TRANSMIT_TIMER);
sys_timer_stop(VALIDATION_TIMER);
#ifdef UART_DEBUG
uint16_t *data = (uint16_t *)sys_malloc(sizeof(uint16_t));
*data = msg->saddr;
post_uart(s->pid, s->pid, UART_PANIC, sizeof(uint16_t), data, SOS_MSG_RELEASE, UART_ADDRESS);
#endif //UART_DEBUG
post_net(s->pid, s->pid, MSG_TIMESTAMP, sizeof(ts_packet_t), &s->ts_packet, 0, BCAST_ADDRESS);
s->transmit_timer_counter = (uint16_t)(s->ts_packet.transmission_period / MIN_SAMPLING_PERIOD);
s->validation_timer_counter = s->transmit_timer_counter + 4;
s->validation_timer_retransmissions = TOTAL_VALIDATION_RETRANSMISSIONS;
sys_timer_start(TRANSMIT_TIMER, INITIAL_TRANSMISSION_PERIOD*1024, TIMER_REPEAT);
sys_timer_start(VALIDATION_TIMER, INITIAL_TRANSMISSION_PERIOD*1024, TIMER_REPEAT);
break;
}
case MSG_INVALIDATE_ENTRY:
{
DEBUG("RATS: Received invalidation message from node %d\n", msg->saddr);
timesync_t * temp_ts_ptr = get_timesync_ptr(s, msg->saddr);
if(temp_ts_ptr == NULL)
{
DEBUG("RATS: Discarding MSG_INVALIDATE_ENTRY\n");
break;
}
DEBUG("RATS: Invalidation entry for node %d\n", msg->saddr);
temp_ts_ptr->packet_count = 0;
temp_ts_ptr->a = 0;
temp_ts_ptr->b = 0;
temp_ts_ptr->sampling_period = INITIAL_TRANSMISSION_PERIOD;
temp_ts_ptr->window_size = (uint8_t)BUFFER_SIZE;
temp_ts_ptr->panic_timer_counter = 5; //(s->ts_list->sampling_period / INITIAL_TRANSMISSION_PERIOD) + 4;
temp_ts_ptr->panic_timer_retransmissions = PANIC_TIMER_RETRANSMISSIONS;
memset(temp_ts_ptr->timestamps, 0, BUFFER_SIZE*sizeof(uint32_t));
memset(temp_ts_ptr->my_time, 0, BUFFER_SIZE*sizeof(uint32_t));
//Notify node to start procedure from beginning
post_net(s->pid, s->pid, MSG_RATS_SERVER_START, 0, NULL, 0, msg->saddr);
break;
}
case MSG_FINAL:
{
sys_timer_stop(TRANSMIT_TIMER);
sys_timer_stop(VALIDATION_TIMER);
#ifdef USE_PANIC_PACKETS
sys_timer_stop(PANIC_TIMER);
#endif //USE_PANIC_PACKETS
return SOS_OK;
}
default:
return -EINVAL;
}
/**
* Return SOS_OK for those handlers that have successfully been handled.
*/
return SOS_OK;
}
static int8_t rfidreader_msg_handler(void *state, Message *msg)
{
app_state_t *s = (app_state_t*)state;
switch (msg->type){
case MSG_INIT:
{
s->pid = msg->did;
s->state = 0;
DEBUG("Rfidreader Start\n");
s->counter = 0;
break;
}
case MSG_FINAL:
{
DEBUG("Rfidreader Stop\n");
break;
}
case MSG_RFID_TAG:
{
sys_led(LED_RED_TOGGLE);
uint8_t *payload;
payload = sys_msg_take_data(msg);
//rfid = sys_malloc(3*sizeof(uint8_t));
id = (uint8_t)*payload;
rssi = msg->rssi;
code = id ;
code = code << 8;
code = code | rssi;
//if (rfid !=NULL)
//{
// rfid[0]=sys_id();
// rfid[1]=msg->rssi;
// memcpy(rfid+2, payload, 1);
// uint8_t time = (uint8_t)ker_rand();
sys_timer_start(RAND_TID, 1024/20, TIMER_REPEAT);
//}
sys_free(payload);
break;
}
case MSG_TIMER_TIMEOUT:
{
//if (rfid !=NULL)
//{
//sys_post_net(DFLT_APP_ID0, MSG_RFID_RESPONSE, 3, (void*)rfid, SOS_MSG_RELEASE | SOS_MSG_RELIABLE, 0x01);
//}
if(counter >= 16)
{
sys_led(LED_YELLOW_OFF);
counter--;
}
else
{
if(GETBIT(code, counter) ==1)
{
sys_led(LED_YELLOW_OFF);
} else
{
sys_led(LED_YELLOW_ON);
}
counter --;
if(counter == -1)
{
counter = 17;
sys_timer_stop(RAND_TID);
sys_led(LED_YELLOW_ON);
}
}
break;
}
default:
return -EINVAL;
}
return SOS_OK;
}
static int8_t tpsn_net_module_handler(void *state, Message *msg)
{
app_state_t *s = (app_state_t *) state;
MsgParam *p = (MsgParam*)(msg->data);
/**
* Switch to the correct message handler
*/
switch (msg->type)
{
case MSG_INIT:
{
msg_adv_level_t* msg_adv_level = (msg_adv_level_t*)sys_malloc(sizeof(msg_adv_level_t));
DEBUG("TPSN NET: Started\n");
s->pid = msg->did;
s->level = -1;
s->parent_id = 0;
s->last_refresh = 0;
s->sync_state = INIT;
s->current_seq_no = 0;
// try to join the sync tree
msg_adv_level->level = s->level;
sys_post_net(s->pid, MSG_ADV_LEVEL, sizeof(msg_adv_level_t), msg_adv_level, SOS_MSG_RELEASE, BCAST_ADDRESS);
sys_timer_start(ADV_TIMER_ID, 5*1024, TIMER_REPEAT);
return SOS_OK;
}
case MSG_GET_GLOBAL_TIME:
{
DEBUG("TPSN_NET: state: %d\n", s->sync_state);
msg_global_time_t* time_msg = (msg_global_time_t*)msg->data;
msg_global_time_t* time_reply_msg = (msg_global_time_t*)sys_malloc(sizeof(msg_global_time_t));
if(s->sync_state == SYNCED || s->sync_state == SYNCING) {
// we are synced, and thus had at least one time sync exchange
// if we are level 1, then just return our time
if(s->level == 1) {
time_reply_msg->time = time_msg->time;
time_reply_msg->refreshed = 0;
} else {
uint32_t delta_refresh = 0;
uint32_t cur_time = sys_time32();
// check for overflow
if(cur_time < s->last_refresh) {
cur_time += 0x7F000000;
}
delta_refresh = cur_time - s->last_refresh;
// only try to refresh if we are not already syncing.
if (s->sync_state == SYNCED && delta_refresh > REFRESH_INTERVAL) {
DEBUG("TPSN_NET: Refresh needed refresh: %d\n", delta_refresh);
s->sync_state = SYNCING;
start_sync();
}
// even though we might be syncing, reply with the current estimate.
time_reply_msg->time = time_msg->time + s->clock_drift;
time_reply_msg->refreshed = delta_refresh;
}
} else {
time_reply_msg->time = NOT_SYNCED;
time_reply_msg->refreshed = NOT_SYNCED;
}
DEBUG("TPSN_NET: converted time for module %d, drift %d, refreshed %d, global time %d, sync state %d\n", msg->sid, s->clock_drift, time_reply_msg->refreshed, time_reply_msg->time, s->sync_state);
sys_post(msg->sid, MSG_GLOBAL_TIME_REPLY, sizeof(msg_global_time_t), time_reply_msg, SOS_MSG_RELEASE);
return SOS_OK;
}
case MSG_TIMESTAMP:
{
DEBUG("TPSN_NET: state: %d\n", s->sync_state);
LED_DBG(LED_RED_TOGGLE);
tpsn_req_t *tpsn_req_ptr = (tpsn_req_t *)msg->data;
switch(tpsn_req_ptr->type) {
case TPSN_REQUEST:
{
DEBUG("TPSN_NET: Received TPSN_REQUEST (seq_no=%d) from node %d\n", tpsn_req_ptr->seq_no, msg->saddr);
DEBUG("TPSN_NET: Transmitting TPSN_REPLY to node %d at time %d\n", msg->saddr, sys_time32());
tpsn_reply_t *tpsn_reply_ptr = (tpsn_reply_t *)sys_malloc(sizeof(tpsn_reply_t));
memcpy(tpsn_reply_ptr->previous_time, tpsn_req_ptr->time, sizeof(tpsn_req_ptr->time));
tpsn_reply_ptr->type = TPSN_REPLY;
tpsn_reply_ptr->seq_no = tpsn_req_ptr->seq_no;
if(msg->saddr == UART_ADDRESS) {
sys_post_uart(s->pid, MSG_TIMESTAMP, sizeof(tpsn_reply_t), tpsn_reply_ptr, SOS_MSG_RELEASE, msg->saddr);
} else {
sys_post_net(s->pid, MSG_TIMESTAMP, sizeof(tpsn_reply_t), tpsn_reply_ptr, SOS_MSG_RELEASE, msg->saddr);
}
break;
}
case TPSN_REPLY:
{
if(s->sync_state == SYNCING || s->sync_state == INIT_SYNCING) {
//LED_DBG(LED_YELLOW_TOGGLE);
DEBUG("TPSN: Received TPSN_REPLY from node %d\n", msg->saddr);
tpsn_reply_t *tpsn_reply_ptr = (tpsn_reply_t *)msg->data;
if (tpsn_reply_ptr->seq_no == s->current_seq_no) {
sys_timer_stop(SYNC_TIMER_ID);
s->current_seq_no++;
//T1=tpsn_reply_ptr->previous_time[0]
//T2=tpsn_reply_ptr->previous_time[1]
//T3=tpsn_reply_ptr->time[0]
//T4=tpsn_reply_ptr->time[1]
//CLOCK_DRIFT = ((T2 - T1) - (T4 - T3))/2
//PROPAGATION_DELAY=((T2 - T1) + (T4 - T3))/2
//Take care of overflow in the node that sent the TPSN request (T1 > T4)
if(tpsn_reply_ptr->previous_time[0] > tpsn_reply_ptr->time[1])
tpsn_reply_ptr->time[1] += INT_MAX_GTIME;
//Take care of overflow in the node that sent the TPSN reply (T2 > T3)
if(tpsn_reply_ptr->previous_time[1] > tpsn_reply_ptr->time[0])
tpsn_reply_ptr->time[0] += INT_MAX_GTIME;
s->clock_drift = ( ((int32_t)tpsn_reply_ptr->previous_time[1] - (int32_t)tpsn_reply_ptr->previous_time[0]) -
((int32_t)tpsn_reply_ptr->time[1] - (int32_t)tpsn_reply_ptr->time[0]) )/2;
s->last_refresh = sys_time32();
s->sync_state = SYNCED;
DEBUG("TPSN: The clock offset for node %d is %d\n", msg->saddr, s->clock_drift);
}
}
break;
}
default:
{
DEBUG("Received unknown packet\n");
break;
}
}
}
case MSG_TIMER_TIMEOUT:
{
DEBUG("TPSN_NET: state: %d\n", s->sync_state);
switch(p->byte)
{
case ADV_TIMER_ID:
{
if( s->sync_state == INIT) {
// try to join the sync tree
DEBUG("TPSN_NET: Trying to join sync tree. Sending out ADV\n");
msg_adv_level_t* msg_adv_level = (msg_adv_level_t*)sys_malloc(sizeof(msg_adv_level_t));
msg_adv_level->level = s->level;
sys_post_net(s->pid, MSG_ADV_LEVEL, sizeof(msg_adv_level_t), msg_adv_level, SOS_MSG_RELEASE, BCAST_ADDRESS);
}
break;
}
case SYNC_TIMER_ID:
{
if(s->sync_state == SYNCING || s->sync_state == INIT_SYNCING)
{
// didn't receive the timestamp in time. Try to resend
DEBUG("TPSN_NET: SYNC_TIMER fired. Trying to sync again.\n");
start_sync();
}
break;
}
default:
break;
}
return SOS_OK;
}
case MSG_ADV_LEVEL:
{
// FIXME: only allow level 1 to reply for now!
if ( s->sync_state == SYNCED && s->level < 2) {
msg_adv_level_t* msg_adv_level = (msg_adv_level_t*)sys_malloc(sizeof(msg_adv_level_t));
msg_adv_level->level = s->level;
sys_post_net(s->pid, MSG_ADV_REPLY, sizeof(msg_adv_level_t), msg_adv_level, SOS_MSG_RELEASE, msg->saddr);
}
return SOS_OK;
}
case MSG_ADV_REPLY:
{
DEBUG("TPSN_NET: state: %d\n", s->sync_state);
msg_adv_level_t* msg_adv_level = (msg_adv_level_t*)msg->data;
if ( (s->sync_state == INIT) || msg_adv_level->level < s->level) {
DEBUG("TPSN_NET: received new level %d from %d\n", msg_adv_level->level, msg->saddr);
sys_timer_stop(ADV_TIMER_ID);
s->level = msg_adv_level->level+1;
s->parent_id = msg->saddr;
if(s->level == 1) {
// special case for root.
msg_adv_level_t* msg_adv_level = (msg_adv_level_t*)sys_malloc(sizeof(msg_adv_level_t));
msg_adv_level->level = s->level;
sys_post_net(s->pid, MSG_ADV_REPLY, sizeof(msg_adv_level_t), msg_adv_level, SOS_MSG_RELEASE, msg->saddr);
s->sync_state = SYNCED;
} else {
s->sync_state = INIT_SYNCING;
start_sync();
}
}
return SOS_OK;
}
case MSG_FINAL:
{
return SOS_OK;
}
default:
return -EINVAL;
}
/**
* Return SOS_OK for those handlers that have successfully been handled.
*/
return SOS_OK;
}
static int8_t temp_test_msg_handler(void *state, Message *msg)
{
app_state_t *s = (app_state_t *) state;
switch ( msg->type ) {
case MSG_INIT:
s->state = TEMP_TEST_APP_INIT;
s->pid = msg->did;
sys_timer_start(TEMP_TEST_APP_TID, TEMP_TEST_APP_INTERVAL, TIMER_REPEAT);
if(sys_sensor_enable(TEMP_SID) != SOS_OK) {
LED_DBG(LED_RED_ON);
sys_timer_stop(TEMP_TEST_APP_TID);
}
break;
case MSG_FINAL:
sys_sensor_disable( TEMP_SID);
sys_timer_stop(TEMP_TEST_APP_TID);
break;
case MSG_TIMER_TIMEOUT:
{
LED_DBG(LED_YELLOW_TOGGLE);
switch (s->state) {
case TEMP_TEST_APP_INIT:
// do any necessary init here
s->state = TEMP_TEST_APP_IDLE;
break;
case TEMP_TEST_APP_IDLE:
s->state = TEMP_TEST_APP_TEMP;
break;
case TEMP_TEST_APP_TEMP:
s->state = TEMP_TEST_APP_TEMP_BUSY;
sys_sensor_get_data( TEMP_SID);
break;
case TEMP_TEST_APP_TEMP_BUSY:
break;
default:
LED_DBG(LED_RED_TOGGLE);
s->state = TEMP_TEST_APP_INIT;
break;
}
}
break;
case MSG_DATA_READY:
{
uint8_t *data_msg;
LED_DBG(LED_GREEN_TOGGLE);
data_msg = sys_malloc ( sizeof(MsgParam));
if ( data_msg ) {
LED_DBG(LED_RED_TOGGLE);
memcpy((void*)data_msg, (void*)msg->data, sizeof(MsgParam));
sys_post_uart ( s->pid,
MSG_DATA_READY,
sizeof(MsgParam),
data_msg,
SOS_MSG_RELEASE,
BCAST_ADDRESS);
}
switch(s->state) {
case TEMP_TEST_APP_TEMP_BUSY:
s->state = TEMP_TEST_APP_TEMP;
break;
}
}
break;
default:
return -EINVAL;
break;
}
return SOS_OK;
}
