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);
}
