void term_process(void) {
//TODO exec commands
enum CMD cmd_i = 255;
for(unsigned char i=0; i<ARRAY_SIZE(cmds); i++) {
if(cmp(i, buff)) cmd_i = i;
}
unsigned int a, b;
float fa, fb;
switch(cmd_i) {
case HELP:
bprintf("< commands >\n");
bprintf("set\t- edit int value\n"
"fset\t- edit float value\n"
"all\t- view all\n"
"view\t- view one\n"
"follow\t- view changes\n"
"motor\t- control motor\n");
break;
case SET:
sscanf(buff, "%*s %d %d", &a, &b);
bprintf("term: id=%d val=%d", a, b);
dbg_set(a, &b, INT);
break;
case FSET:
sscanf(buff, "%*s %d %d", &a, &b);
fa = (float)a;
dbg_set(a, &fa, FLOAT);
break;
case ALL:
bprintf("| %d vars\t|\n", dbg_watch_count);
for(unsigned char i=0; i<dbg_watch_count; i++) {
bprintf("%d:\t", i);
dbg_print(i);
bprintf("\n");
}
break;
case VIEW:
sscanf(buff, "%*s %d", &a);
bprintf("val is ");
dbg_print(a);
break;
case FOLLOW:
sscanf(buff, "%*s %d", &dbg_index);
mode = FOLLOWING;
create_thread(&update, STACK_DEFAULT, 1, "follower_thread");
break;
case MOTOR:
sscanf(buff, "%*s %d %d", &a, &b);
motor_set_vel(a, b);
break;
default:
bprintf("not understood.\n");
}
}
void recv_payload(void) {
uint8_t idx = 0;
// get target address
tx_addr[0] = uart_rb_rx();
tx_addr[1] = uart_rb_rx();
tx_addr[2] = uart_rb_rx();
printf("%c%c%c", tx_addr[0], tx_addr[1], tx_addr[2]);
// get a full payload's worth of characters
idx = 0;
while (idx < COM_PL_SIZE) {
txbuf[idx++] = uart_rb_rx();
}
// send payload, wait until done
idx = nrf_transmit_packet(tx_addr, txbuf);
// print result
if (idx) {
uart_rb_tx('.');
} else {
uart_rb_tx('X');
}
// happy blinky lights
dbg_set(txbuf[0]);
return;
}
int main(void) {
dbg_init();
dbg_set(0xA);
uart_rb_init();
uart_printf_init();
nrf_init(rxbuf);
nrf_set_channel(115);
nrf_set_power(NRF_CFG_RF_GAIN_M12);
nrf_enable_pipe(0, tx_addr);
sei();
while (1) {
transmitter_loop();
}
}
// Actually process a command. Most command types are turned into
// events on the event queue, but others are processed instantaneously.
// if there's a response event, fill in reply_msg with the event and return 1
int processCommand(int clifd, int clidx, GuiMsg *msg, char *payLoad,
unsigned char** replyMsg, int* replyLen) {
int ret = 0;
switch (msg->msgType) {
case AM_SETLINKPROBCOMMAND:
{
SetLinkProbCommand *linkmsg = (SetLinkProbCommand*)payLoad;
double prob = ((double)linkmsg->scaledProb)/10000;
set_link_prob_value(msg->moteID, linkmsg->moteReceiver, prob);
break;
}
case AM_SETADCPORTVALUECOMMAND:
{
SetADCPortValueCommand *adcmsg = (SetADCPortValueCommand*)payLoad;
set_adc_value(msg->moteID, adcmsg->port, adcmsg->value);
break;
}
case AM_SETRATECOMMAND:
{
SetRateCommand *ratemsg = (SetRateCommand*)payLoad;
set_sim_rate(ratemsg->rate);
break;
}
case AM_VARIABLERESOLVECOMMAND:
{
VariableResolveResponse varResult;
VariableResolveCommand *rmsg = (VariableResolveCommand*)payLoad;
/*
* Note that the following will need to be changed on
* non-32bit systems.
*/
if (__nesc_nido_resolve(msg->moteID, (char*)rmsg->name,
(uintptr_t*)&varResult.addr,
(size_t*)&varResult.length) != 0)
{
varResult.addr = 0;
varResult.length = -1;
}
dbg_clear(DBG_SIM, "SIM: Resolving variable %s for mote %d: 0x%x %d\n",
rmsg->name, msg->moteID, varResult.addr, varResult.length);
buildTossimEvent(TOS_BCAST_ADDR, AM_VARIABLERESOLVERESPONSE,
tos_state.tos_time, &varResult, replyMsg, replyLen);
ret = 1;
break;
}
case AM_VARIABLEREQUESTCOMMAND:
{
VariableRequestResponse varResult;
VariableRequestCommand *rmsg = (VariableRequestCommand*)payLoad;
uint8_t* ptr = (uint8_t*)rmsg->addr;
varResult.length = rmsg->length;
if (varResult.length == 0)
varResult.length = 256; // special case
memcpy(varResult.value, ptr, varResult.length);
buildTossimEvent(TOS_BCAST_ADDR, AM_VARIABLEREQUESTRESPONSE,
tos_state.tos_time, &varResult, replyMsg, replyLen);
ret = 1;
break;
}
case AM_GETMOTECOUNTCOMMAND:
{
int i;
GetMoteCountResponse countResponse;
countResponse.totalMotes = tos_state.num_nodes;
bzero(&countResponse.bitmask, sizeof(countResponse.bitmask));
for (i = 0; i < TOSNODES; i++) {
countResponse.bitmask[i/8] |= (1 << (7 - (i % 8)));
}
buildTossimEvent(TOS_BCAST_ADDR, AM_GETMOTECOUNTRESPONSE,
tos_state.tos_time, &countResponse, replyMsg, replyLen);
ret = 1;
break;
}
case AM_SETDBGCOMMAND:
{
SetDBGCommand* cmd = (SetDBGCommand*)payLoad;
dbg_set(cmd->dbg);
break;
}
case AM_SETEVENTMASKCOMMAND:
{
SetEventMaskCommand* setMaskCommand = (SetEventMaskCommand*)payLoad;
eventMask = setMaskCommand->mask;
break;
}
case AM_BEGINBATCHCOMMAND:
{
if (batchState[clidx] != 0) {
dbg(DBG_SIM|DBG_ERROR, "SIM: duplicate begin batch");
}
dbg(DBG_SIM, "SIM: begin batch");
batchState[clidx] = 1;
break;
}
case AM_ENDBATCHCOMMAND:
{
if (batchState[clidx] == 0) {
dbg(DBG_SIM|DBG_ERROR, "SIM: end batch without begin");
}
dbg(DBG_SIM, "SIM: end batch");
batchState[clidx] = 0;
break;
}
default:
{
// For all other commands, place on the event queue.
// See event_command_in_handle for these
event_t* event = (event_t*)malloc(sizeof(event_t));
event_command_in_create(event, msg, payLoad);
dbg(DBG_SIM, "SIM: Enqueuing command event 0x%lx\n", (unsigned long)event);
TOS_queue_insert_event(event);
}
}
return ret;
}
