コード例 #1
0
ファイル: imu.c プロジェクト: cs8425/cleanflight
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;
    }
}
コード例 #2
0
ファイル: mw.c プロジェクト: AquaSoftware/betaflight
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);
    }
}
コード例 #3
0
ファイル: frsky.c プロジェクト: airmamaf/cleanflight
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();
    }
}
コード例 #4
0
ファイル: fc_tasks.c プロジェクト: basdelfos/betaflight
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
}
コード例 #5
0
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
}
コード例 #6
0
ファイル: mw.c プロジェクト: bluejayrc/betaflight
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
}
コード例 #7
0
ファイル: mw.c プロジェクト: JoachimF/cleanflight
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
}
コード例 #8
0
ファイル: DetectorModule.cpp プロジェクト: kacichy/tkLayout
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;
}
コード例 #9
0
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;
}
コード例 #10
0
ファイル: LogFile.cpp プロジェクト: aarontc/kde-workspace
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;
}
コード例 #11
0
ファイル: DetectorModule.cpp プロジェクト: kacichy/tkLayout
double DetectorModule::stripOccupancyPerEventEndcap() const {
  double rho = center().Rho()/10.;
  double theta = center().Theta();
  double z = center().Z()/10.;
  double myOccupancyEndcap = (-6.20e-5)+(1.75e-4)*rho-(1.08e-6)*rho*rho+(1.50e-5)*(z);
  double factor=fabs(cos(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 = myOccupancyEndcap / factor / (90/1e3) * (dphideta / minNSegments) * (modWidth / numStripsAcrossEstimate);

  return occupancy;
}
コード例 #12
0
ファイル: action.c プロジェクト: makamuy/frob
/* 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();
}
コード例 #13
0
ファイル: TemperatureNode.cpp プロジェクト: hadzim/bb
		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;
		}
コード例 #14
0
/**
 * 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;
        }
    }
}
コード例 #15
0
ファイル: imu.c プロジェクト: slashphotos/TestCode3
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];
            }
        }
    }
}
コード例 #16
0
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();
        }
    }
}
コード例 #17
0
ファイル: garule.cpp プロジェクト: edlau/GABot
void GARule::randomRule() {
   int rInt;                           // random integer in case we need it more than once
   int j;                              // variable for scratch use
   
   j = teamBall(Random::randint(-1,1));
   if (j == 1) myBall(Random::randbool());     // have at least some logic in the random rules;
   else myBall(false);                 // if my team doesn't have the ball, then I can't have it

   Thing sens[8];
   for (int i = 0; i < 8; i++){
      rInt = Random::randint(0,5);
      switch(rInt){

         case(0):
	      sens[i] = Nothing;
	      break;
         case(1):
	      sens[i] = MyBot;
	      break;
         case(2):
	      sens[i] = OtherBot;
	      break;
         case(3):
	      sens[i] = TheBall;
	      break;
         case(4):
	      sens[i] = Net;
	      break;
         case(5):
	      sens[i] = Wall;
	      break;

      }//end switch
   }//end for
   sensors(sens);
   fire(Random::randbool());
   move(Random::randbool());
   rInt = Random::randint(0,2);

   if (rInt == 0)      turn(BotRotation::Left);
   else if (rInt == 1) turn(BotRotation::Right);
   else                turn(BotRotation::None);
   // generates rules with more than one action, but should work anyway since
   // only one action will occur
}
コード例 #18
0
ファイル: telemetry.c プロジェクト: X-charles/Xfly
void sendTelemetry(void)
{
    if (mcfg.telemetry_softserial == TELEMETRY_UART && !f.ARMED)
        return;

    if (serialTotalBytesWaiting(core.telemport) != 0)
        return;

    if (millis() - lastCycleTime >= CYCLETIME) {
        lastCycleTime = millis();
        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();
        }
    }
}
コード例 #19
0
void RobotMove::GoToPose(Pose newPose){
	xDestination = newPose.getX();
	yDestination = newPose.getY();
	//Vector worldCoordinateVector(xDestination + (*position).GetXPos(), yDestination + (*position).GetYPos());
	//Vector localCoordinateVector(worldCoordinateVector.getDistance(), dtor(worldCoordinateVector.getAngle()) + (*position).GetYaw(), false);
	//xDestination = localCoordinateVector.getXIntensity();//x + (*position).GetXPos();
	//yDestination = localCoordinateVector.getYIntensity();//y + (*position).GetYPos();

	SensorScan sensors(&(*laser), &(*position), &(*worldView), sensorFunction, 1000.0);
	double distance = closenessCutOff;
	while (distance >= closenessCutOff) {				//cout << "Setup";
		(*robot).Read();			
		sensors.ReadSensors();
		Vector sensorVector = sensors.VectorSum();
		sensorVector.invertVector();
		//sensorVector.debugIntensity();
		Vector avoidVector(sensorVector.getDistance(), dtor(sensorVector.getAngle()) + (*position).GetYaw() , false);
		
		Vector driveVector((*position).GetXPos(), (*position).GetYPos(), xDestination, yDestination);
		distance = driveVector.getDistance();
		TargetFunction(driveVector);
		
		//TargetFunction(driveVector);
		//cout << endl << "AvoidVector       : ";
		//avoidVector.debug();avoidVector.debugIntensity();
		//cout << endl << "Drive Vector      : ";
		//cout << closenessCutOff << endl;
		//driveVector.debug();
		//driveVector.debugIntensity();
		//cout << endl;
		driveVector.add(avoidVector);
		//cout << endl << "Resulting Vector  : " << endl;
		//driveVector.debug();driveVector.debugIntensity();
		DriveToVector(driveVector, &(*position));
	}
	Vector directionVector(.1, newPose.getAngle(), true);
	//while (abs((*position).GetYaw() - dtor(newPose.getAngle())) > closenessCutOff) {
	//	DriveToVector(directionVector, &(*position));
	//}
	(*position).SetSpeed(0,0);
	

}
コード例 #20
0
ファイル: hott.c プロジェクト: raul-ortega/iNav
static bool processBinaryModeRequest(uint8_t address)
{
    switch (address) {
#ifdef USE_GPS
    case 0x8A:
        if (sensors(SENSOR_GPS)) {
            hottPrepareGPSResponse(&hottGPSMessage);
            hottQueueSendResponse((uint8_t *)&hottGPSMessage, sizeof(hottGPSMessage));
            return true;
        }
        break;
#endif
    case 0x8E:
        hottPrepareEAMResponse(&hottEAMMessage);
        hottQueueSendResponse((uint8_t *)&hottEAMMessage, sizeof(hottEAMMessage));
        return true;
    }

    return false;
}
コード例 #21
0
ファイル: cli.c プロジェクト: DuinoPilot/TMR
static void cliStatus(char *cmdline)
{
    uint8_t i;
    uint32_t mask;

    printf("System Uptime: %d seconds, Voltage: %d * 0.1V (%dS battery)\r\n",
        millis() / 1000, vbat, batteryCellCount);
    mask = sensorsMask();

    uartPrint("Detected sensors: ");
    for (i = 0; ; i++) {
        if (sensorNames[i] == NULL)
            break;
        if (mask & (1 << i))
            printf("%s ", sensorNames[i]);
    }
    if (sensors(SENSOR_ACC))
        printf("ACCHW: %s", accNames[accHardware]);
    uartPrint("\r\n");

    printf("Cycle Time: %d, I2C Errors: %d\r\n", cycleTime, i2cGetErrorCounter());
}
コード例 #22
0
ファイル: hott.c プロジェクト: 180jacob/cleanflight
static void processBinaryModeRequest(uint8_t address)
{

#ifdef HOTT_DEBUG
    static uint8_t hottBinaryRequests = 0;
    static uint8_t hottGPSRequests = 0;
    static uint8_t hottEAMRequests = 0;
#endif

    switch (address) {
#ifdef GPS
        case 0x8A:
#ifdef HOTT_DEBUG
            hottGPSRequests++;
#endif
            if (sensors(SENSOR_GPS)) {
                hottSendGPSResponse();
            }
            break;
#endif
        case 0x8E:
#ifdef HOTT_DEBUG
            hottEAMRequests++;
#endif
            hottSendEAMResponse();
            break;
    }


#ifdef HOTT_DEBUG
    hottBinaryRequests++;
    debug[0] = hottBinaryRequests;
#ifdef GPS
    debug[1] = hottGPSRequests;
#endif
    debug[2] = hottEAMRequests;
#endif

}
コード例 #23
0
ファイル: updates.c プロジェクト: guillep19/frob
void simUpdate (void)
{
  unsigned long msec, t;
  
  if( rotation_velocity != 0 || translation_velocity != 0 ) {
    stepEndTime = getTime();
    msec = stepEndTime - stepStartTime;
    t = 1000*msec/timeScale;
    if (t >= timeIncrement) {
      if (t > 3 * timeIncrement) {
        fprintf(stderr,"Missed Simulations steps for %ld msec\n",msec);
        t = 2 * timeIncrement;
      }
      updatePosition(t);
      sensors();
      stepStartTime = stepEndTime;
    }
  } else {
    UpdateElapsedTime();
    RedrawTime();
  }
}
コード例 #24
0
ファイル: imu.c プロジェクト: Feldsalat/inav
/* Calculate measured acceleration in body frame cm/s/s */
static void imuUpdateMeasuredAcceleration(void)
{
    int axis;

    /* Convert acceleration to cm/s/s */
    for (axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
        imuAccelInBodyFrame.A[axis] = accADC[axis] * (GRAVITY_CMSS / acc.acc_1G);
        imuMeasuredGravityBF.A[axis] = imuAccelInBodyFrame.A[axis];
    }

#ifdef GPS
    /** Centrifugal force compensation on a fixed-wing aircraft
      * a_c_body = omega x vel_tangential_body
      * assumption: tangential velocity only along body x-axis
      * assumption: GPS velocity equal to body x-axis velocity
      */
    if (STATE(FIXED_WING) && sensors(SENSOR_GPS) && STATE(GPS_FIX) && gpsSol.numSat >= 5) {
        imuMeasuredGravityBF.A[Y] -= gpsSol.groundSpeed * imuMeasuredRotationBF.A[Z];
        imuMeasuredGravityBF.A[Z] += gpsSol.groundSpeed * imuMeasuredRotationBF.A[Y];
    }
#endif
}
コード例 #25
0
ファイル: fc_core.c プロジェクト: 4712/cleanflight
static FAST_CODE_NOINLINE void subTaskPidSubprocesses(timeUs_t currentTimeUs)
{
    uint32_t startTime = 0;
    if (debugMode == DEBUG_PIDLOOP) {
        startTime = micros();
    }

#ifdef USE_MAG
    if (sensors(SENSOR_MAG)) {
        updateMagHold();
    }
#endif

#ifdef USE_BLACKBOX
    if (!cliMode && blackboxConfig()->device) {
        blackboxUpdate(currentTimeUs);
    }
#else
    UNUSED(currentTimeUs);
#endif

    DEBUG_SET(DEBUG_PIDLOOP, 3, micros() - startTime);
}
コード例 #26
0
ファイル: cli.c プロジェクト: DanCInOz/baseflight
static void cliStatus(char *cmdline)
{
    (void)cmdline;
    uint8_t i;
    uint32_t mask;

    printf("System Uptime: %d seconds, Voltage: %d * 0.1V (%dS battery)\r\n",
           millis() / 1000, vbat, batteryCellCount);
    mask = sensorsMask();

    printf("Hardware: %s @ %dMHz, detected sensors: ", hwNames[hw_revision], (SystemCoreClock / 1000000));
    for (i = 0; ; i++) {
        if (sensorNames[i] == NULL)
            break;
        if (mask & (1 << i))
            printf("%s ", sensorNames[i]);
    }
    if (sensors(SENSOR_ACC))
        printf("ACCHW: %s", accNames[accHardware]);
    cliPrint("\r\n");

    printf("Cycle Time: %d, I2C Errors: %d, config size: %d\r\n", cycleTime, i2cGetErrorCounter(), sizeof(master_t));
}
コード例 #27
0
ファイル: imu.c プロジェクト: LaughingLogic/baseflight
void computeIMU(void)
{
    static int16_t gyroYawSmooth = 0;

    Gyro_getADC();
    if (sensors(SENSOR_ACC)) {
        ACC_getADC();
        getEstimatedAttitude();
    } else {
        accADC[X] = 0;
        accADC[Y] = 0;
        accADC[Z] = 0;
    }

    if (mcfg.mixerConfiguration == MULTITYPE_TRI) {
        gyroData[YAW] = (gyroYawSmooth * 2 + gyroADC[YAW]) / 3;
        gyroYawSmooth = gyroData[YAW];
    } else {
        gyroData[YAW] = gyroADC[YAW];
    }
    gyroData[ROLL] = gyroADC[ROLL];
    gyroData[PITCH] = gyroADC[PITCH];
}
コード例 #28
0
ファイル: imu.c プロジェクト: stormin13/cleanflight
void computeIMU(rollAndPitchTrims_t *accelerometerTrims, uint8_t mixerConfiguration)
{
    static int16_t gyroYawSmooth = 0;

    gyroGetADC();
    if (sensors(SENSOR_ACC)) {
        updateAccelerationReadings(accelerometerTrims);
        getEstimatedAttitude();
    } else {
        accADC[X] = 0;
        accADC[Y] = 0;
        accADC[Z] = 0;
    }

    gyroData[FD_ROLL] = gyroADC[FD_ROLL];
    gyroData[FD_PITCH] = gyroADC[FD_PITCH];

    if (mixerConfiguration == MULTITYPE_TRI) {
        gyroData[FD_YAW] = (gyroYawSmooth * 2 + gyroADC[FD_YAW]) / 3;
        gyroYawSmooth = gyroData[FD_YAW];
    } else {
        gyroData[FD_YAW] = gyroADC[FD_YAW];
    }
}
コード例 #29
0
ファイル: smartport.c プロジェクト: Liquidas/betaflight
void handleSmartPortTelemetry(void)
{
    uint32_t smartPortLastServiceTime = millis();

    if (!smartPortTelemetryEnabled) {
        return;
    }

    if (!canSendSmartPortTelemetry()) {
        return;
    }

    while (serialRxBytesWaiting(smartPortSerialPort) > 0) {
        uint8_t c = serialRead(smartPortSerialPort);
        smartPortDataReceive(c);
    }

    uint32_t now = millis();

    // if timed out, reconfigure the UART back to normal so the GUI or CLI works
    if ((now - smartPortLastRequestTime) > SMARTPORT_NOT_CONNECTED_TIMEOUT_MS) {
        smartPortState = SPSTATE_TIMEDOUT;
        return;
    }

    while (smartPortHasRequest) {
        // Ensure we won't get stuck in the loop if there happens to be nothing available to send in a timely manner - dump the slot if we loop in there for too long.
        if ((millis() - smartPortLastServiceTime) > SMARTPORT_SERVICE_TIMEOUT_MS) {
            smartPortHasRequest = 0;
            return;
        }

        // we can send back any data we want, our table keeps track of the order and frequency of each data type we send
        uint16_t id = frSkyDataIdTable[smartPortIdCnt];
        if (id == 0) { // end of table reached, loop back
            smartPortIdCnt = 0;
            id = frSkyDataIdTable[smartPortIdCnt];
        }
        smartPortIdCnt++;

        int32_t tmpi;
        static uint8_t t1Cnt = 0;

        switch(id) {
#ifdef GPS
            case FSSP_DATAID_SPEED      :
                if (sensors(SENSOR_GPS) && STATE(GPS_FIX)) {
                    uint32_t tmpui = (GPS_speed * 36 + 36 / 2) / 100;
                    smartPortSendPackage(id, tmpui); // given in 0.1 m/s, provide in KM/H
                    smartPortHasRequest = 0;
                }
                break;
#endif
            case FSSP_DATAID_VFAS       :
                if (feature(FEATURE_VBAT)) {
                    uint16_t vfasVoltage;
                    if (telemetryConfig->frsky_vfas_cell_voltage) {
                        vfasVoltage = vbat / batteryCellCount;
                    } else {
                        vfasVoltage = vbat;
                    }
                    smartPortSendPackage(id, vfasVoltage * 10); // given in 0.1V, convert to volts
                    smartPortHasRequest = 0;
                }
                break;
            case FSSP_DATAID_CURRENT    :
                if (feature(FEATURE_CURRENT_METER)) {
                    smartPortSendPackage(id, amperage / 10); // given in 10mA steps, unknown requested unit
                    smartPortHasRequest = 0;
                }
                break;
            //case FSSP_DATAID_RPM        :
            case FSSP_DATAID_ALTITUDE   :
                if (sensors(SENSOR_BARO)) {
                    smartPortSendPackage(id, BaroAlt); // unknown given unit, requested 100 = 1 meter
                    smartPortHasRequest = 0;
                }
                break;
            case FSSP_DATAID_FUEL       :
                if (feature(FEATURE_CURRENT_METER)) {
                    smartPortSendPackage(id, mAhDrawn); // given in mAh, unknown requested unit
                    smartPortHasRequest = 0;
                }
                break;
            //case FSSP_DATAID_ADC1       :
            //case FSSP_DATAID_ADC2       :
#ifdef GPS
            case FSSP_DATAID_LATLONG    :
                if (sensors(SENSOR_GPS) && STATE(GPS_FIX)) {
                    uint32_t tmpui = 0;
                    // the same ID is sent twice, one for longitude, one for latitude
                    // the MSB of the sent uint32_t helps FrSky keep track
                    // the even/odd bit of our counter helps us keep track
                    if (smartPortIdCnt & 1) {
                        tmpui = abs(GPS_coord[LON]);  // now we have unsigned value and one bit to spare
                        tmpui = (tmpui + tmpui / 2) / 25 | 0x80000000;  // 6/100 = 1.5/25, division by power of 2 is fast
                        if (GPS_coord[LON] < 0) tmpui |= 0x40000000;
                    }
                    else {
                        tmpui = abs(GPS_coord[LAT]);  // now we have unsigned value and one bit to spare
                        tmpui = (tmpui + tmpui / 2) / 25;  // 6/100 = 1.5/25, division by power of 2 is fast
                        if (GPS_coord[LAT] < 0) tmpui |= 0x40000000;
                    }
                    smartPortSendPackage(id, tmpui);
                    smartPortHasRequest = 0;
                }
                break;
#endif
            //case FSSP_DATAID_CAP_USED   :
            case FSSP_DATAID_VARIO      :
                if (sensors(SENSOR_BARO)) {
                    smartPortSendPackage(id, vario); // unknown given unit but requested in 100 = 1m/s
                    smartPortHasRequest = 0;
                }
                break;
            case FSSP_DATAID_HEADING    :
                smartPortSendPackage(id, attitude.values.yaw * 10); // given in 10*deg, requested in 10000 = 100 deg
                smartPortHasRequest = 0;
                break;
            case FSSP_DATAID_ACCX       :
                smartPortSendPackage(id, accSmooth[X] / 44);
                // unknown input and unknown output unit
                // we can only show 00.00 format, another digit won't display right on Taranis
                // dividing by roughly 44 will give acceleration in G units
                smartPortHasRequest = 0;
                break;
            case FSSP_DATAID_ACCY       :
                smartPortSendPackage(id, accSmooth[Y] / 44);
                smartPortHasRequest = 0;
                break;
            case FSSP_DATAID_ACCZ       :
                smartPortSendPackage(id, accSmooth[Z] / 44);
                smartPortHasRequest = 0;
                break;
            case FSSP_DATAID_T1         :
                // we send all the flags as decimal digits for easy reading

                // the t1Cnt simply allows the telemetry view to show at least some changes
                t1Cnt++;
                if (t1Cnt >= 4) {
                    t1Cnt = 1;
                }
                tmpi = t1Cnt * 10000; // start off with at least one digit so the most significant 0 won't be cut off
                // the Taranis seems to be able to fit 5 digits on the screen
                // the Taranis seems to consider this number a signed 16 bit integer

                if (ARMING_FLAG(OK_TO_ARM))
                    tmpi += 1;
                if (ARMING_FLAG(PREVENT_ARMING))
                    tmpi += 2;
                if (ARMING_FLAG(ARMED))
                    tmpi += 4;

                if (FLIGHT_MODE(ANGLE_MODE))
                    tmpi += 10;
                if (FLIGHT_MODE(HORIZON_MODE))
                    tmpi += 20;
                if (FLIGHT_MODE(UNUSED_MODE))
                    tmpi += 40;
                if (FLIGHT_MODE(PASSTHRU_MODE))
                    tmpi += 40;

                if (FLIGHT_MODE(MAG_MODE))
                    tmpi += 100;
                if (FLIGHT_MODE(BARO_MODE))
                    tmpi += 200;
                if (FLIGHT_MODE(SONAR_MODE))
                    tmpi += 400;

                if (FLIGHT_MODE(GPS_HOLD_MODE))
                    tmpi += 1000;
                if (FLIGHT_MODE(GPS_HOME_MODE))
                    tmpi += 2000;
                if (FLIGHT_MODE(HEADFREE_MODE))
                    tmpi += 4000;

                smartPortSendPackage(id, (uint32_t)tmpi);
                smartPortHasRequest = 0;
                break;
            case FSSP_DATAID_T2         :
                if (sensors(SENSOR_GPS)) {
#ifdef GPS
                    // provide GPS lock status
                    smartPortSendPackage(id, (STATE(GPS_FIX) ? 1000 : 0) + (STATE(GPS_FIX_HOME) ? 2000 : 0) + GPS_numSat);
                    smartPortHasRequest = 0;
#endif
                }
                else if (feature(FEATURE_GPS)) {
                    smartPortSendPackage(id, 0);
                    smartPortHasRequest = 0;
                }
                break;
#ifdef GPS
            case FSSP_DATAID_GPS_ALT    :
                if (sensors(SENSOR_GPS) && STATE(GPS_FIX)) {
                    smartPortSendPackage(id, GPS_altitude * 100); // given in 0.1m , requested in 10 = 1m (should be in mm, probably a bug in opentx, tested on 2.0.1.7)
                    smartPortHasRequest = 0;
                }
                break;
#endif
            case FSSP_DATAID_A4         :
                if (feature(FEATURE_VBAT)) {
                    smartPortSendPackage(id, vbat * 10 / batteryCellCount ); // given in 0.1V, convert to volts
                    smartPortHasRequest = 0;
                }
                break;
            default:
                break;
                // if nothing is sent, smartPortHasRequest isn't cleared, we already incremented the counter, just loop back to the start
        }
    }
}
コード例 #30
0
ファイル: mw.c プロジェクト: ezikiel12/baseflight
void annexCode(void)
{
    static uint32_t calibratedAccTime;
    int32_t tmp, tmp2;
    int32_t axis, prop1, prop2;
    static uint8_t buzzerFreq;  // delay between buzzer ring

    // vbat shit
    static uint8_t vbatTimer = 0;
    static int32_t vbatRaw = 0;
    static int32_t amperageRaw = 0;
    static int64_t mAhdrawnRaw = 0;
    static int32_t vbatCycleTime = 0;

    // PITCH & ROLL only dynamic PID adjustemnt,  depending on throttle value
    if (rcData[THROTTLE] < cfg.tpa_breakpoint) {
        prop2 = 100;
    } else {
        if (rcData[THROTTLE] < 2000) {
            prop2 = 100 - (uint16_t)cfg.dynThrPID * (rcData[THROTTLE] - cfg.tpa_breakpoint) / (2000 - cfg.tpa_breakpoint);
        } else {
            prop2 = 100 - cfg.dynThrPID;
        }
    }

    for (axis = 0; axis < 3; axis++) {
        tmp = min(abs(rcData[axis] - mcfg.midrc), 500);
        if (axis != 2) {        // ROLL & PITCH
            if (cfg.deadband) {
                if (tmp > cfg.deadband) {
                    tmp -= cfg.deadband;
                } else {
                    tmp = 0;
                }
            }

            tmp2 = tmp / 100;
            rcCommand[axis] = lookupPitchRollRC[tmp2] + (tmp - tmp2 * 100) * (lookupPitchRollRC[tmp2 + 1] - lookupPitchRollRC[tmp2]) / 100;
            prop1 = 100 - (uint16_t)cfg.rollPitchRate * tmp / 500;
            prop1 = (uint16_t)prop1 * prop2 / 100;
        } else {                // YAW
            if (cfg.yawdeadband) {
                if (tmp > cfg.yawdeadband) {
                    tmp -= cfg.yawdeadband;
                } else {
                    tmp = 0;
                }
            }
            rcCommand[axis] = tmp * -mcfg.yaw_control_direction;
            prop1 = 100 - (uint16_t)cfg.yawRate * abs(tmp) / 500;
        }
        dynP8[axis] = (uint16_t)cfg.P8[axis] * prop1 / 100;
        dynI8[axis] = (uint16_t)cfg.I8[axis] * prop1 / 100;
        dynD8[axis] = (uint16_t)cfg.D8[axis] * prop1 / 100;
        if (rcData[axis] < mcfg.midrc)
            rcCommand[axis] = -rcCommand[axis];
    }

    tmp = constrain(rcData[THROTTLE], mcfg.mincheck, 2000);
    tmp = (uint32_t)(tmp - mcfg.mincheck) * 1000 / (2000 - mcfg.mincheck);       // [MINCHECK;2000] -> [0;1000]
    tmp2 = tmp / 100;
    rcCommand[THROTTLE] = lookupThrottleRC[tmp2] + (tmp - tmp2 * 100) * (lookupThrottleRC[tmp2 + 1] - lookupThrottleRC[tmp2]) / 100;    // [0;1000] -> expo -> [MINTHROTTLE;MAXTHROTTLE]

    if (f.HEADFREE_MODE) {
        float radDiff = (heading - headFreeModeHold) * M_PI / 180.0f;
        float cosDiff = cosf(radDiff);
        float sinDiff = sinf(radDiff);
        int16_t rcCommand_PITCH = rcCommand[PITCH] * cosDiff + rcCommand[ROLL] * sinDiff;
        rcCommand[ROLL] = rcCommand[ROLL] * cosDiff - rcCommand[PITCH] * sinDiff;
        rcCommand[PITCH] = rcCommand_PITCH;
    }

    if (feature(FEATURE_VBAT)) {
        vbatCycleTime += cycleTime;
        if (!(++vbatTimer % VBATFREQ)) {
            vbatRaw -= vbatRaw / 8;
            vbatRaw += adcGetChannel(ADC_BATTERY);
            vbat = batteryAdcToVoltage(vbatRaw / 8);
            
            if (mcfg.power_adc_channel > 0) {
                amperageRaw -= amperageRaw / 8;
                amperageRaw += adcGetChannel(ADC_EXTERNAL_CURRENT);
                amperage = currentSensorToCentiamps(amperageRaw / 8);
                mAhdrawnRaw += (amperage * vbatCycleTime) / 1000;
                mAhdrawn = mAhdrawnRaw / (3600 * 100);
                vbatCycleTime = 0;
            }
            
        }
        if ((vbat > batteryWarningVoltage) || (vbat < mcfg.vbatmincellvoltage)) { // VBAT ok, buzzer off
            buzzerFreq = 0;
        } else
            buzzerFreq = 4;     // low battery
    }

    buzzer(buzzerFreq);         // external buzzer routine that handles buzzer events globally now

    if ((calibratingA > 0 && sensors(SENSOR_ACC)) || (calibratingG > 0)) {      // Calibration phasis
        LED0_TOGGLE;
    } else {
        if (f.ACC_CALIBRATED)
            LED0_OFF;
        if (f.ARMED)
            LED0_ON;

#ifndef CJMCU
        checkTelemetryState();
#endif
    }

#ifdef LEDRING
    if (feature(FEATURE_LED_RING)) {
        static uint32_t LEDTime;
        if ((int32_t)(currentTime - LEDTime) >= 0) {
            LEDTime = currentTime + 50000;
            ledringState();
        }
    }
#endif

    if ((int32_t)(currentTime - calibratedAccTime) >= 0) {
        if (!f.SMALL_ANGLE) {
            f.ACC_CALIBRATED = 0; // the multi uses ACC and is not calibrated or is too much inclinated
            LED0_TOGGLE;
            calibratedAccTime = currentTime + 500000;
        } else {
            f.ACC_CALIBRATED = 1;
        }
    }

    serialCom();

#ifndef CJMCU
    if (!cliMode && feature(FEATURE_TELEMETRY)) {
        handleTelemetry();
    }
#endif

    if (sensors(SENSOR_GPS)) {
        static uint32_t GPSLEDTime;
        if ((int32_t)(currentTime - GPSLEDTime) >= 0 && (GPS_numSat >= 5)) {
            GPSLEDTime = currentTime + 150000;
            LED1_TOGGLE;
        }
    }

    // Read out gyro temperature. can use it for something somewhere. maybe get MCU temperature instead? lots of fun possibilities.
    if (gyro.temperature)
        gyro.temperature(&telemTemperature1);
    else {
        // TODO MCU temp
    }
}