int main() {
printf("Hello, World!\n");
int listenfd, connfd;
socklen_t clilen;
struct sockaddr_in cliaddr, servaddr;
listenfd = Socket(AF_INET, SOCK_STREAM, 0);
bzero(&servaddr, sizeof(servaddr));
servaddr.sin_family = AF_INET;
servaddr.sin_addr.s_addr = htonl(INADDR_ANY);
servaddr.sin_port = htons(SERV_PORT);
Bind(listenfd, (SA *) &servaddr, sizeof(servaddr));
Listen(listenfd, LISTENQ);
for (;;) {
clilen = sizeof(cliaddr);
connfd = Accept(listenfd, (SA *) &cliaddr, &clilen);
receive_cmd(connfd);
Close(connfd); // parent closes connected socket
}
return 0;
}
void text_interface(void)
{
char cmd[MAX_CMD_LEN];
uint8_t speed, current_sense;
uint16_t ticks;
uint16_t t;
uint8_t i, cs;
for(i = 0; i < 5; i++)
{
set_led_rgb(0, 0, 255);
_delay_ms(150);
set_led_rgb(0, 0, 0);
_delay_ms(150);
}
set_led_pattern(LED_PATTERN_IDLE);
for(;;)
{
cli();
g_reset = 0;
g_current_sense_detected = 0;
setup();
stop_motor();
serial_init();
cs = 0;
sei();
_delay_ms(10);
dprintf("\nParty Robotics Dispenser at your service!\n\n");
for(;;)
{
cli();
cs = g_current_sense_detected;
sei();
if (!receive_cmd(cmd))
break;
if (sscanf(cmd, "speed %hhu %hhu", &speed, ¤t_sense) == 2)
{
if (!cs)
set_motor_speed(speed, current_sense);
if (current_sense == 0)
flush_saved_tick_count(0);
continue;
}
if (sscanf(cmd, "tickdisp %hu %hhu", (short unsigned int *)&ticks, &speed) == 2)
{
if (!cs)
{
dispense_ticks(ticks, speed);
flush_saved_tick_count(0);
}
continue;
}
if (sscanf(cmd, "timedisp %hu", (short unsigned int *)&t) == 1)
{
if (!cs)
{
run_motor_timed(t);
flush_saved_tick_count(0);
}
continue;
}
if (strncmp(cmd, "forward", 7) == 0)
{
set_motor_direction(MOTOR_DIRECTION_FORWARD);
continue;
}
if (strncmp(cmd, "backward", 8) == 0)
{
set_motor_direction(MOTOR_DIRECTION_BACKWARD);
continue;
}
if (strncmp(cmd, "led_idle", 8) == 0)
{
set_led_pattern(LED_PATTERN_IDLE);
continue;
}
if (strncmp(cmd, "led_dispense", 12) == 0)
{
set_led_pattern(LED_PATTERN_DISPENSE);
continue;
}
if (strncmp(cmd, "led_done", 8) == 0)
{
set_led_pattern(LED_PATTERN_DRINK_DONE);
continue;
}
if (strncmp(cmd, "led_clean", 8) == 0)
{
set_led_pattern(LED_PATTERN_CLEAN);
continue;
}
if (strncmp(cmd, "help", 4) == 0)
{
dprintf("You can use these commands:\n");
dprintf(" speed <speed> <cs>\n");
dprintf(" tickdisp <ticks> <speed>\n");
dprintf(" timedisp <ms>\n");
dprintf(" forward\n");
dprintf(" backward\n");
dprintf(" reset\n");
dprintf(" led_idle\n");
dprintf(" led_dispense\n");
dprintf(" led_done\n");
dprintf(" led_clean\n\n");
dprintf("speed is from 0 - 255. cs = current sense and is 0 or 1.\n");
dprintf("ticks == number of quarter turns. ms == milliseconds\n");
continue;
}
if (strncmp(cmd, "reset", 5) == 0)
break;
dprintf("Unknown command. Use help to get, eh help. Duh.\n");
}
}
}
static int handle_xml_cmd(int sock, char *data, int len)
{
int out_fields[RESP_FIELD_MAX + 1];
void *out_params[RESP_FIELD_MAX + 1];
int in_fields[CMD_FIELD_MAX + 1];
void *in_params[CMD_FIELD_MAX];
int ret;
char *resp_buf = NULL;
struct node_info info = {
.cmd = -1,
.timeout = DEF_TIMEOUT,
.debug = 0
};
INFO("recv cmd: %s", data);
memset(in_fields, 0, sizeof(in_fields));
memset(in_params, 0, sizeof(in_params));
memset(out_fields, 0, sizeof(out_fields));
memset(out_params, 0, sizeof(out_params));
if (parseXmlContent(data, len, in_fields, in_params, &info)) {
ERROR("xml parsing error.");
return -1;
}
if (!info.debug) {
/* DEBUG("alarm %d", info.timeout); */
alarm(info.timeout);
}
ret = executeCmd(info.cmd, in_fields, in_params, out_fields, out_params);
freeParams(in_fields, in_params);
if (ret)
ERROR("cmd %s failed: %s", cmd_name[info.cmd], strerror(ret));
ret = createXmlResult(info.cmd, ret, out_fields, out_params, &resp_buf);
freeParams(out_fields, out_params);
if (!info.debug)
alarm(0);
if (ret || !resp_buf) {
ERROR("create result string error.");
return -1;
}
ret = send_resp(sock, resp_buf, strlen(resp_buf));
free(resp_buf);
return ret;
}
static void handle_request(int sock)
{
int command, ret = 0;
char *buffer = malloc(MAX_CMD_BUF + 1);
if (!buffer) {
ERROR("malloc error : %s", strerror(errno));
return;
}
while (ret == 0) {
ret = receive_cmd(sock, buffer, MAX_CMD_BUF, &command);
//DEBUG("received cmd %d len %d", command, ret);
if (ret == 0) {
ERROR("Socket closed");
break;
}
if (ret > 0 && command != CMD_XML) {
ERROR("Not supported command %d", command);
ret = INVALID_DATA;
}
if (ret < 0) {
sprintf(buffer, resp_xml, status_str[-ret]);
send_resp(sock, buffer, strlen(buffer));
break;
}
ret = handle_xml_cmd(sock, buffer, ret /* len */);
if (ret < 0) {
ERROR("xml command %d error %d", command, ret);
break;
}
}
free(buffer);
}
// Receives from another IP server and sends reply back
void* IPTask(void* _IP_data_task) {
struct IP_data* IP_data_task = (struct IP_data*) _IP_data_task;
struct Context* context = (struct Context*) IP_data_task->ptr_context;
unsigned char VPS = IP_data_task->VPS, *dataPtr, msgFlag;
char printbuffer[DATASIZE];
///////////////////////////////////////
// task_id needs to be made generic! //
//name_id[SENDERNAMESIZE] = {IPTask1_id,UNUSED,UNUSED};
///////////////////////////////////////
unsigned char name_id[SENDERNAMESIZE] = { context->task_id, UNUSED,UNUSED };
int server_port = IP_data_task->port;
printf("IPTask%i: Started\n", VPS);
// Initialize socket and port
int s, client;
unsigned int tmp;
fd_set fdset;
struct timeval timer;
struct sockaddr_in c_addr;
//unsigned char *pointer;
if ((s = server_init(server_port)) < 0)
//return(s);
FD_ZERO(&fdset);
FD_SET(s, &fdset);
timer.tv_sec = 30;
timer.tv_usec = 0;
pthread_mutex_lock(context->mutex);
while (communication_needed) {
//Wait for IP packet
if (select(s + 1, &fdset, NULL, NULL, NULL)> 0 ) {
//#ifdef _DEBUG_VERBOSE
printf("IPTask%i: SELECT has continued\n",VPS);
//#endif
tmp = sizeof(c_addr);
client = accept(s, (struct sockaddr *)&c_addr, &tmp);
if (client < 0) {
printf("FAIL1: %s\n", strerror(errno));
}
if (PRINT_STATUS) {
printf("IPTask%i: client connect from %s\n",VPS,
inet_ntoa(c_addr.sin_addr));
}
usleep(10000);
if ( (dataPtr = receive_cmd(client, VPS)) != NULL )
{
//#ifdef _DEBUG_VERBOSE
printf("IPTask%i: Got [%s] from receive_cmd (ASCII)\n", VPS,binary_to_hexbinaryString(dataPtr,dataPtr[1]+2,printbuffer));
//#endif
dataPtr[1] = strlen((char*)dataPtr+2); // TODO remove warning
// Convert hexbinary to binary if first char is a hexbin char else skip conv
if( ((0x30 <= dataPtr[2]) && (dataPtr[2] <= 0x39)) || (('a' <= dataPtr[2] ) && (dataPtr[2] <= 'f')) || (('A' <= dataPtr[2]) && (dataPtr[2] <= 'F')))
{ dataPtr[1] = hexbinary_to_binary( &dataPtr[2], &dataPtr[2]);}
printf("IPTask%i: Receiving from port %i: [",VPS, server_port);
printf("%s]\n", binary_to_hexbinaryString(dataPtr,dataPtr[1]+2,printbuffer));
//printf("IPTask%i: Has data after strcat: [%s]\n",VPS,binary_to_hexbinaryString(dataPtr,dataPtr[1]+2,printbuffer));
// Pass message on
//#ifdef _DEBUG_VERBOSE
printf("broadcast: IPTask%i\n",VPS);
//#endif
broadcast_msg(name_id, Out1, dataPtr);
// DEBUG: Must wait for receiver to pass data on
//sleep(1.5);
// Begin listening on Condition for reply
//#ifdef _DEBUG_VERBOSE
printf("IPTask%i: calling receive()\n",VPS);
//#endif
//msgFlag = receive(IPTask1_id, context->condition, context->mutex);
msgFlag = receive(VPS, context->condition, context->mutex);
dataPtr = context->holder;
printf(" *** IPTask%i: Sending on port %i: [%s] ***\n", VPS, server_port, binary_to_hexbinaryString(dataPtr,dataPtr[1]+2,printbuffer));
//#ifdef _DEBUG_VERBOSE
printf("IPTask%i: received from sender: [%s]\n", VPS, binary_to_hexbinaryString(context->sender,SENDERNAMESIZE,printbuffer));
//#endif
// Send reply back thru open connection
// dataPtr+2 to avoid sending the VPS and length
//if(server_port==MANAGEMENTPORT) IP_send_server_String(client, dataPtr+2, dataPtr[1]);
//else IP_send_server_hexbinaryString(client, dataPtr+2, dataPtr[1]);
IP_send_server_hexbinaryString(client, dataPtr+2, dataPtr[1]);
printf("IPTask%i: ip send server func: \n", VPS);
}
else printf("IPTask%i: no data in client \n", VPS);
// Shutdown connection
if ((shutdown(client, 0) < 0) || (close(client) < 0)) {
printf("FAIL2: %s\n", strerror(errno));
}
} // end select
} // while-end
// Shutdown listening server
if (s> 0)
server_end(s);
printf("IPTask%i: unlocking mutex and stopping\n",VPS);
pthread_mutex_unlock(context->mutex);
return NULL;
}
bool avs_start(FilterInfo *info, FilterInfo *avisynth_info,
char *fname, AVS_PIPES *avs_pipes)
{
DEBUG_PRINTF("avsfilter : avs_start()\n");
DEBUG_PRINTF("avsfilter : %X %X %s %X\n",
avs_pipes[PIPE_LOADER_WRITE].hpipe,
avs_pipes[PIPE_FILTER_WRITE].hpipe,
fname, info);
DEBUG_PRINTF("avsfilter : avs_start info : frameIncrement %lu totalDuration %llu\n",
info->frameIncrement, info->totalDuration);
ADV_Info aii, aio;
aii.width = info->width;
aii.height = info->height;
aii.nb_frames = info->totalDuration / info->frameIncrement;
aii.encoding = 1;
aii.codec = 0;
aii.fps1000 = ADM_Fps1000FromUs(info->frameIncrement);
aii.orgFrame = 0;
DEBUG_PRINTF("avsfilter : send ADV_Info to avsloader [fps1000 = %d, nb_frames = %d]\n", aii.fps1000, aii.nb_frames);
if (!send_cmd(avs_pipes[PIPE_LOADER_WRITE].hpipe,
LOAD_AVS_SCRIPT, fname,
strlen(fname) + sizeof("\0")) ||
!send_cmd(avs_pipes[PIPE_FILTER_WRITE].hpipe,
SET_CLIP_PARAMETER, &aii,
sizeof(aii)))
{
DEBUG_PRINTF_RED("avsfilter : cannot set script name or set clip parameters\n");
deinit_pipes(avs_pipes, CMD_PIPE_NUM);
return false;
}
// get avisynth frame info
PIPE_MSG_HEADER msg;
if (!receive_cmd(avs_pipes[PIPE_LOADER_READ].hpipe,
&msg) ||
msg.avs_cmd != SET_CLIP_PARAMETER ||
!receive_data(avs_pipes[PIPE_LOADER_READ].hpipe,
&msg, &aio))
{
DEBUG_PRINTF_RED("avsfilter : cannot receive avisynth clip parameters\n");
deinit_pipes(avs_pipes, CMD_PIPE_NUM);
return false;
}
DEBUG_PRINTF("avsfilter : receive ADV_Info from avsloader [fps1000 = %d, nb_frames = %d]\n", aio.fps1000, aio.nb_frames);
avisynth_info->width = aio.width;
avisynth_info->height = aio.height;
avisynth_info->frameIncrement = ADM_UsecFromFps1000(aio.fps1000);
avisynth_info->totalDuration = aio.nb_frames * avisynth_info->frameIncrement;
// correct avisynth_info for span of frames, calculate fps change metrics
/* float k_fps;
k_fps = float(avisynth_info->frameIncrement) / float(info->frameIncrement);
DEBUG_PRINTF("avsfilter : FPS change metrics %f\n", k_fps);
avisynth_info->nb_frames = int (info->nb_frames * k_fps);
avisynth_info->orgFrame = int (info->orgFrame * k_fps);
DEBUG_PRINTF("avsfilter : Calculate new span for avisynth script [%d - %d]\n",
avisynth_info->orgFrame,avisynth_info->orgFrame + avisynth_info->nb_frames);*/
return true;
}
