C++ sensors函数代码示例

void imuUpdate(rollAndPitchTrims_t *accelerometerTrims)
{
    gyroUpdate();

    if (sensors(SENSOR_ACC)) {
        updateAccelerationReadings(accelerometerTrims); // TODO rename to accelerometerUpdate and rename many other 'Acceleration' references to be 'Accelerometer'
        imuCalculateEstimatedAttitude();
    } else {
        accADC[X] = 0;
        accADC[Y] = 0;
        accADC[Z] = 0;
    }
}

void taskProcessGPS(void)
{
    // if GPS feature is enabled, gpsThread() will be called at some intervals to check for stuck
    // hardware, wrong baud rates, init GPS if needed, etc. Don't use SENSOR_GPS here as gpsThread() can and will
    // change this based on available hardware
    if (feature(FEATURE_GPS)) {
        gpsThread();
    }

    if (sensors(SENSOR_GPS)) {
        updateGpsIndicator(currentTime);
    }
}

void handleFrSkyTelemetry(void)
{
    if (!canSendFrSkyTelemetry()) {
        return;
    }

    uint32_t now = millis();

    if (!hasEnoughTimeLapsedSinceLastTelemetryTransmission(now)) {
        return;
    }

    lastCycleTime = now;

    cycleNum++;

    // Sent every 125ms
    sendAccel();
    sendVario();
    sendTelemetryTail();

    if ((cycleNum % 4) == 0) {      // Sent every 500ms
        sendBaro();
        sendHeading();
        sendTelemetryTail();
    }

    if ((cycleNum % 8) == 0) {      // Sent every 1s
        sendTemperature1();

        if (feature(FEATURE_VBAT)) {
            sendVoltage();
            sendVoltageAmp();
            sendAmperage();
            sendFuelLevel();
        }

#ifdef GPS
        if (sensors(SENSOR_GPS))
            sendGPS();
#endif

        sendTelemetryTail();
    }

    if (cycleNum == 40) {     //Frame 3: Sent every 5s
        cycleNum = 0;
        sendTime();
        sendTelemetryTail();
    }
}

static void taskUpdateRxMain(timeUs_t currentTimeUs)
{
    processRx(currentTimeUs);
    isRXDataNew = true;

#if !defined(BARO) && !defined(SONAR)
    // updateRcCommands sets rcCommand, which is needed by updateAltHoldState and updateSonarAltHoldState
    updateRcCommands();
#endif
    updateArmingStatus();

#ifdef BARO
    if (sensors(SENSOR_BARO)) {
        updateAltHoldState();
    }
#endif

#ifdef SONAR
    if (sensors(SENSOR_SONAR)) {
        updateSonarAltHoldState();
    }
#endif
}

void imuInit(void)                                                            // Initialize & precalculate some values here
{
    if (cfg.gy_smrll || cfg.gy_smptc || cfg.gy_smyw)
    {
        SmoothingFactor[ROLL]  = cfg.gy_smrll;
        SmoothingFactor[PITCH] = cfg.gy_smptc;
        SmoothingFactor[YAW]   = cfg.gy_smyw;
        GyroSmoothing          = true;
    } else GyroSmoothing = false;

#ifdef MAG
    if (sensors(SENSOR_MAG)) Mag_init();
#endif
}

void taskUpdateRxMain(void)
{
    processRx();
    isRXDataNew = true;

#if !defined(BARO) && !defined(SONAR)
    // updateRcCommands sets rcCommand, which is needed by updateAltHoldState and updateSonarAltHoldState
    updateRcCommands();
#endif
    updateLEDs();

#ifdef BARO
    if (sensors(SENSOR_BARO)) {
        updateAltHoldState();
    }
#endif

#ifdef SONAR
    if (sensors(SENSOR_SONAR)) {
        updateSonarAltHoldState();
    }
#endif
}

void taskUpdateRxMain(void)
{
    processRx();
    isRXDataNew = true;

#ifdef BARO
    // the 'annexCode' initialses rcCommand, updateAltHoldState depends on valid rcCommand data.
    if (haveProcessedAnnexCodeOnce) {
        if (sensors(SENSOR_BARO)) {
            updateAltHoldState();
        }
    }
#endif

#ifdef SONAR
    // the 'annexCode' initialses rcCommand, updateAltHoldState depends on valid rcCommand data.
    if (haveProcessedAnnexCodeOnce) {
        if (sensors(SENSOR_SONAR)) {
            updateSonarAltHoldState();
        }
    }
#endif
}

double DetectorModule::stripOccupancyPerEventBarrel() const {
  double rho = center().Rho()/10.;
  double theta = center().Theta();
  double myOccupancyBarrel=(1.63e-4)+(2.56e-4)*rho-(1.92e-6)*rho*rho;
  double factor = fabs(sin(theta))*2; // 2 is a magic adjustment factor
  double dphideta = phiAperture() * etaAperture();
  double minNSegments = minSegments();
  int numStripsAcrossEstimate = sensors().begin()->numStripsAcrossEstimate();
  double modWidth = (maxWidth() + minWidth())/2.;

  double occupancy = myOccupancyBarrel / factor / (90/1e3) * (dphideta / minNSegments) * (modWidth / numStripsAcrossEstimate);

  return occupancy;
}

bool ProcessController::saveSettings(QDomDocument& doc, QDomElement& element)
{
    if(!mProcessList)
        return false;
    element.setAttribute("hostName", sensors().at(0)->hostName());
    element.setAttribute("sensorName", sensors().at(0)->name());
    element.setAttribute("sensorType", sensors().at(0)->type());

    element.setAttribute("version", QString::number(PROCESSHEADERVERSION));
    element.setAttribute("treeViewHeader", QString::fromLatin1(mProcessList->treeView()->header()->saveState().toBase64()));
    element.setAttribute("showTotals", mProcessList->showTotals()?1:0);

    element.setAttribute("units", (int)(mProcessList->units()));
    element.setAttribute("ioUnits", (int)(mProcessList->processModel()->ioUnits()));
    element.setAttribute("ioInformation", (int)(mProcessList->processModel()->ioInformation()));
    element.setAttribute("showCommandLineOptions", mProcessList->processModel()->isShowCommandLineOptions());
    element.setAttribute("showTooltips", mProcessList->processModel()->isShowingTooltips());
    element.setAttribute("normalizeCPUUsage", mProcessList->processModel()->isNormalizedCPUUsage());
    element.setAttribute("filterState", (int)(mProcessList->state()));

    SensorDisplay::saveSettings(doc, element);

    return true;
}

bool
LogFile::saveSettings(QDomDocument& doc, QDomElement& element)
{
	element.setAttribute("hostName", sensors().at(0)->hostName());
	element.setAttribute("sensorName", sensors().at(0)->name());
	element.setAttribute("sensorType", sensors().at(0)->type());

	element.setAttribute("font", monitor->font().toString());

	saveColor(element, "textColor", monitor->palette().color( QPalette::Text ) );
	saveColor(element, "backgroundColor", monitor->palette().color( QPalette::Base ) );

	for (QStringList::Iterator it = filterRules.begin();
		 it != filterRules.end(); ++it)
	{
		QDomElement filter = doc.createElement("filter");
		filter.setAttribute("rule", (*it));
		element.appendChild(filter);
	}

	SensorDisplay::saveSettings(doc, element);

	return true;
}

/* This is equivalent to the act of lifting the robot and moving it
 * to a new location without the encoders registering the movement.
 */
void hyperJump(double new_x, double new_y, double new_o)
{
    robot_x = new_x;
    robot_y = new_y;
    robot_o = new_o;

    /* this function needs a value */
    /* greater than 0 to update the */
    /* position, so this is how long */
    /* a teleportation will take, 1 msec */
    updatePosition(1);

    sensors();
    refreshSimDisplay(TRUE);
    RedrawWholeMap();
}

		static Node::Info tempInfo(std::string uid){

			BB::Node::Sensors sensors({ BB::Node::Sensor("Temperature", BB::Node::Info::Temperature, "C") });

			BB::Node::Settings settings({
				BB::Node::Setting("name"),
				BB::Node::Setting("place"),
				BB::Node::Setting("low"),
				BB::Node::Setting("high"),
				BB::Node::Setting("extremelyLow"),
				BB::Node::Setting("extremelyHigh"),
			});

			Node::Info info(uid, Node::Info::Temperature, sensors, settings);
			return info;
		}

/**
 * Read BARO and update alt/vel topic
 *  Function is called at main loop rate, updates happen at reduced rate
 */
static void updateBaroTopic(uint32_t currentTime)
{
    static navigationTimer_t baroUpdateTimer;

    if (updateTimer(&baroUpdateTimer, HZ2US(INAV_BARO_UPDATE_RATE), currentTime)) {
        float newBaroAlt = baroCalculateAltitude();
        if (sensors(SENSOR_BARO) && isBaroCalibrationComplete()) {
            posEstimator.baro.alt = newBaroAlt;
            posEstimator.baro.epv = posControl.navConfig->inav.baro_epv;
            posEstimator.baro.lastUpdateTime = currentTime;
        }
        else {
            posEstimator.baro.alt = 0;
            posEstimator.baro.lastUpdateTime = 0;
        }
    }
}

void computeIMU(void)
{
    static   float LastGyroSmooth[3] = { 0.0f, 0.0f, 0.0f };
    static   int16_t triywavg[4];
    static   uint8_t triywavgpIDX = 0;
    static   uint32_t prevT;
    uint32_t curT;
    uint8_t  axis, i;
    float    flttmp;
    if (MpuSpecial) GETMPU6050();
    else
    {
        gyro.temperature(&telemTemperature1);                                 // Read out gyro temperature
        Gyro_getADC();                                                        // Also feeds gyroData
        if (sensors(SENSOR_ACC)) ACC_getADC();
    }

    curT               = micros();
    ACCDeltaTimeINS    = (float)(curT - prevT) * 0.000001f;                   // ACCDeltaTimeINS is in seconds now
    ACCDeltaTimeINS    = constrain(ACCDeltaTimeINS, 0.0001f, 0.5f);           // Constrain to range 0,1ms - 500ms
    prevT              = curT;

    if(cfg.acc_calibrated) getEstimatedAttitude();                            // acc_calibrated just can turn true if acc present.
    
    if(cfg.mixerConfiguration == MULTITYPE_TRI && cfg.gy_smyw)                // Moving average for yaw in tri mode
    {
        triywavg[triywavgpIDX] = (int16_t)gyroData[YAW]; triywavgpIDX++;
        if (triywavgpIDX == 4) triywavgpIDX = 0;
        flttmp = 0;
        for (i = 0; i < 4; i++) flttmp += triywavg[i];
        gyroData[YAW] = flttmp * 0.25f;
    }

    if (GyroSmoothing)
    {
        for (axis = 0; axis < 3; axis++)
        {
            if (SmoothingFactor[axis] > 1)                                    // Circumvent useless action
            {
                flttmp               = (float)SmoothingFactor[axis];
                gyroData[axis]       = ((LastGyroSmooth[axis] * (flttmp - 1.0f)) + gyroData[axis]) / flttmp;
                LastGyroSmooth[axis] = gyroData[axis];
            }
        }
    }
}

void sendFRSKYTelemetry(void)
{
    static uint32_t lastCycleTime = 0;
    static uint8_t  cycleNum = 0;

    if (currentTimeMS - lastCycleTime >= CYCLETIME)
    {
        lastCycleTime = currentTimeMS;
        cycleNum++;

        // Sent every 125ms
        sendAccel();
        sendTelemetryTail();

        if ((cycleNum % 4) == 0)        // Sent every 500ms
        {
            sendBaro();
            sendHeading();
            sendTelemetryTail();
        }

        if ((cycleNum % 8) == 0)        // Sent every 1s
        {
            sendTemperature1();

            if (feature(FEATURE_VBAT))
            {
                sendVoltage();
                sendVoltageAmp();
            }

            if (sensors(SENSOR_GPS))
                sendGPS();

            sendTelemetryTail();
        }

        if (cycleNum == 40)       //Frame 3: Sent every 5s
        {
            cycleNum = 0;
            sendTime();
            sendTelemetryTail();
        }
    }
}