void tack2Heading(int starboard){
motorRunning=0;
txs=0;
p4_1=0;//set move out to off
p4_0=0;//set move in to off
tdelay(50);
if(starboard==1){
motorRunning=1;
}
else{
motorRunning=-1;
}
PID.runTime=130;
txs=1;
setCompass(getCompassReading(1));
while(paused==0&°reeDifferance(1,heading.current,heading.desired)>35){
setCompass(getCompassReading(0));
check4ShaftNOut();
}
motorRunning=0;
txs=0;
p4_1=0;//set move out to off
p4_0=0;//set move in to off
tdelay(50);
}
static void pcm_format
(
void
)
{
int format;
// build the register value based on format
format = 0;
switch( SOUNDSCAPE_SampleRate )
{
case 11025:
format = 0x03;
break;
case 22050:
format = 0x07;
break;
case 44100:
format = 0x0b;
break;
default:
// Set it to 11025 hz
format = 0x03;
break;
}
// set other format bits and format globals
if ( SOUNDSCAPE_MixMode & SIXTEEN_BIT )
{
format |= 0x40;
}
if ( SOUNDSCAPE_MixMode & STEREO )
{
format |= 0x10;
}
// enable mode change, point to format reg
outp( SOUNDSCAPE_Config.WavePort + AD_REGADDR, 0x40 | AD_FORMAT );
// write the format
outp( SOUNDSCAPE_Config.WavePort + AD_REGDATA, format );
// delay for internal re-synch
tdelay();
// exit mode change state
outp( SOUNDSCAPE_Config.WavePort + AD_REGADDR, 0x00 );
// delay for autocalibration
tdelay();
}
static void a_txint(struct pi_local *lp)
{
int cmd;
unsigned long flags;
save_flags(flags);
cli();
cmd = CTL + lp->base;
switch (lp->tstate) {
case IDLE:
/* Transmitter idle. Find a frame for transmission */
if ((lp->sndbuf = skb_dequeue(&lp->sndq)) == NULL) {
rts(lp, OFF);
restore_flags(flags);
return;
}
/* If a buffer to send, we drop thru here */
case DEFER:
/* we may have deferred prev xmit attempt */
/* Check DCD - debounce it
* See Intel Microcommunications Handbook, p2-308
*/
wrtscc(lp->cardbase, cmd, R0, RES_EXT_INT);
wrtscc(lp->cardbase, cmd, R0, RES_EXT_INT);
if ((rdscc(lp->cardbase, cmd, R0) & DCD) != 0) {
lp->tstate = DEFER;
tdelay(lp, 100);
/* defer until DCD transition or timeout */
wrtscc(lp->cardbase, cmd, R15, CTSIE | DCDIE);
restore_flags(flags);
return;
}
if (random() > lp->persist) {
lp->tstate = DEFER;
tdelay(lp, lp->slotime);
restore_flags(flags);
return;
}
/* Assert RTS early minimize collision window */
wrtscc(lp->cardbase, cmd, R5, TxCRC_ENAB | RTS | Tx8);
rts(lp, ON); /* Transmitter on */
lp->tstate = ST_TXDELAY;
tdelay(lp, lp->txdelay);
restore_flags(flags);
return;
default:
break;
} /* end switch(lp->state) */
restore_flags(flags);
} /*a_txint */
/*! \brief Initializes attrubutes using t0_n, rot0_n, L0m_n, order_default_n, move_n and tStep inputs.
*
* Inputs are checked.
*
* #tdelay method is used to set #DelayStore attribute
*
* #position method is used to set #SCposStore attribute
*
* Attributes are set :
*
* \arg #arot =\f$ \frac{2 \cdot \pi}{3} \f$
* \arg #rot :\f$ rot[i] = i \cdot arot \cdot rot0 \textrm{ for i=1,2,3 } \f$
* \arg #crot : \f$ crot[i] = cos(rot_i) \textrm{ for i=1,2,3 } \f$
* \arg #srot : \f$ srot[i=0,1,2] = sin(rot_i) \textrm{ for i=1,2,3 } \f$
* \arg #SCposStore :\f$ SCposStore[i] = position(i,0) \textrm{ for i=1,2,3,4 } \f$
* \arg #DelayStore :
* \f$ \textrm{ for i=1,2,3,4 }
* \left\{ \begin{array}{l}
* DelayStore[i-1] = tdelay(i,i,mod((i+1),3)+1,2,0) \\
* DelayStore[i+2] = tdelay(i,i,mod(i,3)+1,2,0)
* \end{array} \right. \f$
* \arg #tRangeStorePos = #tRangeStorePos_default
* \arg #tRangeStoreDelay = \f$ min(tStep,tRangeStoreDelay\_default) \f$
*/
void GeometryMLDC::init(double t0_n, double rot0_n, double L0m_n, int order_default_n, int move_n, double tStep)
{
//cout << "Create orbits : MLDC ..." << endl;
initGlobal(t0_n, rot0_n, L0m_n, order_default_n, move_n, tStep);
Approx = move_n;
arot=2./3.*M_PI;
rot.resize(3);
crot.resize(3);
srot.resize(3);
for(int nb=1; nb<=3; nb++){
rot[nb-1]=(nb-1)*arot+rot0;
crot[nb-1]=cos(rot[nb-1]);
srot[nb-1]=sin(rot[nb-1]);
}
e_mldc = 0.00964838;
L0s = L0m/c_SI;
sqrt_3 = sqrt(3.0);
i32 = 1.0/32.0;
r1532 = 15.0/32.0;
pi3o2 = 3.0*M_PI/2.0;
pi2o3 = 2.0*M_PI/3.0;
pi4o3 = 4.0*M_PI/3.0;
for(int i=1; i<4; i++){
SCposStore[i-1] = position(i,0.0);
}
for(int i=1; i<4; i++){
DelayStore[i-1] = tdelay(i,(i+1)%3+1,2,0.0);
//cout << " L" << i << (i+1)%3+1<< endl;
DelayStore[i+2] = tdelay(i,i%3+1,2,0.0);
//cout << " L" << i << i%3+1 << endl;
}
//cout << " --> OK" << endl;
}
int BusyFileOpen(char *name, int wait)
{
char *bsyname, *dir, *point;
char temp[PATHLEN];
int bfile;
unsigned bs;
/* Only use if FLAG_BSY is set, and FLAG_FRODO is *not* set. */
if ((config.flag & FLAG_BSY)==0)
return 0;
bsyname=bsy_extension(name);
/* Open the file with O_EXCL, such that the create fails if the file *
* already exists. */
while ((bfile=cshopen(bsyname, O_EXCL | O_CREAT | O_WRONLY | O_BINARY))==-1)
{
if (!wait)
return -1;
dir=sstrdup(bsyname);
point=strrchr(dir, PATH_DELIM);
if (point)
*point='\0';
(void)make_dir(dir);
free(dir);
(void)sprintf(temp," (%s busy; wait)", name);
(void)printf(temp);
while (fexist(bsyname))
{
if (khit() && kgetch()==K_ESC)
break;
tdelay(200);
}
for (bs=strlen(temp); bs--; )
(void)printf("\b \b");
}
if (bfile != -1)
(void)close(bfile);
free(bsyname);
return 0;
}
int HTS221::doOneShotMeasurement()
{
/* Start a humidity and temperature measurement */
if (I2CSensor_Write(HTS221_CTRL_REG2,1))
return ERROR_HTS221_MEASUREMENT_FAILED;
unsigned char STATUS_REG;
/* Check to see whenever a new humidity sample is available. */
do
{
if (I2CSensor_Read(HTS221_STATUS_REG, &STATUS_REG, 1))
return ERROR_HTS221_MEASUREMENT_FAILED;
} while (!(STATUS_REG & 2));
#if 0
/* Wait around 5ms for finishing the measurement and start reading the sensor output */
tdelay(5000000L);
#endif
/* Read humidity registers. MSB bit of HTS221_HUMIDITY_OUT_L address is set to 1 for
* enabling address auto-increment.
*/
if (I2CSensor_Read((HTS221_HUMIDITY_OUT_L | 0x80),HTS221HumidityOut,2))
return ERROR_HTS221_MEASUREMENT_FAILED;
/* Check to see whenever a new temperature sample is available. */
do
{
if (I2CSensor_Read(HTS221_STATUS_REG, &STATUS_REG, 1))
return ERROR_HTS221_MEASUREMENT_FAILED;
} while (!(STATUS_REG & 1));
/* Read temperature registers. MSB bit of HTS221_TEMP_OUT_L address is set to 1 for
* enabling address auto-increment.
*/
if (I2CSensor_Read((HTS221_TEMP_OUT_L | 0x80),HTS221TemperatureOut,2))
return ERROR_HTS221_MEASUREMENT_FAILED;
/* Calculate the relative humidity value based on the measurement data. */
calculateRealtiveHumidity();
/* Calculate the temperature value based on the measurement data. */
calculateTemperature();
return 0;
}
int LPS25H::doOneShotMeasurement()
{
unsigned char pBuffer[3];
unsigned char tBuffer[2];
int16_t temperature=0;
/* Start a pressure and temperature measurement by writing 0x01 in to a CTR_REG2*/
if (I2CSensor_Write(LPS25H_CTRL_REG2,0x01))
return ERROR_LPS25H_MEASUREMENT_FAILED;
/* Wait around 5ms for finishing the measurement and start reading the sensor output. */
tdelay(5000000L);
/* Read pressure registers. MSB bit of LPS25H_PRESS_POUT_XL address is set to 1 for
* enabling address auto-increment.
*/
if (I2CSensor_Read((LPS25H_PRESS_POUT_XL | 0x80),pBuffer,3))
return ERROR_LPS25H_MEASUREMENT_FAILED;
/* Read temperature registers. MSB bit of LPS25H_TEMP_OUT_L address is set to 1 for
* enabling address auto-increment.
*/
if (I2CSensor_Read((LPS25H_TEMP_OUT_L | 0x80),tBuffer,2))
return ERROR_LPS25H_MEASUREMENT_FAILED;
/*
* Calculate the pressure value based on the measurement data.
* The formula is taken from ST Application Note AN4450.
*/
pressureReading = (float)((((uint32_t)pBuffer[2]) << 16) | (((uint32_t)pBuffer[1]) << 8) | (uint32_t)pBuffer[0]) / (float)4096;
/* Calculate the temperature value based on the measurement data. */
temperature=tBuffer[1];
temperature<<=8;
temperature|=tBuffer[0];
/* Convert negative 2's complement values to native negative value */
if (temperature & 0x8000) temperature = -((~temperature)+1);
temperatureReading = 42.5 + float(temperature)/float(480.0);
return 0;
}
void SOUNDSCAPE_StopPlayback
(
void
)
{
// Don't allow anymore interrupts
SOUNDSCAPE_DisableInterrupt();
/* stop the AD-1848 */
ad_write( AD_CONFIG, 0x00 );
/* let it finish it's cycles */
tdelay();
// Disable the DMA channel
DMA_EndTransfer( SOUNDSCAPE_Config.DMAChan );
SOUNDSCAPE_SoundPlaying = FALSE;
SOUNDSCAPE_DMABuffer = NULL;
}
STATIC WORD diskchange(ddt * pddt)
{
COUNT result;
/* if it's a hard drive, media never changes */
if (hd(pddt->ddt_descflags))
return M_NOT_CHANGED;
if (play_dj(pddt) == M_CHANGED)
return M_CHANGED;
if (pddt->ddt_descflags & DF_CHANGELINE) /* if we can detect a change ... */
{
if ((result = fl_diskchanged(pddt->ddt_driveno)) == 1)
/* check if it has changed... */
return M_CHANGED;
else if (result == 0)
return M_NOT_CHANGED;
}
/* can not detect or error... */
return tdelay(pddt, 37ul) ? M_DONT_KNOW : M_NOT_CHANGED;
}
void delay(int u)
{
tdelay(((long long)PPC_TIMEBASE_FREQ) * u);
}
void mdelay(int u)
{
tdelay(((long long)PPC_TIMEBASE_FREQ) * u / 1000);
}
/* Pi SIO External/Status interrupts (for the B channel)
* This can be caused by a receiver abort, or a Tx UNDERRUN/EOM.
* Receiver automatically goes to Hunt on an abort.
*
* If the Tx Underrun interrupt hits, change state and
* issue a reset command for it, and return.
*/
static void b_exint(struct pi_local *lp)
{
unsigned long flags;
char st;
int cmd;
char c;
cmd = CTL + lp->base;
save_flags(flags);
cli(); /* disable interrupts */
st = rdscc(lp->cardbase, cmd, R0); /* Fetch status */
/* reset external status latch */
wrtscc(lp->cardbase, cmd, R0, RES_EXT_INT);
switch (lp->tstate) {
case ACTIVE: /* Unexpected underrun */
free_p(lp->sndbuf);
lp->sndbuf = NULL;
wrtscc(lp->cardbase, cmd, R0, SEND_ABORT);
lp->tstate = FLAGOUT;
lp->stats.tx_errors++;
lp->stats.tx_fifo_errors++;
tdelay(lp, lp->squeldelay);
restore_flags(flags);
return;
case UNDERRUN:
lp->tstate = CRCOUT;
restore_flags(flags);
return;
case FLAGOUT:
/* Find a frame for transmission */
if ((lp->sndbuf = skb_dequeue(&lp->sndq)) == NULL) {
/* Nothing to send - return to receive mode
* Tx OFF now - flag should have gone
*/
rts(lp, OFF);
lp->tstate = IDLE;
restore_flags(flags);
return;
}
lp->txptr = lp->sndbuf->data;
lp->txptr++; /* Ignore KISS control byte */
lp->txcnt = (int) lp->sndbuf->len - 1;
/* Get first char to send */
lp->txcnt--;
c = *lp->txptr++;
wrtscc(lp->cardbase, cmd, R0, RES_Tx_CRC); /* reset for next frame */
/* Send abort on underrun */
if (lp->speed) { /* If internally clocked */
wrtscc(lp->cardbase, cmd, R10, CRCPS | NRZI | ABUNDER);
} else {
wrtscc(lp->cardbase, cmd, R10, CRCPS | ABUNDER);
}
wrtscc(lp->cardbase, cmd, R8, c); /* First char out now */
wrtscc(lp->cardbase, cmd, R0, RES_EOM_L); /* Reset end of message latch */
#ifdef STUFF2
/* stuff an extra one if we can */
if (lp->txcnt) {
lp->txcnt--;
c = *lp->txptr++;
/* Wait for tx buffer empty */
while((rdscc(lp->cardbase, cmd, R0) & 0x04) == 0)
;
wrtscc(lp->cardbase, cmd, R8, c);
}
#endif
/* select transmit interrupts to enable */
wrtscc(lp->cardbase, cmd, R15, TxUIE); /* allow Underrun int only */
wrtscc(lp->cardbase, cmd, R0, RES_EXT_INT);
wrtscc(lp->cardbase, cmd, R1, TxINT_ENAB | EXT_INT_ENAB); /* Tx/Ext ints */
lp->tstate = ACTIVE; /* char going out now */
restore_flags(flags);
return;
case DEFER:
/* Check DCD - debounce it
* See Intel Microcommunications Handbook, p2-308
*/
wrtscc(lp->cardbase, cmd, R0, RES_EXT_INT);
wrtscc(lp->cardbase, cmd, R0, RES_EXT_INT);
if ((rdscc(lp->cardbase, cmd, R0) & DCD) != 0) {
lp->tstate = DEFER;
tdelay(lp, 100);
/* defer until DCD transition or timeout */
wrtscc(lp->cardbase, cmd, R15, CTSIE | DCDIE);
restore_flags(flags);
return;
}
if (random() > lp->persist) {
lp->tstate = DEFER;
tdelay(lp, lp->slotime);
restore_flags(flags);
return;
}
rts(lp, ON); /* Transmitter on */
lp->tstate = ST_TXDELAY;
tdelay(lp, lp->txdelay);
restore_flags(flags);
return;
case ST_TXDELAY:
/* Get first char to send */
lp->txcnt--;
c = *lp->txptr++;
wrtscc(lp->cardbase, cmd, R0, RES_Tx_CRC); /* reset for next frame */
/* Send abort on underrun */
if (lp->speed) { /* If internally clocked */
wrtscc(lp->cardbase, cmd, R10, CRCPS | NRZI | ABUNDER);
} else {
wrtscc(lp->cardbase, cmd, R10, CRCPS | ABUNDER);
}
wrtscc(lp->cardbase, cmd, R8, c); /* First char out now */
wrtscc(lp->cardbase, cmd, R0, RES_EOM_L); /* Reset end of message latch */
#ifdef STUFF2
/* stuff an extra one if we can */
if (lp->txcnt) {
lp->txcnt--;
c = *lp->txptr++;
/* Wait for tx buffer empty */
while((rdscc(lp->cardbase, cmd, R0) & 0x04) == 0)
;
wrtscc(lp->cardbase, cmd, R8, c);
}
#endif
/* select transmit interrupts to enable */
wrtscc(lp->cardbase, cmd, R15, TxUIE); /* allow Underrun int only */
wrtscc(lp->cardbase, cmd, R0, RES_EXT_INT);
/* Tx/Extern ints on */
wrtscc(lp->cardbase, cmd, R1, TxINT_ENAB | EXT_INT_ENAB);
lp->tstate = ACTIVE; /* char going out now */
restore_flags(flags);
return;
}
/* Receive Mode only
* This triggers when hunt mode is entered, & since an ABORT
* automatically enters hunt mode, we use that to clean up
* any waiting garbage
*/
if ((lp->rstate == ACTIVE) && (st & BRK_ABRT)) {
(void) rdscc(lp->cardbase, cmd, R8);
(void) rdscc(lp->cardbase, cmd, R8);
(void) rdscc(lp->cardbase, cmd, R8);
lp->rcp = lp->rcvbuf->data;
lp->rcvbuf->cnt = 0; /* rewind on DCD transition */
}
restore_flags(flags);
}
static void b_txint(struct pi_local *lp)
{
unsigned long flags;
int cmd;
unsigned char c;
save_flags(flags);
cli();
cmd = CTL + lp->base;
switch (lp->tstate) {
case CRCOUT:
lp->tstate = FLAGOUT;
tdelay(lp, lp->squeldelay);
restore_flags(flags);
return;
case IDLE:
/* Transmitter idle. Find a frame for transmission */
if ((lp->sndbuf = skb_dequeue(&lp->sndq)) == NULL) {
/* Nothing to send - return to receive mode
* Tx OFF now - flag should have gone
*/
rts(lp, OFF);
restore_flags(flags);
return;
}
lp->txptr = lp->sndbuf->data;
lp->txptr++; /* Ignore KISS control byte */
lp->txcnt = (int) lp->sndbuf->len - 1;
/* If a buffer to send, we drop thru here */
case DEFER: /* we may have deferred prev xmit attempt */
/* Check DCD - debounce it */
/* See Intel Microcommunications Handbook, p2-308 */
wrtscc(lp->cardbase, cmd, R0, RES_EXT_INT);
wrtscc(lp->cardbase, cmd, R0, RES_EXT_INT);
if ((rdscc(lp->cardbase, cmd, R0) & DCD) != 0) {
lp->tstate = DEFER;
tdelay(lp, 100);
/* defer until DCD transition or timeout */
wrtscc(lp->cardbase, cmd, R15, CTSIE | DCDIE);
restore_flags(flags);
return;
}
if (random() > lp->persist) {
lp->tstate = DEFER;
tdelay(lp, lp->slotime);
restore_flags(flags);
return;
}
rts(lp, ON); /* Transmitter on */
lp->tstate = ST_TXDELAY;
tdelay(lp, lp->txdelay);
restore_flags(flags);
return;
case ACTIVE:
/* Here we are actively sending a frame */
if (lp->txcnt--) {
c = *lp->txptr++;
/* next char is gone */
wrtscc(lp->cardbase, cmd, R8, c);
/* stuffing a char satisfies Interrupt condition */
} else {
/* No more to send */
free_p(lp->sndbuf);
lp->sndbuf = NULL;
if ((rdscc(lp->cardbase, cmd, R0) & 0x40)) {
/* Did we underrun? */
/* unexpected underrun */
lp->stats.tx_errors++;
lp->stats.tx_fifo_errors++;
wrtscc(lp->cardbase, cmd, R0, SEND_ABORT);
lp->tstate = FLAGOUT;
tdelay(lp, lp->squeldelay);
restore_flags(flags);
return;
}
lp->tstate = UNDERRUN; /* Now we expect to underrun */
/* Send flags on underrun */
if (lp->speed) { /* If internally clocked */
wrtscc(lp->cardbase, cmd, R10, CRCPS | NRZI);
} else {
wrtscc(lp->cardbase, cmd, R10, CRCPS);
}
wrtscc(lp->cardbase, cmd, R0, RES_Tx_P); /* reset Tx Int Pend */
}
restore_flags(flags);
return; /* back to wait for interrupt */
} /* end switch */
restore_flags(flags);
}
static void a_exint(struct pi_local *lp)
{
unsigned long flags;
int cmd;
char st;
int length;
save_flags(flags);
cli(); /* disable interrupts */
st = rdscc(lp->cardbase, lp->base + CTL, R0); /* Fetch status */
/* reset external status latch */
wrtscc(lp->cardbase, CTL + lp->base, R0, RES_EXT_INT);
cmd = lp->base + CTL;
if ((lp->rstate >= ACTIVE) && (st & BRK_ABRT)) {
setup_rx_dma(lp);
lp->rstate = ACTIVE;
}
switch (lp->tstate) {
case ACTIVE:
free_p(lp->sndbuf);
lp->sndbuf = NULL;
lp->tstate = FLAGOUT;
tdelay(lp, lp->squeldelay);
break;
case FLAGOUT:
if ((lp->sndbuf = skb_dequeue(&lp->sndq)) == NULL) {
/* Nothing to send - return to receive mode */
lp->tstate = IDLE;
rts(lp, OFF);
restore_flags(flags);
return;
}
/* NOTE - fall through if more to send */
case ST_TXDELAY:
/* Disable DMA chan */
disable_dma(lp->dmachan);
/* Set up for TX dma */
wrtscc(lp->cardbase, cmd, R1, WT_FN_RDYFN | EXT_INT_ENAB);
/* Get all chars */
/* Strip KISS control byte */
length = lp->sndbuf->len - 1;
memcpy(lp->txdmabuf, &lp->sndbuf->data[1], length);
/* Setup DMA controller for tx */
setup_tx_dma(lp, length);
/* select transmit interrupts to enable */
/* Allow DMA on chan */
enable_dma(lp->dmachan);
/* reset CRC, Txint pend*/
wrtscc(lp->cardbase, cmd, R0, RES_Tx_CRC | RES_Tx_P);
/* allow Underrun int only */
wrtscc(lp->cardbase, cmd, R15, TxUIE);
/* Enable TX DMA */
wrtscc(lp->cardbase, cmd, R1, WT_RDY_ENAB | WT_FN_RDYFN | EXT_INT_ENAB);
/* Send CRC on underrun */
wrtscc(lp->cardbase, cmd, R0, RES_EOM_L);
/* packet going out now */
lp->tstate = ACTIVE;
break;
case DEFER:
/* we have deferred prev xmit attempt
* See Intel Microcommunications Handbook, p2-308
*/
wrtscc(lp->cardbase, cmd, R0, RES_EXT_INT);
wrtscc(lp->cardbase, cmd, R0, RES_EXT_INT);
if ((rdscc(lp->cardbase, cmd, R0) & DCD) != 0) {
lp->tstate = DEFER;
tdelay(lp, 100);
/* Defer until dcd transition or 100mS timeout */
wrtscc(lp->cardbase, CTL + lp->base, R15, CTSIE | DCDIE);
restore_flags(flags);
return;
}
if (random() > lp->persist) {
lp->tstate = DEFER;
tdelay(lp, lp->slotime);
restore_flags(flags);
return;
}
/* Assert RTS early minimize collision window */
wrtscc(lp->cardbase, cmd, R5, TxCRC_ENAB | RTS | Tx8);
rts(lp, ON); /* Transmitter on */
lp->tstate = ST_TXDELAY;
tdelay(lp, lp->txdelay);
restore_flags(flags);
return;
} /* switch(lp->tstate) */
restore_flags(flags);
} /* a_exint() */