/* * Reschedule a volume output item, if necessary. */ static int reschedule_volume_output_item(struct volume *wrld, struct volume_output_item *vo) { /* Find the next time */ if (vo->timer_type == OUTPUT_BY_STEP) vo->t += (vo->step_time / wrld->time_unit); else { double time_scale = 0.0; /* Check if we're done */ if (vo->next_time == vo->times + vo->num_times) { free(vo->filename_prefix); free(vo->molecules); free(vo->times); free(vo); return 0; } /* Compute the next time and advance the next_time ptr */ if (vo->timer_type == OUTPUT_BY_ITERATION_LIST) time_scale = 1.0; else time_scale = 1.0 / wrld->time_unit; vo->t = (*vo->next_time++) * time_scale; } switch (wrld->notify->volume_output_report) { case NOTIFY_NONE: case NOTIFY_BRIEF: break; case NOTIFY_FULL: mcell_log(" Next output scheduled for time %.15g.", vo->t * wrld->time_unit); break; default: UNHANDLED_CASE(wrld->notify->volume_output_report); } /* Add to the schedule */ if (schedule_add(wrld->volume_output_scheduler, vo)) mcell_allocfailed("Failed to add volume output request to scheduler."); return 0; }
void key_press(int hwkey) { int key; fn_action = 0; key = key_map(hwkey); if (!key) return; if (key_special_action(key)) return; if (repeating_key(key)) { rep_key = key; dprintf("Adding Keyrepeat\n"); schedule_add(sched, kbdDelay, FBVNC_EVENT_KEYREPEAT); } #ifdef HAVE_DBOX_HARDWARE keys_sent_map[hwkey] = key; #endif dprintf("key_press: hwkey=%d, keysym=%d\n", hwkey, key); SendKeyEvent(key, 1); }
int RTSP_setup(RTSP_buffer * pRtsp) { char s8TranStr[128], *s8Str; char *pStr; RTP_transport Transport; int s32SessionID=0; RTP_session *rtp_s, *rtp_s_prec; RTSP_session *rtsp_s; if ((s8Str = strstr(pRtsp->in_buffer, HDR_TRANSPORT)) == NULL) { fprintf(stderr, "Error %s,%i\n", __FILE__, __LINE__); send_reply(406, 0, pRtsp); // Not Acceptable printf("not acceptable"); return ERR_NOERROR; } //检查传输层子串是否正确 if (sscanf(s8Str, "%*10s %255s", s8TranStr) != 1) { fprintf(stderr,"SETUP request malformed: Transport string is empty\n"); send_reply(400, 0, pRtsp); // Bad Request printf("check transport 400 bad request"); return ERR_NOERROR; } fprintf(stderr,"*** transport: %s ***\n", s8TranStr); //如果需要增加一个会话 if ( !pRtsp->session_list ) { pRtsp->session_list = (RTSP_session *) calloc(1, sizeof(RTSP_session)); } rtsp_s = pRtsp->session_list; //建立一个新会话,插入到链表中 if (pRtsp->session_list->rtp_session == NULL) { pRtsp->session_list->rtp_session = (RTP_session *) calloc(1, sizeof(RTP_session)); rtp_s = pRtsp->session_list->rtp_session; } else { for (rtp_s = rtsp_s->rtp_session; rtp_s != NULL; rtp_s = rtp_s->next) { rtp_s_prec = rtp_s; } rtp_s_prec->next = (RTP_session *) calloc(1, sizeof(RTP_session)); rtp_s = rtp_s_prec->next; } /* printf("\tFile Name %s\n", Object); if((rtp_s->file_id = open(Object, O_RDONLY)) < 0) { printf("Open File error %s, %d\n", __FILE__, __LINE__); } */ //起始状态为暂停 rtp_s->pause = 1; rtp_s->hndRtp = NULL; Transport.type = RTP_no_transport; if((pStr = strstr(s8TranStr, RTSP_RTP_AVP))) { //Transport: RTP/AVP pStr += strlen(RTSP_RTP_AVP); if ( !*pStr || (*pStr == ';') || (*pStr == ' ')) { //单播 if (strstr(s8TranStr, "unicast")) { //如果指定了客户端端口号,填充对应的两个端口号 if( (pStr = strstr(s8TranStr, "client_port")) ) { pStr = strstr(s8TranStr, "="); sscanf(pStr + 1, "%d", &(Transport.u.udp.cli_ports.RTP)); pStr = strstr(s8TranStr, "-"); sscanf(pStr + 1, "%d", &(Transport.u.udp.cli_ports.RTCP)); } //服务器端口 if (RTP_get_port_pair(&Transport.u.udp.ser_ports) != ERR_NOERROR) { fprintf(stderr, "Error %s,%d\n", __FILE__, __LINE__); send_reply(500, 0, pRtsp);/* Internal server error */ return ERR_GENERIC; } //建立RTP套接字 rtp_s->hndRtp = (struct _tagStRtpHandle*)RtpCreate((unsigned int)(((struct sockaddr_in *)(&pRtsp->stClientAddr))->sin_addr.s_addr), Transport.u.udp.cli_ports.RTP, _h264nalu); printf("<><><><>Creat RTP<><><><>\n"); Transport.u.udp.is_multicast = 0; } else { printf("multicast not codeing\n"); //multicast 多播处理.... } Transport.type = RTP_rtp_avp; } else if (!strncmp(s8TranStr, "/TCP", 4)) { if( (pStr = strstr(s8TranStr, "interleaved")) ) { pStr = strstr(s8TranStr, "="); sscanf(pStr + 1, "%d", &(Transport.u.tcp.interleaved.RTP)); if ((pStr = strstr(pStr, "-"))) sscanf(pStr + 1, "%d", &(Transport.u.tcp.interleaved.RTCP)); else Transport.u.tcp.interleaved.RTCP = Transport.u.tcp.interleaved.RTP + 1; } else { } Transport.rtp_fd = pRtsp->fd; // Transport.rtcp_fd_out = pRtsp->fd; // Transport.rtcp_fd_in = -1; Transport.type = RTP_rtp_avp_tcp; } } printf("pstr=%s\n",pStr); if (Transport.type == RTP_no_transport) { fprintf(stderr,"AAAAAAAAAAA Unsupported Transport,%s,%d\n", __FILE__, __LINE__); send_reply(461, 0, pRtsp);// Bad Request return ERR_NOERROR; } memcpy(&rtp_s->transport, &Transport, sizeof(Transport)); //如果有会话头,就有了一个控制集合 if ((pStr = strstr(pRtsp->in_buffer, HDR_SESSION)) != NULL) { if (sscanf(pStr, "%*s %d", &s32SessionID) != 1) { fprintf(stderr, "Error %s,%i\n", __FILE__, __LINE__); send_reply(454, 0, pRtsp); // Session Not Found return ERR_NOERROR; } } else { //产生一个非0的随机的会话序号 struct timeval stNowTmp; gettimeofday(&stNowTmp, 0); srand((stNowTmp.tv_sec * 1000) + (stNowTmp.tv_usec / 1000)); s32SessionID = 1 + (int) (10.0 * rand() / (100000 + 1.0)); if (s32SessionID == 0) { s32SessionID++; } } pRtsp->session_list->session_id = s32SessionID; pRtsp->session_list->rtp_session->sched_id = schedule_add(rtp_s); send_setup_reply(pRtsp, rtsp_s, rtp_s); return ERR_NOERROR; }
/************************************************************************** update_reaction_output: In: the output_block we want to update Out: 0 on success, 1 on failure. The counters in this block are updated, and the block is rescheduled for the next output time. The counters are saved to an internal buffer, and written out when full. **************************************************************************/ int update_reaction_output(struct volume *world, struct output_block *block) { int report_as_non_trigger = 1; int i = block->buf_index; if (block->data_set_head != NULL && block->data_set_head->column_head != NULL && block->data_set_head->column_head->buffer[i].data_type == COUNT_TRIG_STRUCT) report_as_non_trigger = 0; if (report_as_non_trigger) { switch (world->notify->reaction_output_report) { case NOTIFY_NONE: break; case NOTIFY_BRIEF: mcell_log( "Updating reaction output scheduled at time %.15g on iteration %lld.", block->t, world->current_iterations); break; case NOTIFY_FULL: mcell_log("Updating reaction output scheduled at time %.15g on iteration" " %lld.\n Buffer fill level is at %u/%u.", block->t, world->current_iterations, block->buf_index, block->buffersize); break; default: UNHANDLED_CASE(world->notify->reaction_output_report); } } /* update all counters */ block->t /= (1. + EPS_C); if (world->chkpt_seq_num == 1) { if (block->timer_type == OUTPUT_BY_ITERATION_LIST) block->time_array[i] = block->t; else block->time_array[i] = block->t * world->time_unit; } else { if (block->timer_type == OUTPUT_BY_ITERATION_LIST) { block->time_array[i] = block->t; } else if (block->timer_type == OUTPUT_BY_TIME_LIST) { if (block->time_now == NULL) { return 0; } else { block->time_array[i] = block->time_now->value; } } else { /* OUTPUT_BY_STEP */ block->time_array[i] = convert_iterations_to_seconds( world->start_iterations, world->time_unit, world->simulation_start_seconds, block->t); } } struct output_set *set; struct output_column *column; // Each file for (set = block->data_set_head; set != NULL; set = set->next) { if (report_as_non_trigger) { if (world->notify->reaction_output_report == NOTIFY_FULL) mcell_log(" Processing reaction output file '%s'.", set->outfile_name); } // Each column for (column = set->column_head; column != NULL; column = column->next) { if (column->buffer[i].data_type != COUNT_TRIG_STRUCT) { eval_oexpr_tree(column->expr, 1); switch (column->buffer[i].data_type) { case COUNT_INT: column->buffer[i].val.ival = (int)column->expr->value; break; case COUNT_DBL: column->buffer[i].val.dval = (double)column->expr->value; break; case COUNT_UNSET: column->buffer[i].val.cval = 'X'; break; case COUNT_TRIG_STRUCT: default: UNHANDLED_CASE(column->buffer[i].data_type); } } } } block->buf_index++; int final_chunk_flag = 0; // flag signaling an end to the scheduled // reaction outputs. Takes values {0,1}. // 0 - end not reached yet, // 1 - end reached. /* Pick time of next output, if any */ if (block->timer_type == OUTPUT_BY_STEP) block->t += block->step_time / world->time_unit; else if (block->time_now != NULL) { block->time_now = block->time_now->next; if (block->time_now == NULL) final_chunk_flag = 1; else { if (block->timer_type == OUTPUT_BY_ITERATION_LIST) block->t = block->time_now->value; else { /* OUTPUT_BY_TIME_LIST */ if (world->chkpt_seq_num == 1) { block->t = block->time_now->value / world->time_unit; } else { block->t = world->start_iterations + (block->time_now->value - world->simulation_start_seconds) / world->time_unit; } } } } else final_chunk_flag = 1; /* Schedule next output event--even if we're at the end, since triggers may * not yet be written */ double actual_t; if (final_chunk_flag == 1) { actual_t = block->t; block->t = FOREVER; } else actual_t = -1; block->t *= (1. + EPS_C); if (schedule_add(world->count_scheduler, block)) { mcell_allocfailed_nodie("Failed to add count to scheduler."); return 1; } if (distinguishable(actual_t, -1, EPS_C)) block->t = actual_t; /* Fix time for output */ if (report_as_non_trigger && world->notify->reaction_output_report == NOTIFY_FULL) { mcell_log(" Next output for this block scheduled at time %.15g.", block->t); } if (block->t >= world->iterations + 1) final_chunk_flag = 1; /* write data to outfile */ if (block->buf_index == block->buffersize || final_chunk_flag) { for (set = block->data_set_head; set != NULL; set = set->next) { if (set->column_head->buffer[i].data_type == COUNT_TRIG_STRUCT) continue; if (write_reaction_output(world, set)) { mcell_error_nodie("Failed to write reaction output to file '%s'.", set->outfile_name); return 1; } } block->buf_index = 0; no_printf("Done updating reaction output\n"); } if (distinguishable(actual_t, -1, EPS_C)) block->t = FOREVER; /* Back to infinity if we're done */ return 0; }
int RTSP_setup(RTSP_buffer * rtsp, RTSP_session ** new_session) { char address[16]; char object[255], server[255]; char url[255]; unsigned short port; RTSP_session *rtsp_s; RTP_session *rtp_s, *rtp_s_prec; int SessionID = 0; // port_pair cli_ports; // port_pair ser_ports; struct timeval now_tmp; char *p/* = NULL*/; unsigned int start_seq, start_rtptime; char transport_str[255]; media_entry *list, *matching_me, req; struct sockaddr_storage rtsp_peer; socklen_t namelen = sizeof(rtsp_peer); unsigned long ssrc; SD_descr *matching_descr; unsigned char is_multicast_dad = 1; //unicast and the first multicast RTP_transport transport; char *saved_ptr, *transport_tkn; int max_interlvd; // init memset(&req, 0, sizeof(req)); memset(&transport, 0, sizeof(transport)); // Parse the input message /* Get the URL */ if (!sscanf(rtsp->in_buffer, " %*s %254s ", url)) { send_reply(400, 0, rtsp); /* bad request */ return ERR_NOERROR; } /* Validate the URL */ switch (parse_url(url, server, sizeof(server), &port, object, sizeof(object))) { //object is requested file's name case 1: // bad request send_reply(400, 0, rtsp); return ERR_NOERROR; case -1: // interanl server error send_reply(500, 0, rtsp); return ERR_NOERROR; break; default: break; } if (strcmp(server, prefs_get_hostname()) != 0) { /* Currently this feature is disabled. */ /* wrong server name */ // send_reply(404, 0 , rtsp); /* Not Found */ // return ERR_NOERROR; } if (strstr(object, "../")) { /* disallow relative paths outside of current directory. */ send_reply(403, 0, rtsp); /* Forbidden */ return ERR_NOERROR; } if (strstr(object, "./")) { /* Disallow the ./ */ send_reply(403, 0, rtsp); /* Forbidden */ return ERR_NOERROR; } if (!(p = strrchr(object, '.'))) { // if filename is without extension send_reply(415, 0, rtsp); /* Unsupported media type */ return ERR_NOERROR; } else if (!is_supported_url(p)) { //if filename's extension is not valid send_reply(415, 0, rtsp); /* Unsupported media type */ return ERR_NOERROR; } if ( !(p = strchr(object, '!')) ) { //if '!' is not present then a file has not been specified send_reply(500, 0, rtsp); /* Internal server error */ return ERR_NOERROR; } else { // SETUP name.sd!stream strcpy(req.filename, p + 1); req.flags |= ME_FILENAME; *p = '\0'; } // ------------ START PATCH { char temp[255]; char *pd=NULL; strcpy(temp, object); #if 0 printf("%s\n", object); // BEGIN // if ( (p = strstr(temp, "/")) ) { if ( (p = strchr(temp, '/')) ) { strcpy(object, p + 1); // CRITIC. } printf("%s\n", temp); #endif // pd = strstr(p, ".sd"); // this part is usefull in order to pd = strstr(temp, ".sd"); if ( (p = strstr(pd + 1, ".sd")) ) { // have compatibility with RealOne strcpy(object, pd + 4); // CRITIC. } //Note: It's a critic part // END } // ------------ END PATCH if (enum_media(object, &matching_descr) != ERR_NOERROR) { send_reply(500, 0, rtsp); /* Internal server error */ return ERR_NOERROR; } list=matching_descr->me_list; if (get_media_entry(&req, list, &matching_me) == ERR_NOT_FOUND) { send_reply(404, 0, rtsp); /* Not found */ return ERR_NOERROR; } // Get the CSeq if ((p = strstr(rtsp->in_buffer, HDR_CSEQ)) == NULL) { send_reply(400, 0, rtsp); /* Bad Request */ return ERR_NOERROR; } else { if (sscanf(p, "%*s %d", &(rtsp->rtsp_cseq)) != 1) { send_reply(400, 0, rtsp); /* Bad Request */ return ERR_NOERROR; } } /*if ((p = strstr(rtsp->in_buffer, "ssrc")) != NULL) { p = strchr(p, '='); sscanf(p + 1, "%lu", &ssrc); } else {*/ ssrc = random32(0); //} // Start parsing the Transport header if ((p = strstr(rtsp->in_buffer, HDR_TRANSPORT)) == NULL) { send_reply(406, "Require: Transport settings" /* of rtp/udp;port=nnnn. "*/, rtsp); /* Not Acceptable */ return ERR_NOERROR; } if (sscanf(p, "%*10s%255s", transport_str) != 1) { fnc_log(FNC_LOG_ERR,"SETUP request malformed: Transport string is empty\n"); send_reply(400, 0, rtsp); /* Bad Request */ return ERR_NOERROR; } printf("transport: %s\n", transport_str); // XXX tmp. // tokenize the coma seaparated list of transport settings: if ( !(transport_tkn=strtok_r(transport_str, ",", &saved_ptr)) ) { fnc_log(FNC_LOG_ERR,"Malformed Transport string from client\n"); send_reply(400, 0, rtsp); /* Bad Request */ return ERR_NOERROR; } if (getpeername(rtsp->fd, (struct sockaddr *)&rtsp_peer, &namelen) != 0) { send_reply(415, 0, rtsp); // Internal server error return ERR_GENERIC; } transport.type = RTP_no_transport; do { // search a good transport string if ( (p = strstr(transport_tkn, RTSP_RTP_AVP)) ) { // Transport: RTP/AVP p += strlen(RTSP_RTP_AVP); if ( !*p || (*p == ';') || (*p == ' ')) { #if 0 // if ((p = strstr(rtsp->in_buffer, "client_port")) == NULL && strstr(rtsp->in_buffer, "multicast") == NULL) { if ((p = strstr(transport_tkn, "client_port")) == NULL && strstr(transport_tkn, "multicast") == NULL) { send_reply(406, "Require: Transport settings of rtp/udp;port=nnnn. ", rtsp); /* Not Acceptable */ return ERR_NOERROR; } #endif // #if 0 if (strstr(transport_tkn, "unicast")) { if( (p = strstr(transport_tkn, "client_port")) ) { p = strstr(p, "="); sscanf(p + 1, "%d", &(transport.u.udp.cli_ports.RTP)); p = strstr(p, "-"); sscanf(p + 1, "%d", &(transport.u.udp.cli_ports.RTCP)); } if (RTP_get_port_pair(&transport.u.udp.ser_ports) != ERR_NOERROR) { send_reply(500, 0, rtsp); /* Internal server error */ return ERR_GENERIC; } // strcpy(address, get_address()); //UDP connection for outgoing RTP packets udp_connect(transport.u.udp.cli_ports.RTP, &transport.u.udp.rtp_peer, (*((struct sockaddr_in *) (&rtsp_peer))).sin_addr.s_addr,&transport.rtp_fd); //UDP connection for outgoing RTCP packets udp_connect(transport.u.udp.cli_ports.RTCP, &transport.u.udp.rtcp_out_peer,(*((struct sockaddr_in *) (&rtsp_peer))).sin_addr.s_addr, &transport.rtcp_fd_out); udp_open(transport.u.udp.ser_ports.RTCP, &transport.u.udp.rtcp_in_peer, &transport.rtcp_fd_in); //bind transport.u.udp.is_multicast = 0; } else if ( matching_descr->flags & SD_FL_MULTICAST ) { /*multicast*/ // TODO: make the difference between only multicast allowed or unicast fallback allowed. transport.u.udp.cli_ports.RTP = transport.u.udp.ser_ports.RTP =matching_me->rtp_multicast_port; transport.u.udp.cli_ports.RTCP = transport.u.udp.ser_ports.RTCP =matching_me->rtp_multicast_port+1; is_multicast_dad = 0; if (!(matching_descr->flags & SD_FL_MULTICAST_PORT) ) { struct in_addr inp; unsigned char ttl=DEFAULT_TTL; struct ip_mreq mreq; mreq.imr_multiaddr.s_addr = inet_addr(matching_descr->multicast); mreq.imr_interface.s_addr = INADDR_ANY; setsockopt(transport.rtp_fd, IPPROTO_IP, IP_ADD_MEMBERSHIP, &mreq, sizeof(mreq)); setsockopt(transport.rtp_fd, IPPROTO_IP, IP_MULTICAST_TTL, &ttl, sizeof(ttl)); is_multicast_dad = 1; strcpy(address, matching_descr->multicast); //RTP outgoing packets inet_aton(address, &inp); udp_connect(transport.u.udp.ser_ports.RTP, &transport.u.udp.rtp_peer, inp.s_addr, &transport.rtp_fd); //RTCP outgoing packets inet_aton(address, &inp); udp_connect(transport.u.udp.ser_ports.RTCP, &transport.u.udp.rtcp_out_peer, inp.s_addr, &transport.rtcp_fd_out); //udp_open(transport.u.udp.ser_ports.RTCP, &(sp2->rtcp_in_peer), &(sp2->rtcp_fd_in)); //bind if(matching_me->next==NULL) matching_descr->flags |= SD_FL_MULTICAST_PORT; matching_me->rtp_multicast_port = transport.u.udp.ser_ports.RTP; transport.u.udp.is_multicast = 1; fnc_log(FNC_LOG_DEBUG,"\nSet up socket for multicast ok\n"); } } else continue; transport.type = RTP_rtp_avp; break; // found a valid transport } else if (!strncmp(p, "/TCP", 4)) { // Transport: RTP/AVP/TCP;interleaved=x-y // XXX still not finished if( (p = strstr(transport_tkn, "interleaved")) ) { p = strstr(p, "="); sscanf(p + 1, "%d", &(transport.u.tcp.interleaved.RTP)); if ( (p = strstr(p, "-")) ) sscanf(p + 1, "%d", &(transport.u.tcp.interleaved.RTCP)); else transport.u.tcp.interleaved.RTCP = transport.u.tcp.interleaved.RTP + 1; } else { // search for max used interleved channel. max_interlvd = -1; for (rtp_s = (rtsp->session_list)?rtsp->session_list->rtp_session:NULL; rtp_s; rtp_s = rtp_s->next) max_interlvd = max(max_interlvd, (rtp_s->transport.type == RTP_rtp_avp_tcp)?rtp_s->transport.u.tcp.interleaved.RTCP:-1); transport.u.tcp.interleaved.RTP = max_interlvd + 1; transport.u.tcp.interleaved.RTCP = max_interlvd + 2; } transport.rtp_fd = rtsp->fd; // dup(rtsp->fd); transport.rtcp_fd_out = rtsp->fd; // dup(rtsp->fd); transport.rtcp_fd_in = -1; transport.type = RTP_rtp_avp_tcp; break; // found a valid transport } } } while ((transport_tkn=strtok_r(NULL, ",", &saved_ptr))); printf("rtp transport: %d\n", transport.type); if (transport.type == RTP_no_transport) { // fnc_log(FNC_LOG_ERR,"Unsupported Transport\n"); send_reply(461, "Unsupported Transport", rtsp); /* Bad Request */ return ERR_NOERROR; } // If there's a Session header we have an aggregate control if ((p = strstr(rtsp->in_buffer, HDR_SESSION)) != NULL) { if (sscanf(p, "%*s %d", &SessionID) != 1) { send_reply(454, 0, rtsp); /* Session Not Found */ return ERR_NOERROR; } } else { // Generate a random Session number gettimeofday(&now_tmp, 0); srand((now_tmp.tv_sec * 1000) + (now_tmp.tv_usec / 1000)); #ifdef WIN32 SessionID = rand(); #else SessionID = 1 + (int) (10.0 * rand() / (100000 + 1.0)); #endif if (SessionID == 0) { SessionID++; } } // Add an RTSP session if necessary if ( !rtsp->session_list ) { rtsp->session_list = (RTSP_session *) calloc(1, sizeof(RTSP_session)); } rtsp_s = rtsp->session_list; // Setup the RTP session if (rtsp->session_list->rtp_session == NULL) { rtsp->session_list->rtp_session = (RTP_session *) calloc(1, sizeof(RTP_session)); rtp_s = rtsp->session_list->rtp_session; } else { for (rtp_s = rtsp_s->rtp_session; rtp_s != NULL; rtp_s = rtp_s->next) { rtp_s_prec = rtp_s; } rtp_s_prec->next = (RTP_session *) calloc(1, sizeof(RTP_session)); rtp_s = rtp_s_prec->next; } #ifdef WIN32 start_seq = rand(); start_rtptime = rand(); #else // start_seq = 1 + (int) (10.0 * rand() / (100000 + 1.0)); // start_rtptime = 1 + (int) (10.0 * rand() / (100000 + 1.0)); #if 0 start_seq = 1 + (unsigned int) ((float)(0xFFFF) * ((float)rand() / (float)RAND_MAX)); start_rtptime = 1 + (unsigned int) ((float)(0xFFFFFFFF) * ((float)rand() / (float)RAND_MAX)); #else start_seq = 1 + (unsigned int) (rand()%(0xFFFF)); start_rtptime = 1 + (unsigned int) (rand()%(0xFFFFFFFF)); #endif #endif if (start_seq == 0) { start_seq++; } if (start_rtptime == 0) { start_rtptime++; } rtp_s->pause = 1; strcpy(rtp_s->sd_filename, object); /*xxx*/ rtp_s->current_media = (media_entry *) calloc(1, sizeof(media_entry)); // if(!(matching_descr->flags & SD_FL_MULTICAST_PORT)){ if( is_multicast_dad ) { if ( mediacpy(&rtp_s->current_media, &matching_me) ) { send_reply(500, 0, rtsp); /* Internal server error */ return ERR_GENERIC; } } gettimeofday(&now_tmp, 0); srand((now_tmp.tv_sec * 1000) + (now_tmp.tv_usec / 1000)); rtp_s->start_rtptime = start_rtptime; rtp_s->start_seq = start_seq; memcpy(&rtp_s->transport, &transport, sizeof(transport)); rtp_s->is_multicast_dad = is_multicast_dad; /*xxx*/ rtp_s->sd_descr=matching_descr; rtp_s->sched_id = schedule_add(rtp_s); rtp_s->ssrc = ssrc; // Setup the RTSP session rtsp_s->session_id = SessionID; *new_session = rtsp_s; fnc_log(FNC_LOG_INFO,"SETUP %s RTSP/1.0 ",url); send_setup_reply(rtsp, rtsp_s, matching_descr, rtp_s); // See User-Agent if ((p=strstr(rtsp->in_buffer, HDR_USER_AGENT))!=NULL) { char cut[strlen(p)]; strcpy(cut,p); p=strstr(cut, "\n"); cut[strlen(cut)-strlen(p)-1]='\0'; fnc_log(FNC_LOG_CLIENT,"%s\n",cut); } else fnc_log(FNC_LOG_CLIENT,"- \n"); return ERR_NOERROR; }
/*Mainloop*/ void loop() { /*Auswertung des anstehenden Aufgaben im Scheduler*/ int i; unsigned char new_value = 0; unsigned char new_heading = 0; for (i = 0; i < MAX_TASKS; i++) { if (scheduler[i].task == NOTHING || get_current_millis() < scheduler[i].tv_msec) {continue;} switch (scheduler[i].task) { case NOTHING: break; case MEASURE: /*Fordert den als Parameter uebergebenen Sensor auf eine Messung zu starten und bestimmt den Lese-Zeitpunkt*/ srf[scheduler[i].param - SE0_ADDRESS].measure(); schedule_add((long)srf_speed[scheduler[i].param - SE0_ADDRESS], READ_MEASURE, scheduler[i].param); scheduler[i].task = NOTHING; break; case READ_MEASURE: { unsigned short index = scheduler[i].param - SE0_ADDRESS; /*Liest die Messdaten des als Parameter uebergebenen Sensors aus und veranlasst erneute Messung*/ srf[index].read_it(get_current_millis()); schedule_add((long)srf[index].get_delay(), MEASURE, scheduler[i].param); if(state == HOLD_STILL || state == MEASURE_ENVIRONMENT || state == GET_ANCHOR) { nvalue[index] = 1; } /*if (state == HOLD_STILL || state == HEAD_TO_MIDDLE || state = HTM_DELAY) { nvalue2[scheduler[i].param - SE0_ADDRESS] = 1; }*/ new_value = scheduler[i].param; /*Korrektur der Messwerte bei Schieflage des Copters bei glaubhaften Winkeln*/ if ((index == 0 || index == 1) && (abs(current_roll_rad) < MAX_ROLL_ANGLE)) srf[index].set_mean((unsigned short)(srf[index].get_mean()*cos(current_roll_rad))); else if ((index == 2 || index == 3) && (abs(current_pitch_rad) < MAX_PITCH_ANGLE)) srf[index].set_mean((unsigned short)(srf[index].get_mean()*cos(current_roll_rad))); /*Korrektur der Messwerte, wenn sich ein Sensor im Nahbereich (< SE_MIN_DISTANCE) befindet*/ static unsigned char rm_state[SE_COUNT]; static short rm_min[SE_COUNT]; if (state != IDLE && state != INIT && state != DELAY) { if (rm_state[index] == 0 && srf[index].get_mean() < SE_MIN_DISTANCE) { rm_state[index] = 1; if (index == 0 || index == 2) rm_min[index] = srf[index+1].get_mean() - SE_MIN_DIFF; else rm_min[index] = srf[index-1].get_mean() - SE_MIN_DIFF; srf[index].set_mean(SE_MIN); } else if (rm_state[index] == 1) { if ((index == 0 || index == 2) && srf[index+1].get_mean() < rm_min[index]) { rm_state[index] = 0; } else if ((index == 1 || index == 3) && srf[index-1].get_mean() < rm_min[index]) { rm_state[index] = 0; } else { srf[index].set_mean(SE_MIN); } } } if (rm_state[0] == 1 && rm_state[1] == 1) {rm_state[0] = 0; rm_state[1] = 0;} if (rm_state[2] == 1 && rm_state[3] == 1) {rm_state[2] = 0; rm_state[3] = 0;} if (state == HOLD_STILL || state == GET_ANCHOR || state == HTM_DELAY) { if (nvalue2[0] == 1 && nvalue2[1] == 1 && nvalue2[2] == 1 && nvalue2[3] == 1) { slam_insert_v((srf[0].get_cm_per_s() + srf[1].get_cm_per_s())/2,(srf[2].get_cm_per_s() + srf[3].get_cm_per_s())/2,current_heading,get_current_millis()); nvalue2[0] = 0; nvalue2[1] = 0; nvalue2[2] = 0; nvalue2[3] = 0; } } /*Speichert den gelesenen Messwert in der entsprechenden Log-Datei*/ #if LOG > 0 FILE *fd; if (scheduler[i].param == SE0_ADDRESS) fd = fd_112; else if (scheduler[i].param == SE1_ADDRESS) fd = fd_113; else if (scheduler[i].param == SE2_ADDRESS) fd = fd_114; else fd = fd_115; fprintf(fd,"%lu\t%u\t%u\n", srf[scheduler[i].param - SE0_ADDRESS].get_msec(), srf[scheduler[i].param - SE0_ADDRESS].get_data(), srf[scheduler[i].param - SE0_ADDRESS].get_mean()); #endif scheduler[i].task = NOTHING; break; } case SAVE_LOG: /*Sichert die bisher geloggten Daten*/ #if LOG > 0 fclose(fd_112); fclose(fd_113); fclose(fd_114); fclose(fd_115); fclose(fd_data); char tmp_dir[32]; strcpy(tmp_dir,log_dir); fd_112 = fopen(strcat(tmp_dir,"/112"), "a"); strcpy(tmp_dir,log_dir); fd_113 = fopen(strcat(tmp_dir,"/113"), "a"); strcpy(tmp_dir,log_dir); fd_114 = fopen(strcat(tmp_dir,"/114"), "a"); strcpy(tmp_dir,log_dir); fd_115 = fopen(strcat(tmp_dir,"/115"), "a"); strcpy(tmp_dir,log_dir); fd_data= fopen(strcat(tmp_dir,"/data"),"a"); strcpy(tmp_dir,log_dir); schedule_add(LOG_SPEED,SAVE_LOG,0); #endif scheduler[i].task = NOTHING; break; case CHECK_STILL: /*Prueft auf Stillstand des Copters*/ if (still && state == HOLD_STILL) { if (scheduler[i].param < CHECK_STILL_COUNT) { schedule_add(CHECK_STILL_SPEED,CHECK_STILL,scheduler[i].param+1); } else { state = MEASURE_ENVIRONMENT; still = 0; schedule_add(CHECK_STILL_SPEED,CHECK_STILL, 0); } } else { schedule_add(CHECK_STILL_SPEED,CHECK_STILL,0); } scheduler[i].task = NOTHING; break; case CHANGE_STATE: /*Ändert den aktuellen Status zum als Parameter übergebenen Status*/ state = (states)scheduler[i].param; if ((states)scheduler[i].param == HOLD_STILL) hs_state = 0; scheduler[i].task = NOTHING; break; case SHOW_ME: { /*Schreibt Messwerte und Neigungswinkel auf das Terminal*/ char st[16]; switch (state) { case INIT: sprintf(st, "INIT"); break; case MEASURE_ENVIRONMENT: sprintf(st, "ME"); break; case IDLE: sprintf(st, "IDLE"); break; case HOLD_STILL: sprintf(st, "HOLD_STILL"); break; case GET_ALIGNMENT: sprintf(st, "GET_ALIGNMENT"); break; case GET_ANCHOR: sprintf(st, "GET_ANCHOR"); break; case DELAY: sprintf(st, "DELAY"); break; default: sprintf(st, "???"); break; } if (roll > 9 || roll < 0) printf("roll: %d\tpitch: %d\tyaw: %d\theading: %d\tdhead: %d\tSE0: %d\tSE1: %d\tSE2: %d\tSE3:%d\tstate: %s\n", roll, pitch, yaw, current_heading, desired_heading, srf[0].get_mean(), srf[1].get_mean(), srf[2].get_mean(), srf[3].get_mean(),st); else printf("roll: %d\t\tpitch: %d\tyaw: %d\theading: %d\tdhead. %d\tSE0: %d\tSE1: %d\tSE2: %d\tSE3:%d\tstate: %s\n", roll, pitch, yaw, current_heading, desired_heading, srf[0].get_mean(), srf[1].get_mean(), srf[2].get_mean(), srf[3].get_mean(),st); schedule_add(100,SHOW_ME,0); scheduler[i].task = NOTHING; /*Mavlinkkommunikation mit QGroundcontrol (basierend auf Based on http://qgroundcontrol.org/dev/mavlink_linux_integration_tutorial)*/ mavlink_message_t msg; uint16_t len; uint8_t buf[MAVLINK_MAX_PACKET_LEN]; static int i; i++; if (i == 10) { i = 0; mavlink_msg_heartbeat_pack(0, 0, &msg, 0, 0, 0, 0, 0); len = mavlink_msg_to_send_buffer(buf, &msg); sendto(s, buf, len, 0, (struct sockaddr*)&gcAddr, sizeof(struct sockaddr_in)); } mavlink_msg_debug_vect_pack(0,0,&msg,"DEBUG",0,roll,srf[0].get_mean(),srf[0].get_data()); len = mavlink_msg_to_send_buffer(buf, &msg); sendto(s, buf, len, 0, (struct sockaddr*)&gcAddr, sizeof(struct sockaddr_in)); break; } default: if(WARNINGS){perror("LOOP: Unbekannte Aufgabe im Scheduler.");} break; } } /*Auswertung der Daten, die an der seriellen Schnittstelle anliegen.*/ struct timeval tv; tv.tv_sec = 0; tv.tv_usec = 0; /*Prueft, ob Daten an der seriellen Schnittstelle anliegen*/ FD_ZERO(&tty_fdset); FD_SET(tty_fd, &tty_fdset); if (select(tty_fd+1, &tty_fdset, NULL, NULL, &tv) == -1) {perror("LOOP: select() fehlgeschlagen"); exit(1);} /*Liest ein einzelnes Byte von der seriellen Schnittstelle und prueft, ob ein Mavlinkpaket vervollstaendigt wurde*/ if (FD_ISSET(tty_fd, &tty_fdset)) { static mavlink_message_t msg; static mavlink_status_t status; char c[1]; if (read(tty_fd,c,1) == -1) {if(WARNINGS){perror("LOOP: Fehler beim Lesen aus " TTY_DEVICE);}} #if DEBUG_LEVEL > 2 printf("%#x\n",c[0]); #endif if (mavlink_parse_char(MAVLINK_COMM_0,(uint8_t) *c, &msg, &status)) { #if DEBUG_LEVEL > 1 printf("%u Mavlinkpaket empfangen: %d\n", get_current_millis(), msg.msgid); #endif //printf("%lu Mavlinkpaket empfangen: %d\n", get_current_millis(), msg.msgid); switch (msg.msgid) { case MAVLINK_MSG_ID_HEARTBEAT: /*Beim Empfang des ersten Heartbeats wird zunaechst ein eventuell vorhandener Datenstream gekuendigt und ein neuer Datenstream abonnemiert*/ mavlink_heartbeat_t hb; mavlink_msg_heartbeat_decode(&msg,&hb); if (!first_heartbeat) { first_heartbeat = 1; request_data_stream(MAV_DATA_STREAM_ALL,0,0); } //if (state == IDLE) { if (state == IDLE && (hb.custom_mode == 13 /*EXT_CTRL*/ || desktop_build)) { std::cout << "State changed from IDLE to INIT.\n"; state = INIT; schedule_add(2000,CHANGE_STATE,(int)HOLD_STILL); request_data_stream(MAV_DATA_STREAM_EXTRA2/*VFR_HUD*/,DATA_STREAM_SPEED,1); init_state = 1; } break; case MAVLINK_MSG_ID_VFR_HUD: /*Speichert bei Empfang von HUD-Daten die aktuelle Ausrichtung des Copters*/ mavlink_vfr_hud_t hud_data; mavlink_msg_vfr_hud_decode(&msg, &hud_data); old_heading = current_heading; current_heading = hud_data.heading; new_heading = 1; break; case MAVLINK_MSG_ID_ATTITUDE: mavlink_attitude_t adata; mavlink_msg_attitude_decode(&msg,&adata); old_heading = current_heading; current_heading = ((short)lround(DEG(adata.yaw))+360)%360; current_pitch_rad = adata.pitch; current_roll_rad = adata.roll; new_heading = 1; break; case MAVLINK_MSG_ID_RAW_IMU: /*Speichert die Beschleunigungen auf der x- und y-Achse*/ mavlink_raw_imu_t raw_imu; mavlink_msg_raw_imu_decode(&msg,&raw_imu); xacc = raw_imu.xacc; yacc = raw_imu.yacc; //slam_insert_acc(xacc,yacc,current_heading,get_current_millis()); std::cout << "xacc: " << xacc << " yacc: " << yacc << "\n"; break; case MAVLINK_MSG_ID_RC_CHANNELS_RAW: /*Prüft, ob eine Umstellung auf externe Kontrolle erfolgt ist*/ /*if (state != IDLE) {break;} mavlink_rc_channels_raw_t rc; mavlink_msg_rc_channels_raw_decode(&msg,&rc); if (desktop_build) init_state = 1; rc.chan5_raw = MODE_SWITCH_RANGE_UP-1; if (init_state && rc.chan5_raw < MODE_SWITCH_RANGE_UP && rc.chan5_raw > MODE_SWITCH_RANGE_DOWN) { std::cout << "State changed from IDLE to INIT.\n"; schedule_add(0,CHANGE_STATE,(int)INIT); schedule_add(2000,CHANGE_STATE,(int)HOLD_STILL); request_data_stream(MAV_DATA_STREAM_RC_CHANNELS,0,0); request_data_stream(MAV_DATA_STREAM_EXTRA2,DATA_STREAM_SPEED,1); request_data_stream(MAV_DATA_STREAM_RAW_SENSORS,DATA_STREAM_SPEED,1); } if(rc.chan5_raw > MODE_SWITCH_RANGE_UP || rc.chan5_raw < MODE_SWITCH_RANGE_DOWN) { init_state = 1; }*/ break; default: break; } } } /*Berechnung der vorgeschlagenen Werte fuer roll, pitch und yaw an Hand der neuen Messwerte*/ if (state != INIT && !new_value && !new_heading) {/*Wenn keine neuen Daten vorhanden sind -> Abarbeitung überspringen*/return;} switch (state) { case IDLE: /*Wartet auf Aktivierung der externen Kontrolle*/ break; case INIT: std::cout << "INIT\n"; /*Initialisiert Abarbeitung, wenn externe Kontrolle zum ersten Mal aktiviert wurde*/ schedule_add(5000,CHECK_STILL,0); #if LOG > 0 schedule_add(LOG_SPEED,SAVE_LOG,0); //schedule_add(SLAM_SPEED,SAVE_SLAM,0); #endif for (int i = 0; i < SE_COUNT; i++) { if ((int)pow(2,i) & ENABLED_SENSORS) { schedule_add(0, MEASURE, SE0_ADDRESS + i); } } slam_init(current_heading,get_current_millis()); state = DELAY; break; case HOLD_STILL: if(1) { static short set_point[SE_COUNT]; if (hs_state == 0) { hs_state = 1; set_point[0] = (srf[0].get_mean() + srf[1].get_mean())/2; set_point[1] = set_point[0]; set_point[2] = (srf[2].get_mean() + srf[3].get_mean())/2; set_point[3] = set_point[2]; //for (int i = 0; i < SE_COUNT; i++) { // set_point[i] = srf[i].get_mean(); // std::cout << "Set_Point " << i << ":" << set_point[i] << "\n"; //} /*for (int i = 0; i < SE_COUNT; i++) { if (set_point[i] < 100 || set_point[i] > 0) { if (i == 0 || i == 2) { set_point[i+1] -= (100 - set_point[i]); } else { set_point[i-1] -= (100 - set_point[i]); } set_point[i] = 100; } }*/ for (int i = 0; i < SE_COUNT; i++) { std::cout << "Set_Point " << i << ":" << set_point[i] << "\n"; } } /*Veranlasst den Copter auf Grundlage der Änderungen der Messwerte still zu stehen*/ if (!new_value) break; if (nvalue[0] == 1 && nvalue[1] == 1) { if (srf[0].get_mean() == SE_MIN) roll = between<short>(pid_roll.get(0 - srf[1].get_mean() + set_point[1],srf[1].get_msec_diff()),-max_roll,max_roll); else if (srf[1].get_mean() == SE_MIN) roll = between<short>(pid_roll.get(srf[0].get_mean() - set_point[0],srf[0].get_msec_diff()),-max_roll,max_roll); else roll = between<short>(pid_roll.get(((srf[0].get_mean() - set_point[0]) - (srf[1].get_mean() - set_point[1]))/2,(srf[0].get_msec_diff() + srf[1].get_msec_diff())/2),-max_roll,max_roll); nvalue[0] = 0; nvalue[1] = 0; } if (nvalue[2] == 1 && nvalue[3] == 1) { if (srf[2].get_mean() == SE_MIN) pitch = between<short>(pid_pitch.get(0 - srf[3].get_mean() + set_point[3],srf[3].get_msec_diff()),-max_pitch,max_pitch); else if (srf[3].get_mean() == SE_MIN) pitch = between<short>(pid_pitch.get(srf[2].get_mean() - set_point[2],srf[2].get_msec_diff()),-max_pitch,max_pitch); else pitch = between<short>(pid_pitch.get(((srf[2].get_mean() - set_point[2]) - (srf[3].get_mean() - set_point[3]))/2,(srf[2].get_msec_diff() + srf[3].get_msec_diff())/2),-max_pitch,max_pitch); nvalue[2] = 0; nvalue[3] = 0; } yaw = 0; send_ext_ctrl(); /*Prüfung auf Stillstand*/ if (abs(roll) <= STILL_FAK_ROLL*max_roll/100 && abs(pitch) <= STILL_FAK_PITCH*max_pitch/100) { //still = 1; still = 0; /*FIXME*/ } else { still = 0; } } break; case MEASURE_ENVIRONMENT: /*Vermisst die Umgebung durch eine Drehung um 360/SE_COUNT Grad*/ if (breakpoint == 1) {state = HOLD_STILL; hs_state= 0; break;} if (abs(xacc) > HOLD_STILL_MAX_XACC || abs(yacc) > HOLD_STILL_MAX_YACC) {/*Wurde der Copter zu stark bewegt: Abbruch*/state = HOLD_STILL; hs_state = 0; std::cout << "State changed to HOLD_STILL\n"; break;} if (first_heading == -1) { /*Speichert die anfaengliche Ausrichtung und veranlasst den Copter sich zu drehen*/ std::cout << "State changed to MEASURE_ENVIRONMENT\n"; first_heading = current_heading; roll = 0; pitch = 0; rotation = 0; yaw = rotation_angle_sign*CONST_YAW; send_ext_ctrl(); } //if (new_value) slam_insert_measurement(srf[new_value-SE0_ADDRESS].get_mean(),(current_heading + (360/SE_COUNT)*(new_value-SE0_ADDRESS))%360); /*Berechnung der aktuellen Drehung*/ if (new_heading) { if (abs(current_heading - old_heading) > 180/SE_COUNT) rotation += current_heading - old_heading - sign(current_heading - old_heading)*360; else rotation += current_heading - old_heading; for(int i = 0; i < SE_COUNT; i++) { if(nvalue[i] == 1) { env[(current_heading + srf[i].get_alignment()*(360/SE_COUNT))%360] = srf[i].get_mean(); nvalue[i] = 0; } } //if (abs(rotation) >= 360/SE_COUNT) { if (abs(rotation) >= rotation_angle) { /*Copter hat sich ausreichend gedreht, Abbruch der Vermessung*/ //state = HEAD_TO_MIDDLE; state = GET_ALIGNMENT; roll = 0; pitch = 0; yaw = 0; send_ext_ctrl(); rotation_angle_sign = sign(rotation); } } break; case GET_ALIGNMENT: { /*Bestimme für jeden Sensor die Minimalentfernung der letzten Drehung*/ /*short min_v[SE_COUNT]; short min_h[SE_COUNT]; for (int i = 0; i < SE_COUNT; i++) { min_v[i] = 0; min_h[i] = 0; }*/ short min_v = 0; for (int i = 0; i < rotation_angle; i++) { if (min_v < env[(first_heading + rotation_angle_sign*i + 360 + srf[align_se].get_alignment()*(360/SE_COUNT))%360]) { min_v = env[(first_heading + rotation_angle_sign*i + 360 + srf[align_se].get_alignment()*(360/SE_COUNT))%360]; desired_heading = (first_heading + rotation_angle_sign*i + 360 + srf[align_se].get_alignment()*(360/SE_COUNT))%360; } /*for (int j = 0; j < SE_COUNT; j++) { if (min_v[j] < env[(first_heading + rotation_angle_sign*i + j*360/SE_COUNT + 360)%360]) { min_v[j] = env[(first_heading + rotation_angle_sign*i + j*360/SE_COUNT + 360)%360]; min_h[j] = (first_heading + rotation_angle_sign*i + 360)%360; } }*/ } //desired_heading = round((float)(min_h[0]+min_h[1]+min_h[2]+min_h[3])/4); /*Bestimmung der künftigen Drehrichtung*/ for (int k = 0; k < rotation_angle/2; k++) { /*Wenn Minimum in der ersten Hälfte der Drehung gefunden wurde, Wechsel der Drehrichtung bzw. Abbruch*/ if (desired_heading == (first_heading+rotation_angle_sign*k)%360) { rotation_angle_sign *= -1; if (init_state == 2) init_state = 3; break; } } /*Cleanup*/ first_heading = -1; yaw = 0; pid_roll.reset(); pid_pitch.reset(); for(int i = 0; i < 360; i++) env[i] = 0; if (init_state == 1) init_state = 2; if (init_state == 3) { state = GET_ANCHOR; pid_yaw.reset(); pid_yaw.set(HTM_YAW_KP,HTM_YAW_TN,HTM_YAW_TV); pid_yaw.set_target(0); roll = 0; pitch = 0; yaw = 0; std::cout << "State changed to GET_ANCHOR\n"; tv_old_heading = get_current_millis(); } else { state = HOLD_STILL; hs_state = 0; } break; } case GET_ANCHOR: /*Copter versucht sich stabil auf ANCHOR_DISTANCE und desired_heading zu stellen*/ if (new_heading) { short heading_diff; /*Berechnung der Differenz zwischen aktueller Ausrichtung und gewünschter Ausrichtung*/ if (abs(desired_heading - current_heading) > 180) { heading_diff = desired_heading - current_heading - sign(desired_heading - current_heading)*360; } else { heading_diff = desired_heading - current_heading; } yaw = pid_yaw.get(heading_diff,get_current_millis() - tv_old_heading); tv_old_heading = get_current_millis(); } if (new_value) { roll = 0; pitch = 0; if (nvalue[align_se] == 1) { if (align_se == 0) roll = between<short>(pid_roll_anc.get(srf[align_se].get_mean(), srf[align_se].get_msec_diff()), -max_roll, max_roll); else if (align_se == 1) roll = between<short>(pid_roll_anc.get((-1)*srf[align_se].get_mean(), srf[align_se].get_msec_diff()), -max_roll, max_roll); else if (align_se == 2) pitch = between<short>(pid_pitch_anc.get(srf[align_se].get_mean(), srf[align_se].get_msec_diff()), -max_pitch, max_pitch); else if (align_se == 3) pitch = between<short>(pid_pitch_anc.get((-1)*srf[align_se].get_mean(), srf[align_se].get_msec_diff()), -max_pitch, max_pitch); nvalue[align_se] = 0; } if (srf[0].get_mean() < 30) roll = 1000; else if (srf[1].get_mean() < 30) roll = -1000; else if (srf[2].get_mean() < 30) pitch = 1000; else if (srf[3].get_mean() < 30) pitch = -1000; if (abs(srf[0].get_mean() - ANCHOR_DISTANCE) < 5) { for (int i = 0; i < SE_COUNT; i++) nvalue[i] = 0; /*state = MOVE_BACKWARD*/ /*TODO Neuausrichtung des Copters?*/ } } if (breakpoint == 3) {roll = 0; pitch = 0; send_ext_ctrl();} else if (breakpoint != 2) {send_ext_ctrl();} break; case DELAY: break; case HTM_DELAY: break; default: break; } #if LOG > 0 fprintf(fd_data,"%lu\t%d\t%d\t%d\t%d\t%d\t%f\t%f\n", (unsigned long)get_current_millis(), roll, pitch, yaw, current_heading, state, current_roll_rad, current_pitch_rad); #endif }
/*Initialisierung*/ void setup() { /*Oeffnen der seriellen Schnitstelle TTY_DEVICE*/ tty_fd = open(TTY_DEVICE, O_RDWR); if (tty_fd == -1) {perror("SETUP: " TTY_DEVICE " kann nicht geoeffnet werden."); exit(1);} /*Konfiguration der seriellen Schnittstelle*/ if (tcgetattr(tty_fd, &attr) != 0) {perror("SETUP: tcgetattr() fehlgeschlagen."); exit(1);} /*input modes*/ attr.c_iflag = 0; /*output modes*/ attr.c_oflag = OPOST | ONLCR; /*control modes*/ attr.c_cflag = TTY_DEVICE_SPEED | CS8 | CRTSCTS | CLOCAL | CREAD; /*local modes*/ attr.c_lflag = 0; if (tcsetattr(tty_fd, TCSAFLUSH, &attr) != 0){ perror("SETUP: tcsetattr() fehlgeschlagen"); exit(1);} first_heartbeat = 0; srf_fd = open(SRF_DEVICE, O_RDWR); if (srf_fd == -1) {perror("SETUP: " SRF_DEVICE "kann nicht geoeffnet werden."); exit(1);} /*Einstellen der anfaenglichen Messgeschwindigkeiten, Erzeugen der Sensoren*/ for (int i = 0; i < SE_COUNT; i++) { srf_speed[i] = SRF_SPEED; unsigned char align = i; /*Korrektur zur Winkelberechnung*/ if (i == 1) align = 2; if (i == 2) align = 1; srf.push_back(SRF((SE0_ADDRESS + i),srf_fd,align)); nvalue[i] = 0; nvalue2[i] = 0; } /*signal handler für SIGINT (Strg+C) registrieren*/ signal(SIGINT, signal_callback_handler); /*Initialisierung des Schedulers*/ gettimeofday(&tv_start,NULL); for (int i = 0; i < MAX_TASKS; i++) scheduler[i].task = NOTHING; /*Erzeugen des Log-Verzeichnis und Log-Dateien*/ #if LOG > 0 /*time_t current_timestamp = time(0); tm *current_time; current_time = localtime(¤t_timestamp); sprintf(log_dir,"log/%d_%d_%d %d:%d:%d",(current_time->tm_year+1900),(current_time->tm_mon+1),(current_time->tm_mday),(current_time->tm_hour),(current_time->tm_min),(current_time->tm_sec));*/ char tmp_dir[32]; strcpy(tmp_dir,log_dir); fd_112 = fopen(strcat(tmp_dir,"/112"), "a"); fprintf(fd_112,"#TS\tdata\tmean\n"); strcpy(tmp_dir,log_dir); fd_113 = fopen(strcat(tmp_dir,"/113"), "a"); fprintf(fd_113,"#TS\tdata\tmean\n"); strcpy(tmp_dir,log_dir); fd_114 = fopen(strcat(tmp_dir,"/114"), "a"); fprintf(fd_114,"#TS\tdata\tmean\n"); strcpy(tmp_dir,log_dir); fd_115 = fopen(strcat(tmp_dir,"/115"), "a"); fprintf(fd_115,"#TS\tdata\tmean\n"); strcpy(tmp_dir,log_dir); fd_data= fopen(strcat(tmp_dir,"/data"),"a"); fprintf(fd_data,"#TS\troll\tpitch\tyaw\theading\tstate\troll_rad\tpitch_rad\n"); strcpy(tmp_dir,log_dir); #endif current_pitch_rad = 0; current_roll_rad = 0; current_heading = 0; state = IDLE; roll = 0; pitch = 0; yaw = 0; still = 0; first_heading = -1; for(int i = 0; i < 360; i++) env[i] = 0; xacc = 0; yacc = 0; init_state = 0; rotation_angle = 6; yaw_sign = 1; rotation_angle_sign = 1; hs_state = 0; align_se = 3; char target_ip[15]; if (desktop_build) strcpy(target_ip, "192.168.0.180"); else strcpy(target_ip, "192.168.2.170"); memset(&gcAddr, 0, sizeof(gcAddr)); gcAddr.sin_family = AF_INET; gcAddr.sin_addr.s_addr = inet_addr(target_ip); gcAddr.sin_port = htons(14550); /*Einkommentieren, damit regelmäßig Daten auf das Terminal geschrieben werden*/ schedule_add(0,SHOW_ME,0); }