void CNewPredEncoder::makeNextBuf(
NEWPREDcnt* newpredCnt,
int vop_id,
int slice_no)
{
m_pShiftBufTmp = newpredCnt->NPRefBuf[slice_no];
shiftBuffer(vop_id, m_iNumBuffEnc);
}
bool MemBuffer::write(const Byte* array, size_t size)
{
bool bOk = false;
if(canWrite(g_sizeBYTE*size))
{
memcpy(getBuffer(), array, size);
bOk = shiftBuffer(g_sizeBYTE*size);
}
return bOk;
}
bool MemBuffer::writeByte(const Byte b)
{
bool bOk = false;
if(canWrite(g_sizeBYTE))
{
memcpy(getBuffer(), &b, g_sizeBYTE);
bOk = shiftBuffer(g_sizeBYTE);
}
return bOk;
}
Byte MemBuffer::readSymbol( )
{
Byte b = 0;
if(canRead(g_sizeBYTE))
{
b = *getBuffer();
shiftBuffer(g_sizeBYTE);
}
return b;
}
bool MemBuffer::write(uint32_t uint32)
{
bool bOk = false;
if(canWrite(g_sizeUINT32))
{
uint32 = htonl(uint32);
memcpy(getBuffer(), &uint32, g_sizeUINT32);
bOk = shiftBuffer(g_sizeUINT32);
}
return bOk;
}
char InputStream::read(void) {
const size_t limit = calculateReadLimit(1);
if (limit > 0) {
return shiftBuffer();
}
// All empty, nothing to return. Java would return NULL here,
// but C++ cannot. Possibly will throw an exception here.
return '\0';
}
bool MemBuffer::read(Byte* array, const size_t size)
{
bool bOk = false;
if(canRead(g_sizeBYTE*size))
{
memcpy(getBuffer(), array, g_sizeBYTE*size);
shiftBuffer(g_sizeBYTE*size);
bOk = true;
}
return bOk;
}
string InputStream::read(const int numBytes) {
const size_t limit = calculateReadLimit(numBytes);
stringstream str;
for (size_t i = 0; i < limit; ++i) {
str << shiftBuffer();
}
return str.str();
}
int CNewPred::make_next_decbuf(
NEWPREDcnt* newpredCnt,
int vop_id,
int slice_no)
{
int noStore_vop_id = 0;
m_pShiftBufTmp = newpredCnt->NPRefBuf[slice_no];
shiftBuffer(vop_id, m_iNumBuffDec);
return(noStore_vop_id);
}
bool MemBuffer::writeSymbol(const Symbol& s)
{
bool bOk = false;
size_t size = sizeof(Symbol);
if(canWrite(size))
{
Byte b = (Byte)s.toInt();
memcpy(getBuffer(), &b, size);
bOk = shiftBuffer(size);
}
return bOk;
}
uint32_t MemBuffer::read( )
{
uint32_t uint32 = 0;
if(canRead(g_sizeUINT32))
{
memcpy(getBuffer(), &uint32, sizeof(uint32_t));
shiftBuffer(g_sizeUINT32);
}
else
{
fprintf(stdout, "Error: cannot read buffer.\n");
}
return uint32;
}
Byte MemBuffer::readByte( )
{
Byte b = 0;
if(canRead(g_sizeBYTE))
{
b = *getBuffer();
shiftBuffer(g_sizeBYTE);
}
else
{
fprintf(stdout, "Error: cannot read buffer.\n");
}
return b;
}
void JpegReceiver::nextStep()
{
while (bufferLength < headerPrediction)
{
receivePacket();
}
std::string firstPart((char*)(buffer.get() + bufferStartPoint), headerPrediction);
std::string lengthBorderBig = "Content-Length: ";
std::string lengthBorderSmall = "Content-length: ";
int placeBig = firstPart.find(lengthBorderBig);
int placeSmall = firstPart.find(lengthBorderSmall);
int startLen = std::max(placeBig, placeSmall) + lengthBorderBig.length();
int endLen = startLen;
while (firstPart.at(endLen) >= '0' && firstPart.at(endLen) <= '9')
{
endLen++;
}
int jpegLength = atoi(firstPart.substr(startLen, endLen - startLen).c_str());
while (firstPart.at(endLen) == ' ' || firstPart.at(endLen) == '\n' || firstPart.at(endLen) == '\r')
{
++endLen;
}
if (placeSmall != -1) endLen += 28;
while (bufferLength - endLen < jpegLength)
{
receivePacket();
}
lockImage();
currentPic = JpegWrap(buffer.get() + (bufferStartPoint + endLen) * sizeof(unsigned char), jpegLength);
// createNextBmp();
releaseImage();
// createNextPicture();
//jpegWrap.pic = sh_ptr_uch(new unsigned char[jpegWrap.length]);
//memcpy(jpegWrap.pic.get(), buffer.get() + (bufferStartPoint + endLen) * sizeof(unsigned char), jpegWrap.length * sizeof(unsigned char));
/* for (int i = 0; i < jpegLength; ++i)
{
data.get()[i] = buffer.get()[bufferStartPoint + endLen + i];
}*/
shiftBuffer(bufferStartPoint + endLen + jpegLength);
}
__interrupt void USCIA0RX_ISR(void)
{
uint8 i;
uint8 checksum;
// Rotate received buffer
shiftBuffer(gripperMessageIn);
// Retrieve received data
gripperMessageIn[GRIPPER_MSG_LENGTH - 1] = UCA0RXBUF;
// Send a byte
while (!(IFG2 & UCA0TXIFG)); // USCI_A0 TX buffer ready?
if (sync)
UCA0TXBUF = gripperMessageOut[gripperMessageIdx];
else
UCA0TXBUF = 0xBE; // send 0 if not in sync
if (!sync) {
// when out of sync, find the first appearance of code to sync up
for (i = 0; i < GRIPPER_CODE_LENGTH; i++) {
if (gripperMessageIn[i] != gripperCode[i])
break;
}
if (i == GRIPPER_CODE_LENGTH) {
gripperMessageIdx = 0;
sync = 1;
}
} else if (gripperMessageIdx == GRIPPER_MSG_LENGTH - 1) {
// Checkcode
for (i = 0; i < GRIPPER_CODE_LENGTH; i++) {
if (gripperMessageIn[i] != gripperCode[i]) //check for ROB
break;
}
// If code matches, checksum. Else out of sync
if (i == GRIPPER_CODE_LENGTH) {
// If checksum matches, carry on. Else out of sync.
checksum = SPIChecksum(gripperMessageIn);
if (checksum == gripperMessageIn[GRIPPER_MSG_LENGTH - 1]) {
// Success!
//P3OUT &= ~LED;
if (!gripperMessageInBufLock) {
memcpy(gripperMessageInBuf, gripperMessageIn, GRIPPER_MSG_LENGTH);
}
// Pack received code to next out message
messagePackOut();
} else {
// Fail checksum
sync = 0;
//P3OUT |= LED;
}
} else {
// Fail checkcode
sync = 0;
}
gripperMessageIdx = 0; // Reset index counter for sync or next message
} else {
// In sync and no special event, carry on
gripperMessageIdx++;
}
IFG2 &= ~UCA0RXIFG;
}
void rk_auto(
/* integers */
int fsal, int neq, int stage,
int isDll, int isForcing, int verbose,
int nknots, int interpolate, int densetype, int maxsteps, int nt,
/* int pointers */
int* _iknots, int* _it, int* _it_ext, int* _it_tot, int* _it_rej,
int* istate, int* ipar,
/* double */
double t, double tmax, double hmin, double hmax,
double alpha, double beta,
/* double pointers */
double* _dt, double* _errold,
/* arrays */
double* tt, double* y0, double* y1, double* y2, double* dy1, double* dy2,
double* f, double* y, double* Fj, double* tmp,
double* FF, double* rr, double* A, double* out,
double* bb1, double* bb2, double* cc, double* dd,
double* atol, double* rtol, double* yknots, double* yout,
/* SEXPs */
SEXP Func, SEXP Parms, SEXP Rho
)
{
int i = 0, j = 0, j1 = 0, k = 0, accept = FALSE, nreject = *_it_rej, one = 1;
int iknots = *_iknots, it = *_it, it_ext = *_it_ext, it_tot = *_it_tot;
double err, dtnew, t_ext;
double dt = *_dt, errold = *_errold;
/* todo: make this user adjustable */
static const double minscale = 0.2, maxscale = 10.0, safe = 0.9;
/*------------------------------------------------------------------------*/
/* Main Loop */
/*------------------------------------------------------------------------*/
do {
if (accept) timesteps[0] = timesteps[1];
timesteps[1] = dt;
/* save former results of last step if the method allows this
(first same as last) */
/* Karline: improve by saving "accepted" FF, use this when rejected */
if (fsal && accept){
j1 = 1;
for (i = 0; i < neq; i++) FF[i] = FF[i + neq * (stage - 1)];
} else {
j1 = 0;
}
/****** Prepare Coefficients from Butcher table ******/
for (j = j1; j < stage; j++) {
for(i = 0; i < neq; i++) Fj[i] = 0;
k = 0;
while(k < j) {
for(i = 0; i < neq; i++)
Fj[i] = Fj[i] + A[j + stage * k] * FF[i + neq * k] * dt;
k++;
}
for (int i = 0; i < neq; i++) {
tmp[i] = Fj[i] + y0[i];
}
/****** Compute Derivatives ******/
/* pass option to avoid unnecessary copying in derivs */
derivs(Func, t + dt * cc[j], tmp, Parms, Rho, FF, out, j, neq,
ipar, isDll, isForcing);
}
/*====================================================================*/
/* Estimation of new values */
/*====================================================================*/
/* use BLAS wrapper with reduced error checking */
blas_matprod1(FF, neq, stage, bb1, stage, one, dy1);
blas_matprod1(FF, neq, stage, bb2, stage, one, dy2);
it_tot++; /* count total number of time steps */
for (i = 0; i < neq; i++) {
y1[i] = y0[i] + dt * dy1[i];
y2[i] = y0[i] + dt * dy2[i];
}
/*====================================================================*/
/* stepsize adjustment */
/*====================================================================*/
err = maxerr(y0, y1, y2, atol, rtol, neq);
dtnew = dt;
if (err == 0) { /* use max scale if all tolerances are zero */
dtnew = fmin(dt * 10, hmax);
errold = fmax(err, 1e-4); /* 1e-4 taken from Press et al. */
accept = TRUE;
} else if (err < 1.0) {
/* increase step size only if last one was accepted */
if (accept)
dtnew = fmin(hmax, dt *
fmin(safe * pow(err, -alpha) * pow(errold, beta), maxscale));
errold = fmax(err, 1e-4); /* 1e-4 taken from Press et al. */
accept = TRUE;
} else if (err > 1.0) {
nreject++; /* count total number of rejected steps */
accept = FALSE;
dtnew = dt * fmax(safe * pow(err, -alpha), minscale);
}
if (dtnew < hmin) {
accept = TRUE;
if (verbose) Rprintf("warning, h < Hmin\n");
istate[0] = -2;
dtnew = hmin;
}
/*====================================================================*/
/* Interpolation and Data Storage */
/*====================================================================*/
if (accept) {
if (interpolate) {
/*--------------------------------------------------------------------*/
/* case A1) "dense output type 1": built-in polynomial interpolation */
/* available for certain rk formulae, e.g. for rk45dp7 */
/*--------------------------------------------------------------------*/
if (densetype == 1) {
denspar(FF, y0, y2, dt, dd, neq, stage, rr);
t_ext = tt[it_ext];
while (t_ext <= t + dt) {
densout(rr, t, t_ext, dt, tmp, neq);
/* store outputs */
if (it_ext < nt) {
yout[it_ext] = t_ext;
for (i = 0; i < neq; i++)
yout[it_ext + nt * (1 + i)] = tmp[i];
}
if(it_ext < nt-1) t_ext = tt[++it_ext]; else break;
}
/*--------------------------------------------------------------------*/
/* case A2) dense output type 2: the Cash-Karp method */
/*--------------------------------------------------------------------*/
} else if (densetype == 2) { /* dense output method 2 = Cash-Karp */
derivs(Func, t + dt, y2, Parms, Rho, dy2, out, 0, neq,
ipar, isDll, isForcing);
t_ext = tt[it_ext];
while (t_ext <= t + dt) {
densoutck(t, t_ext, dt, y0, FF, dy2, tmp, neq);
/* store outputs */
if (it_ext < nt) {
yout[it_ext] = t_ext;
for (i = 0; i < neq; i++)
yout[it_ext + nt * (1 + i)] = tmp[i];
}
if(it_ext < nt-1) t_ext = tt[++it_ext]; else break;
}
/* FSAL (first same as last) for Cash-Karp */
for (i = 0; i < neq; i++) FF[i + neq * (stage - 1)] = dy2[i] ;
/*--------------------------------------------------------------------*/
/* case B) Neville-Aitken-Interpolation for integrators */
/* without dense output */
/*--------------------------------------------------------------------*/
} else {
/* (1) collect number "nknots" of knots in advance */
yknots[iknots] = t + dt; /* time is first column */
for (i = 0; i < neq; i++) yknots[iknots + nknots * (1 + i)] = y2[i];
if (iknots < (nknots - 1)) {
iknots++;
} else {
/* (2) do polynomial interpolation */
t_ext = tt[it_ext];
while (t_ext <= t + dt) {
neville(yknots, &yknots[nknots], t_ext, tmp, nknots, neq);
/* (3) store outputs */
if (it_ext < nt) {
yout[it_ext] = t_ext;
for (i = 0; i < neq; i++)
yout[it_ext + nt * (1 + i)] = tmp[i];
}
if(it_ext < nt-1) t_ext = tt[++it_ext]; else break;
}
shiftBuffer(yknots, nknots, neq + 1);
}
}
} else {
/*--------------------------------------------------------------------*/
/* Case C) no interpolation at all (for step to step integration); */
/* results are stored after the call */
/*--------------------------------------------------------------------*/
}
/*--------------------------------------------------------------------*/
/* next time step */
/*--------------------------------------------------------------------*/
t = t + dt;
it++;
for (i=0; i < neq; i++) y0[i] = y2[i];
} /* else rejected time step */
dt = fmin(dtnew, tmax - t);
if (it_ext > nt) {
Rprintf("error in RK solver rk_auto.c: output buffer overflow\n");
break;
}
if (it_tot > maxsteps) {
istate[0] = -1;
warning("Number of time steps %i exceeded maxsteps at t = %g\n", it, t);
break;
}
/* tolerance to avoid rounding errors */
} while (t < (tmax - 100.0 * DBL_EPSILON * dt)); /* end of rk main loop */
/* return reference values */
*_iknots = iknots; *_it = it; *_it_ext = it_ext; *_it_rej = nreject;
*_it_tot = it_tot; *_dt = dtnew; *_errold = errold;
}
/*
* @brief Expansion0 MSP430 board ISR
*
* @returns void
*/
uint8 expand0BoardISR() {
uint32 data, i;
uint32 timerLoadPeriod;
uint8 checksum;
long val;
static uint8 gripperData = 0;
TimerIntClear(TIMER1_BASE, TIMER_TIMB_TIMEOUT);
// Multiplex outputs (gripper/radio), set delay
if (MSPSPIState == MSPSPI_STATE_XMIT_BYTE) {
radioIntDisable();
SPISelectDeviceISR(SPI_EXPAND0);
timerLoadPeriod = MSP430_SPI_DESELECT_PERIOD;
} else if (MSPSPIState == MSPSPI_STATE_DESELECT_SPI) {
SPIDeselectISR();
radioIntEnable();
timerLoadPeriod = (MSP430_SPI_BYTE_PERIOD);
}
MAP_TimerLoadSet(TIMER1_BASE, TIMER_B, timerLoadPeriod);
MAP_TimerEnable(TIMER1_BASE, TIMER_B);
// Transmit a byte or just switch state
if (MSPSPIState == MSPSPI_STATE_XMIT_BYTE) {
// Pack message
if (expand0MessageIdx == 0) {
// Grab next message from buffer
if (!expand0MessageOutBufLock) {
memcpy(expand0MessageOut, expand0MessageOutBuf, EXPAND0_MSG_LENGTH);
}
}
// Rotate received buffer
shiftBuffer(expand0MessageIn, EXPAND0_MSG_LENGTH);
// Put data into the transmit buffer
val = MAP_SSIDataPutNonBlocking(SSI0_BASE, (uint32)expand0MessageOut[expand0MessageIdx]);
// Retrieve the received byte
MAP_SSIDataGet(SSI0_BASE, &data);
// Put byte the the end of the received buffer
expand0MessageIn[EXPAND0_MSG_LENGTH - 1] = data;
//Integrity checking
if (expand0MessageIdx == 0) {
// Checkcode
for (i = 0; i < EXPAND0_CODE_LENGTH; i++) {
if (expand0MessageIn[i] != expand0Code[i])
break;
}
if (i == EXPAND0_CODE_LENGTH) {
// Checksum
checksum = messageChecksum(expand0MessageIn, EXPAND0_CODE_LENGTH, EXPAND0_MSG_LENGTH);
if (checksum == expand0MessageIn[EXPAND0_MSG_LENGTH - 1]) {
// Avoid loading from buffer if the buffer is updating
if (!expand0MessageInBufLock) {
memcpy(expand0MessageInBuf, expand0MessageIn, EXPAND0_MSG_LENGTH);
}
} else {
// Fail checksum
if (showError) {cprintf("Gsum ");}
}
} else {
// Fail checkcode
if (showError) {cprintf("Gcod ");}
}
// Toggle MSP boards, regardless of result
MSPSelect = MSP_BOTTOM_BOARD_SEL;
}
// Increment index, wrap back to 0 if it exceeds the range
expand0MessageIdx++;
if (expand0MessageIdx >= EXPAND0_MSG_LENGTH) {
expand0MessageIdx = 0;
}
// Toggle states
MSPSPIState = MSPSPI_STATE_DESELECT_SPI;
} else if (MSPSPIState == MSPSPI_STATE_DESELECT_SPI) {
// Toggle states
MSPSPIState = MSPSPI_STATE_XMIT_BYTE;
}
return 0;
}
uint8 bottomBoardISR() {
uint32 data, i;
uint32 timerLoadPeriod;
uint8 checksum;
long val;
TimerIntClear(TIMER1_BASE, TIMER_TIMB_TIMEOUT);
// Multiplex SPI (bottomboard/radio)
if (MSPSPIState == MSPSPI_STATE_XMIT_BYTE) {
radioIntDisable();
SPISelectDeviceISR(SPI_MSP430);
timerLoadPeriod = MSP430_SPI_DESELECT_PERIOD;
} else if (MSPSPIState == MSPSPI_STATE_DESELECT_SPI) {
SPIDeselectISR();
radioIntEnable();
if (bootloaderSet && MSP430MessageIdx == 0) {
// Deselect but then set the boot-loader to operate.
msp430BSLInit();
bootloaderSet = FALSE;
} else {
timerLoadPeriod = MSP430_SPI_BYTE_PERIOD;
}
}
// Set delay
MAP_TimerLoadSet(TIMER1_BASE, TIMER_B, timerLoadPeriod);
MAP_TimerEnable(TIMER1_BASE, TIMER_B);
// Transmit message to bottomboard
if (MSPSPIState == MSPSPI_STATE_XMIT_BYTE) {
// Pack new message at the beginning of each cycle
if (MSP430MessageIdx == 0) {
// Pack code
for (i = 0; i < MSP430_CODE_LENGTH; i++) {
MSP430MessageOut[i] = MSP430Code[i];
}
// Build and pack payload
switch (msp430SystemGetCommandMessage()) {
case MSP430_CMD_COMMAND_NORMAL:
blinkySystemBuildMessage(&MSP430MessageOut[MSP430_CMD_SYSTEM_LED_IDX]);
buttonsBuildMessage(&MSP430MessageOut[MSP430_CMD_BUTTONS_IDX]);
ledsBuildMessage(&MSP430MessageOut[MSP430_CMD_LED_IDX]);
break;
case MSP430_CMD_COMMAND_REPROGRAM:
bootloaderSet = TRUE;
break;
default:
// Do nothing for shutdown or reset
break;
}
msp430SystemCommandBuildMessage(&MSP430MessageOut[MSP430_CMD_COMMAND_IDX]);
MSP430MessageOut[MSP430_CMD_CHECKSUM_IDX] = messageChecksum(MSP430MessageOut, MSP430_CODE_LENGTH, MSP430_MSG_LENGTH);
}
// Shift receive buffer
shiftBuffer(MSP430MessageIn, MSP430_MSG_LENGTH);
// Put data into the transmit buffer
val = MAP_SSIDataPutNonBlocking(SSI0_BASE, (uint32)MSP430MessageOut[MSP430MessageIdx]);
if (val == 0) {
SPIBusError_SSIDataPutNonBlocking = 1;
}
// Retrieve the received byte
MAP_SSIDataGet(SSI0_BASE, &data);
// Put byte the the end of the receive buffer
MSP430MessageIn[MSP430_MSG_LENGTH - 1] = data;
// Check to see if we have a complete message, with correct code and checksum
if (MSP430MessageIdx == 0) {
// Checkcode
for (i = 0; i < MSP430_CODE_LENGTH; i++) {
if (MSP430MessageIn[i] != MSP430Code[i])
break;
}
if (i == MSP430_CODE_LENGTH) {
// Checksum
checksum = messageChecksum(MSP430MessageIn, MSP430_CODE_LENGTH, MSP430_MSG_LENGTH);
if (checksum == MSP430MessageIn[MSP430_MSG_LENGTH - 1]) {
// Process incoming message
bumpSensorsUpdate(MSP430MessageIn[MSP430_MSG_BUMPER_IDX]);
accelerometerUpdate(&MSP430MessageIn[MSP430_MSG_ACCEL_START_IDX]);
gyroUpdate(&MSP430MessageIn[MSP430_MSG_GYRO_START_IDX]);
systemBatteryVoltageUpdate(MSP430MessageIn[MSP430_MSG_VBAT_IDX]);
systemUSBVoltageUpdate(MSP430MessageIn[MSP430_MSG_VUSB_IDX]);
systemPowerButtonUpdate(MSP430MessageIn[MSP430_MSG_POWER_BUTTON_IDX]);
systemMSPVersionUpdate(MSP430MessageIn[MSP430_MSG_VERSION_IDX]);
reflectiveSensorsUpdate(&MSP430MessageIn[MSP430_MSG_REFLECT_START_IDX]);
if((MSP430MessageIn[MSP430_MSG_VERSION_IDX] != 0) && (!systemMSP430CommsValid)) {
systemMSP430CommsValid = TRUE;
}
} else {
checksumFailure++;
// Fail checksum
if (showError) {cprintf("Bsum ");}
}
} else {
// Fail checkcode
if (showError) {cprintf("Bcod ");}
}
// Toggle MSP boards, regardless of result
if (expand0En) {
MSPSelect = MSP_EXPAND0_BOARD_SEL;
}
}
// Increment index, wrap back to 0 if it exceeds the range
MSP430MessageIdx++;
if (MSP430MessageIdx >= MSP430_MSG_LENGTH) {
MSP430MessageIdx = 0;
}
// Toggle states
MSPSPIState = MSPSPI_STATE_DESELECT_SPI;
} else if (MSPSPIState == MSPSPI_STATE_DESELECT_SPI) {
// Toggle states
MSPSPIState = MSPSPI_STATE_XMIT_BYTE;
}
return 0;
}
