/*
* 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);
}
