void MultiFramedRTPSink::buildAndSendPacket(Boolean isFirstPacket) {
fIsFirstPacket = isFirstPacket;
// Set up the RTP header:
unsigned rtpHdr = 0x80000000; // RTP version 2; marker ('M') bit not set (by default; it can be set later)
rtpHdr |= (fRTPPayloadType<<16);
rtpHdr |= fSeqNo; // sequence number
fOutBuf->enqueueWord(rtpHdr);
// Note where the RTP timestamp will go.
// (We can't fill this in until we start packing payload frames.)
fTimestampPosition = fOutBuf->curPacketSize();
fOutBuf->skipBytes(4); // leave a hole for the timestamp
fOutBuf->enqueueWord(SSRC());
// Allow for a special, payload-format-specific header following the
// RTP header:
fSpecialHeaderPosition = fOutBuf->curPacketSize();
fSpecialHeaderSize = specialHeaderSize();
fOutBuf->skipBytes(fSpecialHeaderSize);
// Begin packing as many (complete) frames into the packet as we can:
fTotalFrameSpecificHeaderSizes = 0;
fNoFramesLeft = False;
fNumFramesUsedSoFar = 0;
packFrame();
}
void MultiFramedRTPSink
::afterGettingFrame1(unsigned frameSize, unsigned numTruncatedBytes,
struct timeval presentationTime,
unsigned durationInMicroseconds) {
if (fIsFirstPacket) {
// Record the fact that we're starting to play now:
gettimeofday(&fNextSendTime, NULL);
}
fMostRecentPresentationTime = presentationTime;
if (fInitialPresentationTime.tv_sec == 0 && fInitialPresentationTime.tv_usec == 0) {
fInitialPresentationTime = presentationTime;
}
if (numTruncatedBytes > 0) {
unsigned const bufferSize = fOutBuf->totalBytesAvailable();
envir() << "MultiFramedRTPSink::afterGettingFrame1(): The input frame data was too large for our buffer size ("
<< bufferSize << "). "
<< numTruncatedBytes << " bytes of trailing data was dropped! Correct this by increasing \"OutPacketBuffer::maxSize\" to at least "
<< OutPacketBuffer::maxSize + numTruncatedBytes << ", *before* creating this 'RTPSink'. (Current value is "
<< OutPacketBuffer::maxSize << ".)\n";
}
unsigned curFragmentationOffset = fCurFragmentationOffset;
unsigned numFrameBytesToUse = frameSize;
unsigned overflowBytes = 0;
// If we have already packed one or more frames into this packet,
// check whether this new frame is eligible to be packed after them.
// (This is independent of whether the packet has enough room for this
// new frame; that check comes later.)
if (fNumFramesUsedSoFar > 0) {
if ((fPreviousFrameEndedFragmentation
&& !allowOtherFramesAfterLastFragment())
|| !frameCanAppearAfterPacketStart(fOutBuf->curPtr(), frameSize)) {
// Save away this frame for next time:
numFrameBytesToUse = 0;
fOutBuf->setOverflowData(fOutBuf->curPacketSize(), frameSize,
presentationTime, durationInMicroseconds);
}
}
fPreviousFrameEndedFragmentation = False;
if (numFrameBytesToUse > 0) {
// Check whether this frame overflows the packet
if (fOutBuf->wouldOverflow(frameSize)) {
// Don't use this frame now; instead, save it as overflow data, and
// send it in the next packet instead. However, if the frame is too
// big to fit in a packet by itself, then we need to fragment it (and
// use some of it in this packet, if the payload format permits this.)
if (isTooBigForAPacket(frameSize)
&& (fNumFramesUsedSoFar == 0 || allowFragmentationAfterStart())) {
// We need to fragment this frame, and use some of it now:
overflowBytes = computeOverflowForNewFrame(frameSize);
numFrameBytesToUse -= overflowBytes;
fCurFragmentationOffset += numFrameBytesToUse;
} else {
// We don't use any of this frame now:
overflowBytes = frameSize;
numFrameBytesToUse = 0;
}
fOutBuf->setOverflowData(fOutBuf->curPacketSize() + numFrameBytesToUse,
overflowBytes, presentationTime, durationInMicroseconds);
} else if (fCurFragmentationOffset > 0) {
// This is the last fragment of a frame that was fragmented over
// more than one packet. Do any special handling for this case:
fCurFragmentationOffset = 0;
fPreviousFrameEndedFragmentation = True;
}
}
if (numFrameBytesToUse == 0 && frameSize > 0) {
// Send our packet now, because we have filled it up:
sendPacketIfNecessary();
} else {
// Use this frame in our outgoing packet:
unsigned char* frameStart = fOutBuf->curPtr();
fOutBuf->increment(numFrameBytesToUse);
// do this now, in case "doSpecialFrameHandling()" calls "setFramePadding()" to append padding bytes
// Here's where any payload format specific processing gets done:
//qDebug()<<"void MultiFramedRTPSink ::afterGettingFrame 312 ";
doSpecialFrameHandling(curFragmentationOffset, frameStart,
numFrameBytesToUse, presentationTime,
overflowBytes);
++fNumFramesUsedSoFar;
// Update the time at which the next packet should be sent, based
// on the duration of the frame that we just packed into it.
// However, if this frame has overflow data remaining, then don't
// count its duration yet.
if (overflowBytes == 0) {
fNextSendTime.tv_usec += durationInMicroseconds;
fNextSendTime.tv_sec += fNextSendTime.tv_usec/1000000;
fNextSendTime.tv_usec %= 1000000;
}
// Send our packet now if (i) it's already at our preferred size, or
// (ii) (heuristic) another frame of the same size as the one we just
// read would overflow the packet, or
// (iii) it contains the last fragment of a fragmented frame, and we
// don't allow anything else to follow this or
// (iv) one frame per packet is allowed:
if (fOutBuf->isPreferredSize()
|| fOutBuf->wouldOverflow(numFrameBytesToUse)
|| (fPreviousFrameEndedFragmentation &&
!allowOtherFramesAfterLastFragment())
|| !frameCanAppearAfterPacketStart(fOutBuf->curPtr() - frameSize,
frameSize) ) {
// The packet is ready to be sent now
sendPacketIfNecessary();
} else {
// There's room for more frames; try getting another:
packFrame();
}
}
}
void main(void) {
//char c;
signed char length;
unsigned char msgtype;
unsigned char last_reg_recvd;
uart_comm uc;
i2c_comm ic;
unsigned char msgbuffer[MSGLEN + 1];
// unsigned char to_send_buffer[MAX_I2C_SENSOR_DATA_LEN + HEADER_MEMBERS];
// uint8 to_send_len;
// int data_points_count = 0;
//unsigned char i;
//uart_thread_struct uthread_data; // info for uart_lthread
//timer1_thread_struct t1thread_data; // info for timer1_lthread
//timer0_thread_struct t0thread_data; // info for timer0_lthread
#ifdef __USE18F2680
OSCCON = 0xFC; // see datasheet
// We have enough room below the Max Freq to enable the PLL for this chip
OSCTUNEbits.PLLEN = 1; // 4x the clock speed in the previous line
#else
#ifdef __USE18F45J10
OSCCON = 0x82; // see datasheeet
OSCTUNEbits.PLLEN = 0; // Makes the clock exceed the PIC's rated speed if the PLL is on
#else
#ifdef __USE18F26J50
OSCCON = 0xE0; // see datasheeet
OSCTUNEbits.PLLEN = 1;
#else
#ifdef __USE18F46J50
OSCCON = 0xE0; //see datasheet
OSCTUNEbits.PLLEN = 1;
#else
Something is messed up.
The PIC selected is not supported or the preprocessor directives are wrong.
#endif
#endif
#endif
#endif
// initialize my uart recv handling code
init_uart_recv(&uc);
// initialize the i2c code
init_i2c(&ic);
// init the timer1 lthread
// init_timer1_lthread(&t1thread_data);
// initialize message queues before enabling any interrupts
init_queues();
#ifndef __USE18F26J50
// set direction for PORTB to output
#ifndef MOTOR_PIC
TRISB = 0x0;
LATB = 0x0;
#else
TRISBbits.RB0 = 0;
TRISBbits.RB1 = 0;
TRISBbits.RB2 = 0;
TRISBbits.RB3 = 0;
#endif
#endif
// how to set up PORTA for input (for the V4 board with the PIC2680)
/*
PORTA = 0x0; // clear the port
LATA = 0x0; // clear the output latch
ADCON1 = 0x0F; // turn off the A2D function on these pins
// Only for 40-pin version of this chip CMCON = 0x07; // turn the comparator off
TRISA = 0x0F; // set RA3-RA0 to inputs
*/
// initialize Timers
#ifndef MASTER_PIC
#ifdef SENSOR_PIC
OpenTimer0(TIMER_INT_ON & T0_16BIT & T0_SOURCE_INT & T0_PS_1_16);
INTCONbits.T0IE = 1;
#elif !defined(MOTOR_PIC)
OpenTimer0(TIMER_INT_ON & T0_16BIT & T0_SOURCE_INT & T0_PS_1_4);
#else
OpenTimer0(TIMER_INT_ON & T0_8BIT & T0_PS_1_1 & T0_SOURCE_EXT);
#endif
#else
OpenTimer0(TIMER_INT_ON & T0_16BIT & T0_SOURCE_INT & T0_PS_1_16);
#endif
#ifdef __USE18F26J50
// MTJ added second argument for OpenTimer1()
OpenTimer1(TIMER_INT_ON & T1_SOURCE_FOSC_4 & T1_PS_1_8 & T1_16BIT_RW & T1_OSC1EN_OFF & T1_SYNC_EXT_OFF, 0x0);
#else
#ifdef __USE18F46J50
//OpenTimer1(TIMER_INT_ON & T1_SOURCE_FOSC_4 & T1_PS_1_8 & T1_16BIT_RW & T1_OSC1EN_OFF & T1_SYNC_EXT_OFF, 0x0);
#else
#ifndef MOTOR_PIC
// OpenTimer1(TIMER_INT_ON & T1_PS_1_8 & T1_16BIT_RW & T1_SOURCE_INT & T1_OSC1EN_OFF & T1_SYNC_EXT_OFF);
#else
OpenTimer1(TIMER_INT_ON & T1_PS_1_1 & T1_16BIT_RW & T1_SOURCE_EXT & T1_OSC1EN_OFF & T1_SYNC_EXT_OFF);
WRITETIMER1(0xFFE8); // leave in here, encoder's for motor 2 doesn't work without it
#endif
#endif
#endif
// Decide on the priority of the enabled peripheral interrupts
// 0 is low, 1 is high
//Timer0 interrupt
INTCON2bits.TMR0IP = 1;
// Timer1 interrupt
#ifdef SENSOR_PIC
// IPR1bits.TMR1IP = 0;
#else
IPR1bits.TMR1IP = 1;
#endif
// USART RX interrupt
IPR1bits.RCIP = 0;
IPR1bits.TXIP = 0;
// I2C interrupt
IPR1bits.SSPIP = 1;
//set i2c int high
PIE1bits.SSPIE = 1;
#ifdef SENSOR_PIC
//resetAccumulators();
init_adc();
initUS();
// must specifically enable the I2C interrupts
IPR1bits.ADIP = 0;
// configure the hardware i2c device as a slave (0x9E -> 0x4F) or (0x9A -> 0x4D)
i2c_configure_slave(SENSOR_ADDR << 1); //address 0x10
#elif defined MOTOR_PIC
i2c_configure_slave(MOTOR_ADDR << 1); //address 0x20
#elif defined PICMAN
i2c_configure_slave(PICMAN_ADDR << 1); //address 0x10,different bus from sensor
#elif defined I2C_MASTER
//sending clock frequency
i2c_configure_master(); //12MHz clock set hardcoded
#endif
#ifdef MASTER_PIC
///////Color Sensor Interrupt//////////
TRISBbits.TRISB0 = 1;
ANCON1bits.PCFG12 = 1; //not sure which is port b
INTCONbits.INT0IE = 1;
INTCON2bits.INTEDG0 = 0;
INTCONbits.INT0IF = 0;
initializeColorSensor();
///////////////////////////////////////
#endif
// configure the hardware USART device
#ifdef __USE18F26J50
Open1USART(USART_TX_INT_OFF & USART_RX_INT_ON & USART_ASYNCH_MODE & USART_EIGHT_BIT &
USART_CONT_RX & USART_BRGH_LOW, 0x19);
#else
#ifdef __USE18F46J50
#ifndef MOTOR_PIC
Open1USART(USART_TX_INT_OFF & USART_RX_INT_ON & USART_ASYNCH_MODE & USART_EIGHT_BIT &
USART_CONT_RX & USART_BRGH_LOW, 38);
#else
Open1USART(USART_TX_INT_OFF & USART_RX_INT_ON & USART_ASYNCH_MODE & USART_EIGHT_BIT &
USART_CONT_RX & USART_BRGH_LOW, 0x19);
#endif
#else
OpenUSART(USART_TX_INT_OFF & USART_RX_INT_ON & USART_ASYNCH_MODE & USART_EIGHT_BIT &
USART_CONT_RX & USART_BRGH_HIGH, 38);
// BAUDCONbits.BRG16 = 1;
// TXSTAbits.TXEN = 1;
// RCSTAbits.SPEN = 1;
// RCSTAbits.CREN = 1;
#endif
#endif
// Peripheral interrupts can have their priority set to high or low
// enable high-priority interrupts and low-priority interrupts
enable_interrupts();
LATBbits.LB7 = 0;
#ifndef MASTER_PIC
LATBbits.LB0 = 0;
#endif
LATBbits.LB1 = 0;
LATBbits.LB2 = 0;
LATBbits.LB3 = 0;
WRITETIMER0(0x00FF);
// loop forever
// This loop is responsible for "handing off" messages to the subroutines
// that should get them. Although the subroutines are not threads, but
// they can be equated with the tasks in your task diagram if you
// structure them properly.
while (1) {
// Call a routine that blocks until either on the incoming
// messages queues has a message (this may put the processor into
// an idle mode)
block_on_To_msgqueues();
//We have a bunch of queues now - ToMainHigh, ToMainLow, FromMainHigh, FromMainLow,
//FromUARTInt, and FromI2CInt
//From queues are most important because they will be called repeatedly with busy info
//Int queues are second because we'll often get data from either UART or I2C
//ToMain are least
length = FromMainHigh_recvmsg(MSGLEN, &msgtype, (void *) msgbuffer);
if (length < 0) {
// no message, check the error code to see if it is concern
if (length != MSGQUEUE_EMPTY) {
// This case be handled by your code.
}
} else {
switch (msgtype) {
#ifdef I2C_MASTER
case MSGT_MASTER_RECV_BUSY:
{
//retry
//debugNum(4);
i2c_master_recv(msgbuffer[0]);
break;
};
case MSGT_MASTER_SEND_BUSY:
{
//retry
//debugNum(8);
i2c_master_send(msgbuffer[0], length - 1, msgbuffer + 1); // point to second position (actual msg start)
break;
};
case MSGT_MASTER_SEND_NO_RAW_BUSY:
{
i2c_master_send_no_raw(msgbuffer[0], length-1, msgbuffer + 1);
};
case MSGT_TURN_CHECK:
{
//check IR sensors
unsigned char frame[FRAME_MEMBERS] = {0};
packFrame(frame, sizeof frame);
//frame[1] is ir1 and frame[2] is ir2
frame[1] = 1;//just for now, provide these dummy values
frame[2] = 1;
if((frame[1] > frame[2]) && (frame[1] - frame[2]) > 10){
//readjust right
char out[HEADER_MEMBERS] = {0};
uint8 len = generateReadjustCW(out, sizeof out, I2C_COMM);
i2c_master_send(MOTOR_ADDR, len, out);
// no need to call waitForSensorFrame() again
}
else if((frame[2] > frame[1]) && (frame[2] - frame[1]) > 10){
//readjust left
char out[HEADER_MEMBERS] = {0};
uint8 len = generateReadjustCCW(out, sizeof out, I2C_COMM);
i2c_master_send(MOTOR_ADDR, len, out);
// no need to call waitForSensorFrame() again
}
else{
char command[HEADER_MEMBERS] = {0};
uint8 len = generateTurnCompleteReq(command, sizeof command, UART_COMM); //tell picman turn complete
uart_send_array(command, len);
turnCompleted();
}
};
#endif
case MSGT_UART_TX_BUSY:
{
// TODO: take out for now
uart_send_array(msgbuffer, length);
break;
};
default:
break;
}
}
length = FromUARTInt_recvmsg(MSGLEN, &msgtype, (void *) msgbuffer);
if (length < 0) {
// no message, check the error code to see if it is concern
if (length != MSGQUEUE_EMPTY) {
// This case be handled by your code.
}
} else {
switch (msgtype) {
case MSGT_OVERRUN:
break;
case MSGT_UART_DATA:
{
#ifdef PICMAN
setRoverDataLP(msgbuffer);
handleRoverDataLP();
#elif defined(MASTER_PIC) || defined(ROVER_EMU)
setBrainDataLP(msgbuffer); //pass data received and tell will pass over i2c
handleMessageLP(UART_COMM, I2C_COMM); //sends the response and then sets up the command handling
#endif
break;
};
case MSGT_UART_RECV_FAILED:
{
debugNum(2);
debugNum(4);
debugNum(2);
debugNum(4);
break;
};
case MSGT_UART_TX_BUSY:
{
// TODO: take out for now
uart_send_array(msgbuffer, length);
break;
};
default:
break;
}
}
length = FromI2CInt_recvmsg(MSGLEN, &msgtype, (void *) msgbuffer);
if (length < 0) {
// no message, check the error code to see if it is concern
if (length != MSGQUEUE_EMPTY) {
// This case be handled by your code.
}
} else {
switch (msgtype) {
case MSGT_I2C_DATA:
{
#if defined(MASTER_PIC) || defined(ARM_EMU)
//handle whatever data will come through via i2c
//msgbuffer can hold real data - error codes will be returned through the error cases
setRoverDataLP(msgbuffer);
handleRoverDataLP();
#else
setBrainDataLP(msgbuffer);
#endif
break;
};
case MSGT_I2C_RQST:
{
#if defined(MOTOR_PIC) || defined(SENSOR_PIC)
handleMessageLP(I2C_COMM, I2C_COMM);
#elif defined(PICMAN)
handleMessageLP(I2C_COMM, UART_COMM);
#endif
break;
};
case MSGT_I2C_DBG:
{
// Here is where you could handle debugging, if you wanted
// keep track of the first byte received for later use (if desired)
last_reg_recvd = msgbuffer[0];
break;
};
#ifdef MASTER_PIC
case MSGT_I2C_MASTER_RECV_FAILED:
{
uart_send_array(msgbuffer, length);
break;
};
case MSGT_I2C_MASTER_SEND_FAILED:
{
uart_send_array(msgbuffer, length);
break;
};
case MSGT_MASTER_RECV_BUSY:
{
//retry
// debugNum(4);
i2c_master_recv(msgbuffer[0]);
break;
};
case MSGT_MASTER_SEND_BUSY:
{
//retry
// debugNum(8);
i2c_master_send(msgbuffer[0], length - 1, msgbuffer + 1); // point to second position (actual msg start)
break;
};
case MSGT_COLOR_SENSOR_INIT:
{
initializeColorSensorStage();
};
#endif
default:
break;
}
}
length = ToMainHigh_recvmsg(MSGLEN, &msgtype, (void *) msgbuffer);
if (length < 0) {
// no message, check the error code to see if it is concern
if (length != MSGQUEUE_EMPTY) {
// This case be handled by your code.
}
} else {
switch (msgtype) {
case MSGT_TIMER0:
{
//timer0_lthread(&t0thread_data, msgtype, length, msgbuffer);
break;
};
// #ifdef I2C_MASTER
// case MSGT_MASTER_RECV_BUSY:
// {
// //retry
// debugNum(4);
// i2c_master_recv(msgbuffer[0]);
// break;
// };
// case MSGT_MASTER_SEND_BUSY:
// {
// //retry
// debugNum(8);
// i2c_master_send(msgbuffer[0], length-1, msgbuffer + 1); // point to second position (actual msg start)
// break;
// };
// #endif
// case MSGT_I2C_DATA:
// {
// debugNum(4);
//#if defined(MASTER_PIC) || defined(ARM_EMU)
// //handle whatever data will come through via i2c
// //msgbuffer can hold real data - error codes will be returned through the error cases
// setRoverDataLP(msgbuffer);
// handleRoverDataLP();
//#else
// setBrainDataLP(msgbuffer);
//#endif
// debugNum(4);
// break;
// };
// case MSGT_I2C_RQST:
// {
//#if defined(MOTOR_PIC) || defined(SENSOR_PIC)
// handleMessageLP(I2C_COMM, I2C_COMM);
//#elif defined(PICMAN)
// handleMessageLP(I2C_COMM, UART_COMM);
//#endif
// break;
// };
// case MSGT_I2C_DBG:
// {
// // Here is where you could handle debugging, if you wanted
// // keep track of the first byte received for later use (if desired)
// last_reg_recvd = msgbuffer[0];
// break;
// };
//#ifdef MASTER_PIC
// case MSGT_I2C_MASTER_RECV_FAILED:
// {
// uart_send_array(msgbuffer, length);
// break;
// };
// case MSGT_I2C_MASTER_SEND_FAILED:
// {
// uart_send_array(msgbuffer, length);
// break;
// };
//#endif
case MSGT_AD:
{
#ifdef SENSOR_PIC
//addDataPoints(sensorADid, msgbuffer, length);
#endif
break;
};
default:
{
// Your code should handle this error
break;
};
};
}
// Check the low priority queue
length = ToMainLow_recvmsg(MSGLEN, &msgtype, (void *) msgbuffer);
if (length < 0) {
// no message, check the error code to see if it is concern
if (length != MSGQUEUE_EMPTY) {
// Your code should handle this situation
}
} else {
switch (msgtype) {
case MSGT_AD:
{
#ifdef SENSOR_PIC
//addDataPoints(sensorADid, msgbuffer, length);
#endif
break;
};
case MSGT_TIMER1:
{
//timer1_lthread(&t1thread_data, msgtype, length, msgbuffer);
break;
};
// case MSGT_OVERRUN:
// break;
// case MSGT_UART_DATA:
// {
//#ifdef PICMAN
// setRoverDataLP(msgbuffer);
// handleRoverDataLP();
//#elif defined(MASTER_PIC) || defined(ROVER_EMU)
// setBrainDataLP(msgbuffer);//pass data received and tell will pass over i2c
// handleMessageLP(UART_COMM, I2C_COMM); //sends the response and then sets up the command handling
//#endif
// break;
// };
// case MSGT_UART_RECV_FAILED:
// {
// debugNum(1);
// debugNum(2);
// debugNum(1);
// debugNum(2);
// break;
// };
default:
{
// Your code should handle this error
break;
};
};
}
}
//
}
