Esempi in C++ (Cpp) per loggerDoHeader

Esempio n. 1

File: logger.c Progetto: benzeng/AutoQuad

void loggerInit(void) {
    memset((void *)&loggerData, 0, sizeof(loggerData));

    loggerSetup();

    // skip the first 512 bytes (used exclusively by the USB MSC driver)
    loggerData.loggerBuf = (TCHAR *)(filerBuf + 512);
    loggerData.bufSize = ((FILER_BUF_SIZE-512) / loggerData.packetSize / FILER_FLUSH_THRESHOLD) * loggerData.packetSize * FILER_FLUSH_THRESHOLD;

    loggerData.logHandle = filerGetHandle(LOGGER_FNAME);
    filerStream(loggerData.logHandle, loggerData.loggerBuf, loggerData.bufSize);

    loggerDoHeader();
}

Esempio n. 2

File: run.c Progetto: mpaperno/aq_flight_control

void runTaskCode(void *unused) {
    uint32_t axis = 0;
    uint32_t loops = 0;

    AQ_NOTICE("Run task started\n");

    while (1) {
	// wait for data
	CoWaitForSingleFlag(imuData.sensorFlag, 0);

	// soft start GPS accuracy
	runData.accMask *= 0.999f;

	navUkfInertialUpdate();

	// record history for acc & mag & pressure readings for smoothing purposes
	// acc
	runData.sumAcc[0] -= runData.accHist[0][runData.sensorHistIndex];
	runData.sumAcc[1] -= runData.accHist[1][runData.sensorHistIndex];
	runData.sumAcc[2] -= runData.accHist[2][runData.sensorHistIndex];

	runData.accHist[0][runData.sensorHistIndex] = IMU_ACCX;
	runData.accHist[1][runData.sensorHistIndex] = IMU_ACCY;
	runData.accHist[2][runData.sensorHistIndex] = IMU_ACCZ;

	runData.sumAcc[0] += runData.accHist[0][runData.sensorHistIndex];
	runData.sumAcc[1] += runData.accHist[1][runData.sensorHistIndex];
	runData.sumAcc[2] += runData.accHist[2][runData.sensorHistIndex];

	// mag
	runData.sumMag[0] -= runData.magHist[0][runData.sensorHistIndex];
	runData.sumMag[1] -= runData.magHist[1][runData.sensorHistIndex];
	runData.sumMag[2] -= runData.magHist[2][runData.sensorHistIndex];

	runData.magHist[0][runData.sensorHistIndex] = IMU_MAGX;
	runData.magHist[1][runData.sensorHistIndex] = IMU_MAGY;
	runData.magHist[2][runData.sensorHistIndex] = IMU_MAGZ;

	runData.sumMag[0] += runData.magHist[0][runData.sensorHistIndex];
	runData.sumMag[1] += runData.magHist[1][runData.sensorHistIndex];
	runData.sumMag[2] += runData.magHist[2][runData.sensorHistIndex];

	// pressure
	runData.sumPres -= runData.presHist[runData.sensorHistIndex];
	runData.presHist[runData.sensorHistIndex] = AQ_PRESSURE;
	runData.sumPres += runData.presHist[runData.sensorHistIndex];

	runData.sensorHistIndex = (runData.sensorHistIndex + 1) % RUN_SENSOR_HIST;

	if (!((loops+1) % 20)) {
	   simDoAccUpdate(runData.sumAcc[0]*(1.0f / (float)RUN_SENSOR_HIST), runData.sumAcc[1]*(1.0f / (float)RUN_SENSOR_HIST), runData.sumAcc[2]*(1.0f / (float)RUN_SENSOR_HIST));
	}
	else if (!((loops+7) % 20)) {
	   simDoPresUpdate(runData.sumPres*(1.0f / (float)RUN_SENSOR_HIST));
	}
#ifndef USE_DIGITAL_IMU
	else if (!((loops+13) % 20) && AQ_MAG_ENABLED) {
	   simDoMagUpdate(runData.sumMag[0]*(1.0f / (float)RUN_SENSOR_HIST), runData.sumMag[1]*(1.0f / (float)RUN_SENSOR_HIST), runData.sumMag[2]*(1.0f / (float)RUN_SENSOR_HIST));
	}
#endif
	// optical flow update
	else if (navUkfData.flowCount >= 10 && !navUkfData.flowLock) {
	    navUkfFlowUpdate();
	}
	// only accept GPS updates if there is no optical flow
	else if (CoAcceptSingleFlag(gpsData.gpsPosFlag) == E_OK && navUkfData.flowQuality == 0.0f && gpsData.hAcc < NAV_MIN_GPS_ACC && gpsData.tDOP != 0.0f) {
	    navUkfGpsPosUpdate(gpsData.lastPosUpdate, gpsData.lat, gpsData.lon, gpsData.height, gpsData.hAcc + runData.accMask, gpsData.vAcc + runData.accMask);
	    CoClearFlag(gpsData.gpsPosFlag);
	    // refine static sea level pressure based on better GPS altitude fixes
	    if (gpsData.hAcc < runData.bestHacc && gpsData.hAcc < NAV_MIN_GPS_ACC) {
                navPressureAdjust(gpsData.height);
		runData.bestHacc = gpsData.hAcc;
	    }
	}
	else if (CoAcceptSingleFlag(gpsData.gpsVelFlag) == E_OK && navUkfData.flowQuality == 0.0f && gpsData.sAcc < NAV_MIN_GPS_ACC/2 && gpsData.tDOP != 0.0f) {
	    navUkfGpsVelUpdate(gpsData.lastVelUpdate, gpsData.velN, gpsData.velE, gpsData.velD, gpsData.sAcc + runData.accMask);
	    CoClearFlag(gpsData.gpsVelFlag);
	}
	// observe zero position
	else if (!((loops+4) % 20) && (gpsData.hAcc >= NAV_MIN_GPS_ACC || gpsData.tDOP == 0.0f) && navUkfData.flowQuality == 0.0f) {
	    navUkfZeroPos();
	}
	// observer zero velocity
	else if (!((loops+10) % 20) && (gpsData.sAcc >= NAV_MIN_GPS_ACC/2 || gpsData.tDOP == 0.0f) && navUkfData.flowQuality == 0.0f) {
	    navUkfZeroVel();
	}
	// observe that the rates are exactly 0 if not flying or moving
	else if (!(supervisorData.state & STATE_FLYING)) {
	    float stdX, stdY, stdZ;

	    arm_std_f32(runData.accHist[0], RUN_SENSOR_HIST, &stdX);
	    arm_std_f32(runData.accHist[1], RUN_SENSOR_HIST, &stdY);
	    arm_std_f32(runData.accHist[2], RUN_SENSOR_HIST, &stdZ);

	    if ((stdX + stdY + stdZ) < (IMU_STATIC_STD*2)) {
		if (!((axis + 0) % 3))
		    navUkfZeroRate(IMU_RATEX, 0);
		else if (!((axis + 1) % 3))
		    navUkfZeroRate(IMU_RATEY, 1);
		else
		    navUkfZeroRate(IMU_RATEZ, 2);
		axis++;
	    }
	}

        navUkfFinish();
        altUkfProcess(AQ_PRESSURE);

        // determine which altitude estimate to use
        if (gpsData.hAcc > p[NAV_ALT_GPS_ACC]) {
            runData.altPos = &ALT_POS;
            runData.altVel = &ALT_VEL;
        }
        else {
            runData.altPos = &UKF_ALTITUDE;
            runData.altVel = &UKF_VELD;
        }

	CoSetFlag(runData.runFlag);	// new state data

	navNavigate();
#ifndef HAS_AIMU
	analogDecode();
#endif
	if (!(loops % (int)(1.0f / AQ_OUTER_TIMESTEP)))
	    loggerDoHeader();
	loggerDo();
	gimbalUpdate();

#ifdef CAN_CALIB
	canTxIMUData(loops);
#endif
        calibrate();

	loops++;
    }
}