static FAST_CODE FAST_CODE_NOINLINE void gyroUpdateSensor(gyroSensor_t *gyroSensor, timeUs_t currentTimeUs) { if (!gyroSensor->gyroDev.readFn(&gyroSensor->gyroDev)) { return; } gyroSensor->gyroDev.dataReady = false; if (isGyroSensorCalibrationComplete(gyroSensor)) { // move 16-bit gyro data into 32-bit variables to avoid overflows in calculations #if defined(USE_GYRO_SLEW_LIMITER) gyroSensor->gyroDev.gyroADC[X] = gyroSlewLimiter(gyroSensor, X) - gyroSensor->gyroDev.gyroZero[X]; gyroSensor->gyroDev.gyroADC[Y] = gyroSlewLimiter(gyroSensor, Y) - gyroSensor->gyroDev.gyroZero[Y]; gyroSensor->gyroDev.gyroADC[Z] = gyroSlewLimiter(gyroSensor, Z) - gyroSensor->gyroDev.gyroZero[Z]; #else gyroSensor->gyroDev.gyroADC[X] = gyroSensor->gyroDev.gyroADCRaw[X] - gyroSensor->gyroDev.gyroZero[X]; gyroSensor->gyroDev.gyroADC[Y] = gyroSensor->gyroDev.gyroADCRaw[Y] - gyroSensor->gyroDev.gyroZero[Y]; gyroSensor->gyroDev.gyroADC[Z] = gyroSensor->gyroDev.gyroADCRaw[Z] - gyroSensor->gyroDev.gyroZero[Z]; #endif alignSensors(gyroSensor->gyroDev.gyroADC, gyroSensor->gyroDev.gyroAlign); } else { performGyroCalibration(gyroSensor, gyroConfig()->gyroMovementCalibrationThreshold); // still calibrating, so no need to further process gyro data return; } if (gyroDebugMode == DEBUG_NONE) { filterGyro(gyroSensor); } else { filterGyroDebug(gyroSensor); } #ifdef USE_GYRO_OVERFLOW_CHECK if (gyroConfig()->checkOverflow && !gyroHasOverflowProtection) { checkForOverflow(gyroSensor, currentTimeUs); } #endif #ifdef USE_YAW_SPIN_RECOVERY if (gyroConfig()->yaw_spin_recovery) { checkForYawSpin(gyroSensor, currentTimeUs); } #endif #ifdef USE_GYRO_DATA_ANALYSE if (isDynamicFilterActive()) { gyroDataAnalyse(&gyroSensor->gyroAnalyseState, gyroSensor->notchFilterDyn, gyroSensor->notchFilterDyn2); } #endif #if (!defined(USE_GYRO_OVERFLOW_CHECK) && !defined(USE_YAW_SPIN_RECOVERY)) UNUSED(currentTimeUs); #endif }
static void insertElementAtIndex(LSQ_HandleT handle, LSQ_BaseTypeT element, LSQ_IntegerIndexT index) { ArrayPointerT h = (ArrayPointerT)handle; LSQ_BaseTypeT * placeOfElement; if(h == NULL) return; checkForOverflow(handle); placeOfElement = h->elements + index; memmove(placeOfElement + 1, placeOfElement, sizeof(LSQ_BaseTypeT) * (h->count - index)); * placeOfElement = element; h->count++; }
bool LogEncoderDecoder::decodeDirectoryInvalidationEntry(char *buf, int16_t size, int64_t ×tamp) { folly::ByteRange br((uint8_t *)buf, size); try { timestamp = decodeInt(br); } catch (const std::exception &ex) { LOG(ERROR) << "got exception " << folly::exceptionStr(ex); return false; } return !checkForOverflow(br.start() - (uint8_t *)buf, size); }
bool LogEncoderDecoder::decodeFileResizeEntry(char *buf, int16_t size, int64_t ×tamp, int64_t &seqId, int64_t &fileSize) { folly::ByteRange br((uint8_t *)buf, size); try { timestamp = decodeInt(br); seqId = decodeInt(br); fileSize = decodeInt(br); } catch (const std::exception &ex) { LOG(ERROR) << "got exception " << folly::exceptionStr(ex); return false; } return !checkForOverflow(br.start() - (uint8_t *)buf, size); }
bool LogEncoderDecoder::decodeLogHeader(char *buf, int16_t size, int64_t ×tamp, int &version, std::string &recoveryId, std::string &senderIp, int64_t &config) { folly::ByteRange br((uint8_t *)buf, size); try { timestamp = decodeInt(br); version = decodeInt(br); if (!decodeString(br, buf, size, recoveryId)) { return false; } if (!decodeString(br, buf, size, senderIp)) { return false; } config = decodeInt(br); } catch (const std::exception &ex) { LOG(ERROR) << "got exception " << folly::exceptionStr(ex); return false; } return !checkForOverflow(br.start() - (uint8_t *)buf, size); }
std::vector<ImportDescriptor*> FitsImporter::getImportDescriptors(const std::string& fname) { std::string filename = fname; std::vector<std::vector<std::string> >& errors = mErrors[fname]; std::vector<std::vector<std::string> >& warnings = mWarnings[fname]; errors.clear(); warnings.clear(); int status=0; std::vector<ImportDescriptor*> descriptors; FitsFileResource pFile(filename); if (!pFile.isValid()) { errors.resize(1); errors[0].push_back(pFile.getStatus()); RETURN_DESCRIPTORS; } int hduCnt = 0; int specificHdu = 0; int hdu = 1; if (!splitFilename(filename, hduCnt, specificHdu, hdu, pFile, errors, warnings)) { RETURN_DESCRIPTORS; } errors.resize(hduCnt+1); warnings.resize(hduCnt+1); for(; hdu <= hduCnt; ++hdu) { std::string datasetName = filename + "[" + StringUtilities::toDisplayString(hdu) + "]"; int hduType; CHECK_FITS(fits_movabs_hdu(pFile, hdu, &hduType, &status), hdu, false, continue); ImportDescriptorResource pImportDescriptor(static_cast<ImportDescriptor*>(NULL)); FactoryResource<DynamicObject> pMetadata; VERIFYRV(pMetadata.get() != NULL, descriptors); { // scope std::vector<std::string> comments; char pCard[81]; char pValue[81]; char pComment[81]; int nkeys = 0; CHECK_FITS(fits_get_hdrspace(pFile, &nkeys, NULL, &status), hdu, true, ;); for(int keyidx = 1; keyidx <= nkeys; ++keyidx) { CHECK_FITS(fits_read_keyn(pFile, keyidx, pCard, pValue, pComment, &status), hdu, true, continue); std::string name = StringUtilities::stripWhitespace(std::string(pCard)); std::string val = StringUtilities::stripWhitespace(std::string(pValue)); std::string comment = StringUtilities::stripWhitespace(std::string(pComment)); if (!val.empty()) { pMetadata->setAttributeByPath("FITS/" + name, val); } else if (!comment.empty()) { comments.push_back(comment); } } if (!comments.empty()) { // ideally, this would add a multi-line string but Opticks doesn't display this properly // pMetadata->setAttributeByPath("FITS/COMMENT", StringUtilities::join(comments, "\n")); for(unsigned int idx = 0; idx < comments.size(); ++idx) { pMetadata->setAttributeByPath("FITS/COMMENT/" + StringUtilities::toDisplayString(idx), comments[idx]); } } } switch(hduType) { case IMAGE_HDU: { pImportDescriptor = ImportDescriptorResource(datasetName, TypeConverter::toString<RasterElement>()); VERIFYRV(pImportDescriptor.get() != NULL, descriptors); EncodingType fileEncoding; InterleaveFormatType interleave(BSQ); unsigned int rows=0; unsigned int cols=0; unsigned int bands=1; int bitpix; int naxis; long axes[3]; CHECK_FITS(fits_get_img_param(pFile, 3, &bitpix, &naxis, axes, &status), hdu, false, continue); switch(bitpix) { case BYTE_IMG: fileEncoding = INT1UBYTE; break; case SHORT_IMG: fileEncoding = INT2SBYTES; break; case LONG_IMG: fileEncoding = INT4SBYTES; break; case FLOAT_IMG: fileEncoding = FLT4BYTES; break; case DOUBLE_IMG: fileEncoding = FLT8BYTES; break; default: warnings[hdu].push_back("Unsupported BITPIX value " + StringUtilities::toDisplayString(bitpix) + "."); continue; } EncodingType dataEncoding = checkForOverflow(fileEncoding, pMetadata.get(), hdu, errors, warnings); if (naxis == 1) { // 1-D data is a signature pImportDescriptor = ImportDescriptorResource(datasetName, TypeConverter::toString<Signature>()); pMetadata->setAttributeByPath(METADATA_SIG_ENCODING, dataEncoding); pMetadata->setAttributeByPath(METADATA_SIG_LENGTH, axes[0]); RasterUtilities::generateAndSetFileDescriptor(pImportDescriptor->getDataDescriptor(), filename, StringUtilities::toDisplayString(hdu), BIG_ENDIAN_ORDER); // add units SignatureDataDescriptor* pSigDd = dynamic_cast<SignatureDataDescriptor*>(pImportDescriptor->getDataDescriptor()); if (pSigDd != NULL) { FactoryResource<Units> pUnits; pUnits->setUnitName("Custom"); pUnits->setUnitType(CUSTOM_UNIT); pSigDd->setUnits("Reflectance", pUnits.get()); } break; // leave switch() } else if (naxis == 2) { cols = axes[0]; rows = axes[1]; } else if (naxis == 3) { cols = axes[0]; rows = axes[1]; bands = axes[2]; } else { errors[hdu].push_back(StringUtilities::toDisplayString(naxis) + " axis data not supported."); } RasterDataDescriptor* pDataDesc = RasterUtilities::generateRasterDataDescriptor( datasetName, NULL, rows, cols, bands, interleave, dataEncoding, IN_MEMORY); pImportDescriptor->setDataDescriptor(pDataDesc); if (specificHdu == 0 && hdu == 1 && naxis == 2 && (axes[0] <= 5 || axes[1] <= 5)) { // use 5 as this is a good top end for the number of astronomical band pass filters // in general usage. this is not in a spec anywhere and is derived from various sample // FITS files for different astronomical instruments. // // There's a good chance this is really a spectrum. (0th HDU) // We'll create an import descriptor for the spectrum version of this // And disable the raster descriptor by default pImportDescriptor->setImported(false); ImportDescriptorResource pSigDesc(datasetName, TypeConverter::toString<SignatureLibrary>()); DynamicObject* pSigMetadata = pSigDesc->getDataDescriptor()->getMetadata(); pSigMetadata->merge(pMetadata.get()); std::vector<double> centerWavelengths; unsigned int cnt = (axes[0] <= 5) ? axes[1] : axes[0]; double startVal = StringUtilities::fromDisplayString<double>( dv_cast<std::string>(pMetadata->getAttributeByPath("FITS/MINWAVE"), "0.0")); double endVal = StringUtilities::fromDisplayString<double>( dv_cast<std::string>(pMetadata->getAttributeByPath("FITS/MAXWAVE"), "0.0")); double incr = (endVal == 0.0) ? 1.0 : ((endVal - startVal) / static_cast<double>(cnt)); centerWavelengths.reserve(cnt); for (unsigned int idx = 0; idx < cnt; idx++) { centerWavelengths.push_back(startVal + (idx * incr)); } pSigMetadata->setAttributeByPath(CENTER_WAVELENGTHS_METADATA_PATH, centerWavelengths); // Units std::string unitsName = dv_cast<std::string>(pMetadata->getAttributeByPath("FITS/BUNIT"), std::string()); if (!unitsName.empty()) { FactoryResource<Units> units; units->setUnitName(unitsName); units->setUnitType(RADIANCE); SignatureDataDescriptor* pSigDd = dynamic_cast<SignatureDataDescriptor*>(pSigDesc->getDataDescriptor()); if (pSigDd != NULL) { pSigDd->setUnits("Reflectance", units.get()); } } RasterUtilities::generateAndSetFileDescriptor(pSigDesc->getDataDescriptor(), filename, StringUtilities::toDisplayString(hdu), BIG_ENDIAN_ORDER); // If units are not available, set custom units into the data descriptor so that the user can // modify them - this must occur after the call to RasterUtilities::generateAndSetFileDescriptor() // so that the file descriptor will still display no defined units if (unitsName.empty()) { FactoryResource<Units> units; units->setUnitName("Custom"); units->setUnitType(CUSTOM_UNIT); SignatureDataDescriptor* pSigDd = dynamic_cast<SignatureDataDescriptor*>(pSigDesc->getDataDescriptor()); if (pSigDd != NULL) { pSigDd->setUnits("Reflectance", units.get()); } } descriptors.push_back(pSigDesc.release()); } RasterFileDescriptor* pFileDescriptor = dynamic_cast<RasterFileDescriptor*>( RasterUtilities::generateAndSetFileDescriptor(pDataDesc, filename, StringUtilities::toDisplayString(hdu), BIG_ENDIAN_ORDER)); if (pFileDescriptor != NULL) { unsigned int bitsPerElement = RasterUtilities::bytesInEncoding(fileEncoding) * 8; pFileDescriptor->setBitsPerElement(bitsPerElement); } break; // leave switch() } case ASCII_TBL: case BINARY_TBL: warnings[hdu].push_back("Tables not supported. [HDU " + StringUtilities::toDisplayString(hdu) + "]"); continue; default: warnings[hdu].push_back("HDU " + StringUtilities::toDisplayString(hdu) + " is an unknown type."); continue; } pImportDescriptor->getDataDescriptor()->setMetadata(pMetadata.release()); pImportDescriptor->setImported(errors[hdu].empty()); descriptors.push_back(pImportDescriptor.release()); }
void executeExp(Executer &executer) { auto result = std::exp(executer.topDbl()); checkForOverflow(executer, result); executer.setTop(result); }