// Read status register
uint8_t Sensirion::readSR(uint8_t *result) {
uint8_t val;
uint8_t error = 0;
#ifdef CRC_ENA
_crc = bitrev(_stat_reg & SR_MASK); // Initialize CRC calculation
#endif
startTransmission();
if (error = putByte(STAT_REG_R)) {
*result = 0xFF;
return error;
}
#ifdef CRC_ENA
calcCRC(STAT_REG_R, &_crc); // Include command byte in CRC calculation
*result = getByte(ACK);
calcCRC(*result, &_crc);
val = getByte(noACK);
val = bitrev(val);
if (val != _crc) {
*result = 0xFF;
error = S_Err_CRC;
}
#else
*result = getByte(noACK);
#endif
return error;
}
// Initiate measurement. If blocking, wait for result
uint8_t Sensirion::meas(uint8_t cmd, uint16_t *result, bool block) {
uint8_t error, i;
#ifdef CRC_ENA
_crc = bitrev(_stat_reg & SR_MASK); // Initialize CRC calculation
#endif
startTransmission();
if (cmd == TEMP)
cmd = MEAS_TEMP;
else
cmd = MEAS_HUMI;
if (error = putByte(cmd))
return error;
#ifdef CRC_ENA
calcCRC(cmd, &_crc); // Include command byte in CRC calculation
#endif
// If non-blocking, save pointer to result and return
if (!block) {
_presult = result;
return 0;
}
// Otherwise, wait for measurement to complete with 720ms timeout
i = 240;
while (digitalRead(_pinData)) {
i--;
if (i == 0)
return S_Err_TO; // Error: Timeout
delay(3);
}
error = getResult(result);
return error;
}
// Start a measurement (humidity/temperature) with checksum
// p_value returns 2 bytes
// Modes: 1 = humidity 0 = temperature
// Return value: 1 = write error, 2 = crc error, 3 = timeout
uint8_t Sensirion::readSensorRaw(unsigned int *p_value, uint8_t mode)
{
uint8_t i = 0;
startTransmission(); //transmission start
*p_value = 0;
if (mode){
mode = MODE_HUMI;
} else {
mode = MODE_TEMP;
}
if (putByte(mode)) {
return(1);
}
// normal delays: temp i=70, humi i=20
while (i < 240){
delay(3);
if (digitalRead(_pinData) == 0){
i = 0;
break;
}
i++;
}
if (i) {
// or timeout
return(3);
}
i = readByte(1); //read the first byte (MSB)
*p_value = (i << 8) | readByte(0); //read the second byte (LSB) and end with no-ack
return(0);
}
//------------------------------------------------------------------------------
tFirmwareRet firmwareupdate_processUpdateList(tFirmwareUpdateList* ppList_p)
{
tFirmwareRet ret = kFwReturnOk;
tFirmwareUpdateEntry** ppInsertIter;
tFirmwareUpdateTransmissionInfo* pInfo;
ret = checkPointerAndInstance(ppList_p);
if (ret != kFwReturnOk)
{
goto EXIT;
}
pInfo = &instance_l.aTransmissions[(*ppList_p)->nodeId];
ppInsertIter = &pInfo->pUpdateList;
while (*ppInsertIter != NULL)
{
ppInsertIter = &(*ppInsertIter)->pNext;
}
*ppInsertIter = (*ppList_p);
*ppList_p = NULL;
if (!pInfo->fTranmissionActive)
{
pInfo->sdoComCon = FIRMWARE_UPDATE_INVALID_SDO;
}
ret = startTransmission(pInfo);
EXIT:
return ret;
}
// Write status register
uint8_t Sensirion::writeSR(uint8_t value) {
uint8_t error;
value &= SR_MASK; // Mask off unwritable bits
_stat_reg = value; // Save local copy
startTransmission();
if (error = putByte(STAT_REG_W))
return error;
return putByte(value);
}
uint16_t sendCommand(uint8_t ins, uint8_t p1, uint8_t p2, uint8_t len, uint8_t *data)
{
startTransmission();
SPI.transfer(ins);
SPI.transfer(p1);
SPI.transfer(p2);
if (len) {
SPI.transfer(len);
SPI.transfer(data, len);
}
endTransmission();
busyWait();
startTransmission();
uint16_t rc = (SPI.transfer(0x00) << 8) | SPI.transfer(0x00);
endTransmission();
busyWait();
return rc;
}
// Communication link reset
// At least 9 SCK cycles with DATA=1, followed by transmission start sequence
// ______________________________________________________ ________
// DATA: |_______|
// _ _ _ _ _ _ _ _ _ ___ ___
// SCK : __| |__| |__| |__| |__| |__| |__| |__| |__| |______| |___| |______
void Sensirion::resetConnection(void) {
uint8_t i;
digitalWrite(_pinData, HIGH); // Set data register high before turning on
pinMode(_pinData, OUTPUT); // output driver (avoid possible low pulse)
PULSE_LONG;
for (i = 0; i < 9; i++) {
digitalWrite(_pinClock, HIGH);
PULSE_LONG;
digitalWrite(_pinClock, LOW);
PULSE_LONG;
}
startTransmission();
}
void TConnectionProcessor::TOutboxQueue::connectionCheckingLoop()
{
do
{
if(isMailConnected())
{
startTransmission();
return;
}
fc::usleep(fc::milliseconds(250));
}
while(CancelPromise->ready() == false);
}
void ir_icSrv(void)
{
icData.icInhibit = _TRUE;
tim_disableInterrupts(irTimers.icTimerId);
tim_clearFlag(irTimers.ocTimerId);
tim_setValue(irTimers.ocTimerId, tim_getValue(irTimers.icTimerId) + EDGE_TIME_MARGIN); //Margen por rise time lento
if (icData.transmitting == _FALSE)
startTransmission();
else
{
u32 timeElapsed = (icData.overflowCnt * CNT_MAX + tim_getValue(irTimers.icTimerId)) - icData.lastEdge;
icData.lastEdge = tim_getValue(irTimers.icTimerId);
icData.overflowCnt = 0;
if ((timeElapsed >= HBT2_MIN) && (timeElapsed < HBT2_MAX))
{
if (PREVIOUS_BIT() == 1)
store_1();
else
store_0();
}
else if ((timeElapsed >= HBT3_MIN) && (timeElapsed < HBT3_MAX))
{
if (PREVIOUS_BIT() == 0)
store_1();
else
{
store_1();
store_0();
}
}
else if ((timeElapsed >= HBT4_MIN) && (timeElapsed < HBT4_MAX) && (PREVIOUS_BIT() == 0))
{
store_1();
store_0();
}
else
resetTransmission();
}
if (icData.currentBit == (-1))
endTransmission();
return;
}
// Communication reset
// DATA-line=1 and at least 9 SCK cycles followed by transstart
// _____________________________________________________ ________
// DATA: |_______|
// _ _ _ _ _ _ _ _ _ ___ ___
// SCK : __| |__| |__| |__| |__| |__| |__| |__| |__| |______| |___| |______
void Sensirion::resetConnection(void)
{
uint8_t i;
digitalWrite(_pinClock, LOW);
pinMode(_pinData, OUTPUT);
digitalWrite(_pinData, HIGH);
for (i = 0; i < 9; i++) {
digitalWrite(_pinClock, HIGH);
PULSE_LONG;
digitalWrite(_pinClock, LOW);
PULSE_LONG;
}
startTransmission();
}
//------------------------------------------------------------------------------
static tFirmwareRet transmissionSucceeded(tFirmwareUpdateTransmissionInfo* pInfo_p)
{
tFirmwareRet ret = kFwReturnOk;
tFirmwareUpdateTransmissionInfo* pNextInfo;
FWM_TRACE("Update finished for node: %u index: 0x%x subindex: 0x%x\n",
pInfo_p->pUpdateList->nodeId, pInfo_p->pUpdateList->index,
pInfo_p->pUpdateList->subindex);
if (pInfo_p->pUpdateList->fIsNode)
{
// Node callback
if (instance_l.config.pfnNodeUpdateComplete != NULL)
{
ret = instance_l.config.pfnNodeUpdateComplete(pInfo_p->pUpdateList->nodeId,
&pInfo_p->sdoComCon);
}
}
else if (pInfo_p->pUpdateList->pNext == NULL)
{
// Module callback
if (instance_l.config.pfnModuleUpdateComplete != NULL)
{
ret = instance_l.config.pfnModuleUpdateComplete(pInfo_p->pUpdateList->nodeId,
&pInfo_p->sdoComCon);
}
}
instance_l.numberOfFinishedTransmissions++;
if (pInfo_p->pUpdateList->pNext == NULL)
{
pNextInfo = getNextPendingTransmission();
if (pNextInfo != NULL)
{
FWM_TRACE("Start next pending transmission for node 0x%X\n",
pNextInfo->pUpdateList->nodeId);
startTransmission(pNextInfo);
}
}
cleanupTransmission(pInfo_p);
return ret;
}
stereoCam::stereoCam() {
// Find cameras on the 1394 buses
d = dc1394_new ();
// Enumerate cameras connected to the PC
err = dc1394_camera_enumerate (d, &list);
if ( err != DC1394_SUCCESS )
{
fprintf( stderr, "Unable to look for cameras\n\n"
"Please check \n"
" - if the kernel modules `ieee1394',`raw1394' and `ohci1394' "
"are loaded \n"
" - if you have read/write access to /dev/raw1394\n\n");
nThisCam=-1;
//return 1;
}
if (list->num == 0)
{
fprintf( stderr, "No cameras found!\n");
nThisCam=-1;
//return 1;
}
printf( "There were %d camera(s) found attached to your PC\n", list->num );
// Identify cameras. Use the first stereo camera that is found
for ( nThisCam = 0; nThisCam < list->num; nThisCam++ )
{
camera = dc1394_camera_new(d, list->ids[nThisCam].guid);
if(!camera)
{
printf("Failed to initialize camera with guid %llx",
list->ids[nThisCam].guid);
continue;
}
printf( "Camera %d model = '%s'\n", nThisCam, camera->model );
if ( isStereoCamera(camera))
{
printf( "Using this camera\n" );
break;
}
dc1394_camera_free(camera);
}
if ( nThisCam == list->num )
{
printf( "No stereo cameras were detected\n" );
nThisCam=-1;
//return 0;
}
// Free memory used by the camera list
dc1394_camera_free_list (list);
// query information about this stereo camera
err = queryStereoCamera( camera, &stereoCamera );
if ( err != DC1394_SUCCESS )
{
fprintf( stderr, "Cannot query all information from camera\n" );
cleanup_and_exit( camera );
}
if ( stereoCamera.nBytesPerPixel != 2 )
{
// can't handle XB3 3 bytes per pixel
fprintf( stderr,
"Example not updated to work with XB3 in 3 camera mode yet!\n" );
cleanup_and_exit( stereoCamera.camera );
}
// set the capture mode
printf( "Setting stereo video capture mode\n" );
err = setStereoVideoCapture( &stereoCamera );
if ( err != DC1394_SUCCESS )
{
fprintf( stderr, "Could not set up video capture mode\n" );
cleanup_and_exit( stereoCamera.camera );
}
// have the camera start sending us data
printf( "Start transmission\n" );
err = startTransmission( &stereoCamera );
if ( err != DC1394_SUCCESS )
{
fprintf( stderr, "Unable to start camera iso transmission\n" );
cleanup_and_exit( stereoCamera.camera );
}
// give the auto-gain algorithms a chance to catch up
// printf( "Giving auto-gain algorithm a chance to stabilize\n" );
// sleep( 5 );
// Allocate all the buffers.
// Unfortunately color processing is a bit inefficient
// because of the number of
// data copies. Color data needs to be
// - de-interleaved into separate bayer tile images
// - color processed into RGB images
// - de-interleaved to extract the green channel
// for stereo (or other mono conversion)
// size of buffer for all images at mono8
nBufferSize = stereoCamera.nRows *
stereoCamera.nCols *
stereoCamera.nBytesPerPixel;
// allocate a buffer to hold the de-interleaved images
pucDeInterlacedBuffer = new unsigned char[ nBufferSize ];
pucRGBBuffer = new unsigned char[ 3 * nBufferSize ];
pucGreenBuffer = new unsigned char[ nBufferSize ];
// do stereo processing
printf( "Getting TriclopsContext from camera (slowly)... \n" );
e = getTriclopsContextFromCamera( &stereoCamera, &triclops );
TRIERR(e);
printf( "...got it\n" );
// make sure we are in subpixel mode
e = triclopsSetSubpixelInterpolation( triclops, 1 );
TRIERR(e);
e = triclopsSetDisparityMapping(triclops,0,60);
TRIERR(e);
e = triclopsSetDisparityMappingOn(triclops,1);
TRIERR(e);
e = triclopsSetUniquenessValidationMapping(triclops,0);
TRIERR(e);
e = triclopsSetTextureValidationMapping(triclops,0);
TRIERR(e);
isInRoutine=false;
}
int main( int argc, char *argv[] )
{
dc1394camera_t* camera;
dc1394error_t err;
dc1394_t * d;
dc1394camera_list_t * list;
unsigned int nThisCam;
// Find cameras on the 1394 buses
d = dc1394_new ();
// Enumerate cameras connected to the PC
err = dc1394_camera_enumerate (d, &list);
if ( err != DC1394_SUCCESS )
{
fprintf( stderr, "Unable to look for cameras\n\n"
"Please check \n"
" - if the kernel modules `ieee1394',`raw1394' and `ohci1394' "
"are loaded \n"
" - if you have read/write access to /dev/raw1394\n\n");
return 1;
}
if (list->num == 0)
{
fprintf( stderr, "No cameras found!\n");
return 1;
}
printf( "There were %d camera(s) found attached to your PC\n", list->num );
// Identify cameras. Use the first stereo camera that is found
for ( nThisCam = 0; nThisCam < list->num; nThisCam++ )
{
camera = dc1394_camera_new(d, list->ids[nThisCam].guid);
if(!camera)
{
printf("Failed to initialize camera with guid %llx", list->ids[nThisCam].guid);
continue;
}
printf( "Camera %d model = '%s'\n", nThisCam, camera->model );
if ( isStereoCamera(camera))
{
printf( "Using this camera\n" );
break;
}
dc1394_camera_free(camera);
}
if ( nThisCam == list->num )
{
printf( "No stereo cameras were detected\n" );
return 0;
}
// Free memory used by the camera list
dc1394_camera_free_list (list);
PGRStereoCamera_t stereoCamera;
// query information about this stereo camera
err = queryStereoCamera( camera, &stereoCamera );
if ( err != DC1394_SUCCESS )
{
fprintf( stderr, "Cannot query all information from camera\n" );
cleanup_and_exit( camera );
}
// set the capture mode
printf( "Setting stereo video capture mode\n" );
err = setStereoVideoCapture( &stereoCamera );
if ( err != DC1394_SUCCESS )
{
fprintf( stderr, "Could not set up video capture mode\n" );
cleanup_and_exit( stereoCamera.camera );
}
// have the camera start sending us data
printf( "Start transmission\n" );
err = startTransmission( &stereoCamera );
if ( err != DC1394_SUCCESS )
{
fprintf( stderr, "Unable to start camera iso transmission\n" );
cleanup_and_exit( stereoCamera.camera );
}
// give the auto-gain algorithms a chance to catch up
printf( "Wait for the auto-gain algorithm to stabilize\n" );
sleep( 5 );
// Allocate all the buffers.
// Unfortunately color processing is a bit inefficient because of the number of
// data copies. Color data needs to be
// - de-interleaved into separate bayer tile images
// - color processed into RGB images
// - de-interleaved to extract the green channel for stereo (or other mono conversion)
// size of capture buffer
unsigned int nBufferSize = stereoCamera.nRows *
stereoCamera.nCols *
stereoCamera.nBytesPerPixel;
// allocate a buffer to hold the de-interleaved images
unsigned char* pucDeInterlacedBuffer = new unsigned char[ nBufferSize ];
if ( stereoCamera.bColor )
{
unsigned char* pucRGBBuffer = new unsigned char[ 3 * nBufferSize ];
unsigned char* pucGreenBuffer = new unsigned char[ nBufferSize ];
unsigned char* pucRightRGB = NULL;
unsigned char* pucLeftRGB = NULL;
unsigned char* pucCenterRGB = NULL;
TriclopsInput input;
// get the images from the capture buffer and do all required processing
// note: produces a TriclopsInput that can be used for stereo processing
extractImagesColor( &stereoCamera,
DC1394_BAYER_METHOD_NEAREST,
pucDeInterlacedBuffer,
pucRGBBuffer,
pucGreenBuffer,
&pucRightRGB,
&pucLeftRGB,
&pucCenterRGB,
&input );
// write the color images to file
if ( !writePpm( "right.ppm", pucRightRGB, stereoCamera.nCols, stereoCamera.nRows ) )
printf( "wrote right.ppm\n" );
if ( !writePpm( "left.ppm", pucLeftRGB, stereoCamera.nCols, stereoCamera.nRows ) )
printf( "wrote left.ppm\n" );
if ( pucCenterRGB != pucLeftRGB )
if ( !writePpm( "center.ppm", pucCenterRGB, stereoCamera.nCols, stereoCamera.nRows ) )
printf( "wrote center.ppm\n" );
delete[] pucRGBBuffer;
delete[] pucGreenBuffer;
}
else
{
unsigned char* pucRightMono = NULL;
unsigned char* pucLeftMono = NULL;
unsigned char* pucCenterMono = NULL;
TriclopsInput input;
// get the images from the capture buffer and do all required processing
// note: produces a TriclopsInput that can be used for stereo processing
extractImagesMono( &stereoCamera,
pucDeInterlacedBuffer,
&pucRightMono,
&pucLeftMono,
&pucCenterMono,
&input );
// write the greyscale images to file
if ( !writePgm( "right.pgm", pucRightMono, stereoCamera.nCols, stereoCamera.nRows ) )
printf( "wrote right.pgm\n" );
if ( !writePgm( "left.pgm", pucLeftMono, stereoCamera.nCols, stereoCamera.nRows ) )
printf( "wrote left.pgm\n" );
if ( pucCenterMono != pucLeftMono )
if ( !writePgm( "center.pgm", pucCenterMono, stereoCamera.nCols, stereoCamera.nRows ) )
printf( "wrote center.pgm\n" );
}
printf( "Stop transmission\n" );
// Stop data transmission
if ( dc1394_video_set_transmission( stereoCamera.camera, DC1394_OFF ) != DC1394_SUCCESS )
{
fprintf( stderr, "Couldn't stop the camera?\n" );
}
delete[] pucDeInterlacedBuffer;
// close camera
cleanup_and_exit( camera );
return 0;
}
//------------------------------------------------------------------------------
tFirmwareRet firmwareupdate_processSdoEvent(const tSdoComFinished* pSdoComFinished_p)
{
tFirmwareRet ret = kFwReturnOk;
tFirmwareUpdateTransmissionInfo* pInfo = NULL;
BOOL fSucceeded = TRUE;
ret = checkPointerAndInstance(pSdoComFinished_p);
if (ret != kFwReturnOk)
{
goto EXIT;
}
pInfo = &instance_l.aTransmissions[pSdoComFinished_p->nodeId];
if (!isExpectedSdoCompleteEvent(pInfo, pSdoComFinished_p))
{
ret = kFwReturnInvalidSdoEvent;
goto EXIT;
}
if (pSdoComFinished_p->sdoComConState != kSdoComTransferFinished)
{
FWM_ERROR("SDO write failed for node 0x%X with state: 0x%x and abort code 0x%x\n",
pSdoComFinished_p->nodeId, pSdoComFinished_p->sdoComConState, pSdoComFinished_p->abortCode);
fSucceeded = FALSE;
}
if (pSdoComFinished_p->transferredBytes != pInfo->firmwareSize)
{
FWM_ERROR("SDO written number of bytes does not match for node 0x%X: %lu - %lu\n",
pSdoComFinished_p->nodeId, (ULONG)pSdoComFinished_p->transferredBytes, (ULONG)pInfo->firmwareSize);
fSucceeded = FALSE;
}
if (pSdoComFinished_p->abortCode != 0)
{
FWM_ERROR("SDO event with abort code: 0x%x for node 0x%X\n", pSdoComFinished_p->abortCode, pSdoComFinished_p->nodeId);
fSucceeded = FALSE;
}
if (fSucceeded)
{
ret = transmissionSucceeded(pInfo);
}
else
{
transmissionFailed(pInfo);
}
ret = startTransmission(pInfo);
EXIT:
if (ret != kFwReturnOk)
{
cleanupInstance();
}
return ret;
}
